* @param init - Function to call to initialise the actuator (may be NULL)
* @returns Number of actuators added so far
*/
-int Actuator_Add(const char * name, int user_id, SetFn set, InitFn init, CleanFn cleanup, SanityFn sanity)
+int Actuator_Add(const char * name, int user_id, SetFn set, InitFn init, CleanFn cleanup, SanityFn sanity, double initial_value)
{
if (++g_num_actuators > ACTUATORS_MAX)
{
pthread_mutex_init(&(a->mutex), NULL);
+ Actuator_SetValue(a, initial_value, false);
+
return g_num_actuators;
}
void Actuator_Init()
{
//Actuator_Add("ledtest",0, Ledtest_Set, NULL,NULL,NULL);
- Actuator_Add("filetest", 0, Filetest_Set, Filetest_Init, Filetest_Cleanup, Filetest_Sanity);
+ Actuator_Add("filetest", 0, Filetest_Set, Filetest_Init, Filetest_Cleanup, Filetest_Sanity, 0);
}
/**
if (!a->activated)
break;
- Actuator_SetValue(a, a->control.start);
+ Actuator_SetValue(a, a->control.start, true);
// Currently does discrete steps after specified time intervals
- while (a->control.steps > 0 && a->activated)
+ while (!a->control_changed && a->control.steps > 0 && a->activated)
{
usleep(1e6*(a->control.stepwait));
a->control.start += a->control.stepsize;
- Actuator_SetValue(a, a->control.start);
+ Actuator_SetValue(a, a->control.start, true);
a->control.steps--;
}
+ if (a->control_changed)
+ continue;
usleep(1e6*(a->control.stepwait));
//TODO:
* @param a - The Actuator
* @param value - The value to set
*/
-void Actuator_SetValue(Actuator * a, double value)
+void Actuator_SetValue(Actuator * a, double value, bool record)
{
if (a->sanity != NULL && !a->sanity(a->user_id, value))
{
//ARE YOU INSANE?
- Fatal("Insane value %lf for actuator %s", value, a->name);
+ Log(LOGERR,"Insane value %lf for actuator %s", value, a->name);
+ return;
}
if (!(a->set(a->user_id, value)))
{
// Set time stamp
struct timeval t;
gettimeofday(&t, NULL);
- // Record and save DataPoint
- DataPoint d = {TIMEVAL_DIFF(t, *Control_GetStartTime()), value};
- Data_Save(&(a->data_file), &d, 1);
+ DataPoint d = {TIMEVAL_DIFF(t, *Control_GetStartTime()), a->last_setting.value};
+ // Record value change
+ if (record)
+ {
+ d.time_stamp -= 1e-6;
+ Data_Save(&(a->data_file), &d, 1);
+ d.value = value;
+ d.time_stamp += 1e-6;
+ Data_Save(&(a->data_file), &d, 1);
+ }
+ a->last_setting = d;
}
/**
SanityFn sanity;
/** Cleanup function **/
CleanFn clean;
+ /** Last setting **/
+ DataPoint last_setting;
} Actuator;
extern void Actuator_SetMode(Actuator * a, ControlModes mode, void *arg);
extern void * Actuator_Loop(void * args); // Main loop for a thread that handles an Actuator
-extern void Actuator_SetValue(Actuator * a, double value); // Set an actuator by value
+extern void Actuator_SetValue(Actuator * a, double value, bool record); // Set an actuator by value
extern void Actuator_SetControl(Actuator * a, ActuatorControl * c); // Set the control for an Actuator
extern Actuator * Actuator_Identify(const char * str); // Identify a Sensor from a string Id
#define ADC5 5
#define ADC6 6
#define ADC7 7
+#define ADC_VOLTAGE_MAX 1800
+#define ADC_RAW_MAX (2 << ADC_BITS)
+#define ADC_TO_MVOLTS(x) ((double)((x)/2 << ADC_BITS) * (double)ADC_VOLTAGE_MAX)
/** Number of ADC pins **/
#define ADC_NUM_PINS 8
Pin_Init();
// Try and start things
- /*
+
const char *ret;
if ((ret = Control_SetMode(CONTROL_START, "test")) != NULL)
Fatal("Control_SetMode failed with '%s'", ret);
- */
+
// run request thread in the main thread
FCGI_RequestLoop(NULL);
- /*
+
if ((ret = Control_SetMode(CONTROL_STOP, "test")) != NULL)
Fatal("Control_SetMode failed with '%s'", ret);
- */
+
//Sensor_StopAll();
//Actuator_StopAll();
* @param min_warn - Minimum warning threshold; program will log warnings if the value falls below this threshold
* @returns Number of actuators added so far
*/
-int Sensor_Add(const char * name, int user_id, ReadFn read, InitFn init, CleanFn cleanup, double max_error, double min_error, double max_warn, double min_warn)
+int Sensor_Add(const char * name, int user_id, ReadFn read, InitFn init, CleanFn cleanup, SanityFn sanity)
{
if (++g_num_sensors > SENSORS_MAX)
{
s->sample_us = 1e6;
s->averages = 1;
- // Set warning/error thresholds
- s->thresholds.max_error = max_error;
- s->thresholds.min_error = min_error;
- s->thresholds.max_warn = max_warn;
- s->thresholds.min_warn = min_warn;
-
+ // Set sanity function
+ s->sanity = sanity;
return g_num_sensors;
}
*/
#include "sensors/resource.h"
#include "sensors/strain.h"
-#include "sensors/piped.h"
+#include "sensors/pressure.h"
void Sensor_Init()
{
- Sensor_Add("cpu_stime", RESOURCE_CPU_SYS, Resource_Read, NULL, NULL, 1e50,-1e50,1e50,-1e50);
- Sensor_Add("cpu_utime", RESOURCE_CPU_USER, Resource_Read, NULL, NULL, 1e50,-1e50,1e50,-1e50);
+ Sensor_Add("cpu_stime", RESOURCE_CPU_SYS, Resource_Read, NULL, NULL, NULL);
+ Sensor_Add("cpu_utime", RESOURCE_CPU_USER, Resource_Read, NULL, NULL, NULL);
+ Sensor_Add("pressure_high0", PRES_HIGH0, Pressure_Read, Pressure_Init, Pressure_Cleanup, NULL);
+ Sensor_Add("pressure_high1", PRES_HIGH1, Pressure_Read, Pressure_Init, Pressure_Cleanup, NULL);
+ Sensor_Add("pressure_low0", PRES_LOW0, Pressure_Read, Pressure_Init, Pressure_Cleanup, NULL);
//Sensor_Add("../testing/count.py", 0, Piped_Read, Piped_Init, Piped_Cleanup, 1e50,-1e50,1e50,-1e50);
//Sensor_Add("strain0", STRAIN0, Strain_Read, Strain_Init, 5000,0,5000,0);
//Sensor_Add("strain1", STRAIN1, Strain_Read, Strain_Init, 5000,0,5000,0);
}
-/**
- * Checks the sensor data for unsafe or unexpected results
- * @param sensor_id - The ID of the sensor
- * @param value - The value from the sensor to test
- */
-void Sensor_CheckData(Sensor * s, double value)
-{
- if( value > s->thresholds.max_error || value < s->thresholds.min_error)
- {
- Log(LOGERR, "Sensor %s at %f is above or below its safety value of %f or %f\n",s->name,value, s->thresholds.max_error, s->thresholds.min_error);
- //new function that stops actuators?
- //Control_SetMode(CONTROL_EMERGENCY, NULL)
- }
- else if( value > s->thresholds.max_warn || value < s->thresholds.min_warn)
- {
- Log(LOGWARN, "Sensor %s at %f is above or below its warning value of %f or %f\n", s->name,value,s->thresholds.max_warn, s->thresholds.min_warn);
- }
-}
-
-
/**
* Record data from a single Sensor; to be run in a seperate thread
* @param arg - Cast to Sensor* - Sensor that the thread will handle
if (success)
{
-
-
- Sensor_CheckData(s, d.value);
+ if (s->sanity != NULL)
+ {
+ if (!s->sanity(s->user_id, d.value))
+ {
+ Fatal("Sensor %s (%d,%d) reads unsafe value", s->name, s->id, s->user_id);
+ }
+ }
Data_Save(&(s->data_file), &d, 1); // Record it
}
else
InitFn init;
/** Function to cleanup the sensor **/
CleanFn cleanup;
+ /** Function to sanity check the sensor readings **/
+ SanityFn sanity;
/** Human readable name of the sensor **/
const char * name;
- /** Thresholds on the sensor **/
- SensorThreshold thresholds;
/** Sampling rate **/
int sample_us;
- /** Averages per DataPoint **/
+ /** Number of averages per sample **/
int averages;
+
} Sensor;
extern void * Sensor_Loop(void * args); // Main loop for a thread that handles a Sensor
extern bool Sensor_Read(Sensor * s, DataPoint * d); // Read a single DataPoint, indicating if it has changed since the last one
-extern void Sensor_CheckData(Sensor * s, double value); // Check a DataPoint
extern Sensor * Sensor_Identify(const char * str); // Identify a Sensor from a string
extern void Sensor_Handler(FCGIContext *context, char * params); // Handle a FCGI request for Sensor data
FLAGS = -std=c99 -Wall -pedantic -g -I../
#-I/usr/include/opencv -I/usr/include/opencv2/highgui For OpenCV
LIB = -lpthread
-OBJ = strain.o resource.o piped.o
+OBJ = strain.o resource.o pressure.o
HEADERS = $(wildcard *.h)
RM = rm -f
--- /dev/null
+/**
+ * @file pressure.c
+ * @purpose Implementation of Pressure reading functions
+ */
+
+#include "pressure.h"
+#include "../bbb_pin.h"
+#include "../log.h" // For Fatal()
+
+#define PSI_TO_KPA 6.89475729
+
+/**
+ * Get the ADC number of a Pressure sensor
+ * @param id - Id of the sensor
+ * @returns ADC as defined in bbb_pin_defines.h
+ */
+static int Pressure_GetADC(int id)
+{
+ switch (id)
+ {
+ case PRES_HIGH0:
+ return ADC1;
+ case PRES_HIGH1:
+ return ADC3;
+ case PRES_LOW0:
+ return ADC5;
+ default:
+ Fatal("Unknown Pressure id %d", id);
+ return -1; // Should never happen
+ }
+}
+
+/**
+ * Convert an ADC voltage into a Pressure reading
+ * @param id - Sensor ID
+ * @param adc - ADC reading
+ * @returns Pressure in kPa
+ */
+double Pressure_Callibrate(int id, int adc)
+{
+ //double voltage = ADC_TO_VOLTS(adc); // convert reading to voltage
+
+ switch (id)
+ {
+ case PRES_HIGH0:
+ case PRES_HIGH1:
+ {
+ static const double Vs = 5e3; // In mVs
+ static const double Pmin = 0.0;
+ static const double Pmax = 150.0 * PSI_TO_KPA;
+ double Vout = ADC_TO_MVOLTS(adc);
+ return ((Vout - 0.1*Vs)/(0.8*Vs))*(Pmax - Pmin) + Pmin;
+ }
+ case PRES_LOW0:
+ return (200.0 * (adc / ADC_RAW_MAX));
+ default:
+ Fatal("Unknown Pressure id %d", id);
+ return -1; // Should never happen
+ }
+
+}
+
+/**
+ * Initialise a Pressure sensor
+ * @param name - Ignored
+ * @param id - The id of the Pressure sensor
+ * @returns true on success, false on error
+ */
+bool Pressure_Init(const char * name, int id)
+{
+ return ADC_Export(Pressure_GetADC(id));
+}
+
+/**
+ * Cleanup a Pressure Sensor
+ * @param id - The id of the sensor to cleanup
+ * @returns true on success, false on failure
+ */
+bool Pressure_Cleanup(int id)
+{
+ ADC_Unexport(Pressure_GetADC(id));
+ return true;
+}
+
+/**
+ * Read a Pressure Sensor
+ * @param id - id of the sensor to read
+ * @param value - Where the value will be stored on a successful read
+ * @returns true on success, false on failure
+ */
+bool Pressure_Read(int id, double * value)
+{
+ //static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+ //pthread_mutex_lock(&mutex);
+ bool result = false;
+ if (ADC_Read(Pressure_GetADC(id), value))
+ {
+ *value = Pressure_Callibrate(id, *value);
+ result = true;
+ }
+ //pthread_mutex_unlock(&mutex);
+ return result;
+}
--- /dev/null
+/**
+ * @file pressure.h
+ * @purpose Declarations for Pressure Sensor functions
+ */
+
+#ifndef _PRESSURE_H
+
+#include "../common.h"
+
+typedef enum
+{
+ PRES_HIGH0,
+ PRES_HIGH1,
+ PRES_LOW0
+} PressureId;
+
+extern bool Pressure_Init(const char * name, int id);
+extern bool Pressure_Cleanup(int id);
+extern bool Pressure_Read(int id, double * value);
+
+#endif //_PRESSURE_H
+
+