/**
* Set an Actuator value
* @param a - The Actuator
* @param value - The value to set
*/
void Actuator_SetValue(Actuator * a, double value, bool record)
{
if (a->sanity != NULL && !a->sanity(a->user_id, value))
{
//ARE YOU INSANE?
Log(LOGERR,"Insane value %lf for actuator %s", value, a->name);
return;
}
if (!(a->set(a->user_id, value)))
{
Fatal("Failed to set actuator %s to %lf", a->name, value);
}
// Set time stamp
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
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;
}
/**
* Record data from a single Sensor; to be run in a seperate thread
* @param arg - Cast to Sensor* - Sensor that the thread will handle
* @returns NULL (void* required to use the function with pthreads)
*/
void * Sensor_Loop(void * arg)
{
Sensor * s = (Sensor*)(arg);
Log(LOGDEBUG, "Sensor %d starts", s->id);
// Until the sensor is stopped, record data points
while (s->activated)
{
bool success = s->read(s->user_id, &(s->current_data.value));
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
s->current_data.time_stamp = TIMEVAL_DIFF(t, *Control_GetStartTime());
if (success)
{
if (s->sanity != NULL)
{
if (!s->sanity(s->user_id, s->current_data.value))
{
Fatal("Sensor %s (%d,%d) reads unsafe value", s->name, s->id, s->user_id);
}
}
s->averaged_data.time_stamp += s->current_data.time_stamp;
s->averaged_data.value = s->current_data.value;
if (++(s->num_read) >= s->averages)
{
s->averaged_data.time_stamp /= s->averages;
s->averaged_data.value /= s->averages;
Data_Save(&(s->data_file), &(s->averaged_data), 1); // Record it
s->num_read = 0;
s->averaged_data.time_stamp = 0;
s->averaged_data.value = 0;
}
}
else
{
// Silence because strain sensors fail ~50% of the time :S
//Log(LOGWARN, "Failed to read sensor %s (%d,%d)", s->name, s->id,s->user_id);
}
clock_nanosleep(CLOCK_MONOTONIC, 0, &(s->sample_time), NULL);
}
// Needed to keep pthreads happy
Log(LOGDEBUG, "Sensor %s (%d,%d) finished", s->name,s->id,s->user_id);
return NULL;
}
/**
* Handle a request for an Actuator
* @param context - FCGI context
* @param params - Parameters passed
*/
void Actuator_Handler(FCGIContext * context, char * params)
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
double current_time = TIMEVAL_DIFF(now, *Control_GetStartTime());
int id = 0;
char * name = "";
char * set = "";
double start_time = 0;
double end_time = current_time;
char * fmt_str;
// key/value pairs
FCGIValue values[] = {
{"id", &id, FCGI_INT_T},
{"name", &name, FCGI_STRING_T},
{"set", &set, FCGI_STRING_T},
{"start_time", &start_time, FCGI_DOUBLE_T},
{"end_time", &end_time, FCGI_DOUBLE_T},
{"format", &fmt_str, FCGI_STRING_T}
};
// enum to avoid the use of magic numbers
typedef enum {
ID,
NAME,
SET,
START_TIME,
END_TIME,
FORMAT
} ActuatorParams;
// Fill values appropriately
if (!FCGI_ParseRequest(context, params, values, sizeof(values)/sizeof(FCGIValue)))
{
// Error occured; FCGI_RejectJSON already called
return;
}
// Get the Actuator identified
Actuator * a = NULL;
if (FCGI_RECEIVED(values[NAME].flags))
{
if (FCGI_RECEIVED(values[ID].flags))
{
FCGI_RejectJSON(context, "Can't supply both id and name");
return;
}
a = Actuator_Identify(name);
if (a == NULL)
{
FCGI_RejectJSON(context, "Unknown actuator name");
return;
}
}
else if (!FCGI_RECEIVED(values[ID].flags))
{
FCGI_RejectJSON(context, "No id or name supplied");
return;
}
else if (id < 0 || id >= g_num_actuators)
{
FCGI_RejectJSON(context, "Invalid Actuator id");
return;
}
else
{
a = &(g_actuators[id]);
}
DataFormat format = Data_GetFormat(&(values[FORMAT]));
if (FCGI_RECEIVED(values[SET].flags))
{
ActuatorControl c = {0.0, 0.0, 0.0, 0}; // Need to set default values (since we don't require them all)
// sscanf returns the number of fields successfully read...
int n = sscanf(set, "%lf,%lf,%lf,%d", &(c.start), &(c.stepwait), &(c.stepsize), &(c.steps)); // Set provided values in order
if (n != 4)
{
// If the user doesn't provide all 4 values, the Actuator will get set *once* using the first of the provided values
// (see Actuator_Loop)
// Not really a problem if n = 1, but maybe generate a warning for 2 <= n < 4 ?
Log(LOGDEBUG, "Only provided %d values (expect %d) for Actuator setting", n, 4);
}
// SANITY CHECKS
if (c.stepwait < 0 || c.steps < 0 || (a->sanity != NULL && !a->sanity(a->user_id, c.start)))
{
FCGI_RejectJSON(context, "Bad Actuator setting");
return;
}
Actuator_SetControl(a, &c);
}
// Begin response
Actuator_BeginResponse(context, a, format);
if (format == JSON)
FCGI_JSONPair("set", set);
// Print Data
Data_Handler(&(a->data_file), &(values[START_TIME]), &(values[END_TIME]), format, current_time);
// Finish response
Actuator_EndResponse(context, a, format);
}
/**
* Handle a request to the sensor module
* @param context - The context to work in
* @param params - Parameters passed
*/
void Sensor_Handler(FCGIContext *context, char * params)
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
double current_time = TIMEVAL_DIFF(now, *Control_GetStartTime());
int id = 0;
const char * name = "";
double start_time = 0;
double end_time = current_time;
const char * fmt_str;
double sample_s = 0;
// key/value pairs
FCGIValue values[] = {
{"id", &id, FCGI_INT_T},
{"name", &name, FCGI_STRING_T},
{"format", &fmt_str, FCGI_STRING_T},
{"start_time", &start_time, FCGI_DOUBLE_T},
{"end_time", &end_time, FCGI_DOUBLE_T},
{"sample_s", &sample_s, FCGI_DOUBLE_T}
};
// enum to avoid the use of magic numbers
typedef enum {
ID,
NAME,
FORMAT,
START_TIME,
END_TIME,
SAMPLE_S
} SensorParams;
// Fill values appropriately
if (!FCGI_ParseRequest(context, params, values, sizeof(values)/sizeof(FCGIValue)))
{
// Error occured; FCGI_RejectJSON already called
return;
}
Sensor * s = NULL;
if (FCGI_RECEIVED(values[NAME].flags))
{
if (FCGI_RECEIVED(values[ID].flags))
{
FCGI_RejectJSON(context, "Can't supply both sensor id and name");
return;
}
s = Sensor_Identify(name);
if (s == NULL)
{
FCGI_RejectJSON(context, "Unknown sensor name");
return;
}
}
else if (!FCGI_RECEIVED(values[ID].flags))
{
FCGI_RejectJSON(context, "No sensor id or name supplied");
return;
}
else if (id < 0 || id >= g_num_sensors)
{
FCGI_RejectJSON(context, "Invalid sensor id");
return;
}
else
{
s = &(g_sensors[id]);
}
// Adjust sample rate if necessary
if (FCGI_RECEIVED(values[SAMPLE_S].flags))
{
if (sample_s < 0)
{
FCGI_RejectJSON(context, "Negative sampling speed!");
return;
}
DOUBLE_TO_TIMEVAL(sample_s, &(s->sample_time));
}
DataFormat format = Data_GetFormat(&(values[FORMAT]));
// Begin response
Sensor_BeginResponse(context, s, format);
// Print Data
Data_Handler(&(s->data_file), &(values[START_TIME]), &(values[END_TIME]), format, current_time);
// Finish response
Sensor_EndResponse(context, s, format);
}
