-/**
- * 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(SensorId id, double value)
-{
- if( value > thresholds[id].max_error || value < thresholds[id].min_error)
- {
- Log(LOGERR, "Sensor %s at %f is above or below its safety value of %f or %f\n", g_sensor_names[id],value, thresholds[id].max_error, thresholds[id].min_error);
- //new function that stops actuators?
- //Control_SetMode(CONTROL_EMERGENCY, NULL)
- }
- else if( value > thresholds[id].max_warn || value < thresholds[id].min_warn)
- {
- Log(LOGWARN, "Sensor %s at %f is above or below its warning value of %f or %f\n", g_sensor_names[id],value,thresholds[id].max_warn, thresholds[id].min_warn);
- }
-}
-
-
-/**
- * Read a DataPoint from a Sensor; block until value is read
- * @param id - The ID of the sensor
- * @param d - DataPoint to set
- * @returns True if the DataPoint was different from the most recently recorded.
- */
-bool Sensor_Read(Sensor * s, DataPoint * d)
-{
-
-
-
- static bool result = true;
-
- //TODO: Remove this, code should be refactored to not use so many threads
- // Although... if it works, it works...
- static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
-
- pthread_mutex_lock(&mutex); //TODO: Reduce the critical section
-
- usleep(10);
-
- // Set time stamp
- struct timeval t;
- gettimeofday(&t, NULL);
- d->time_stamp = TIMEVAL_DIFF(t, *Control_GetStartTime());
-
- // Read value based on Sensor Id
- int value; bool success = true;
- //TODO: Can probably do this nicer than a switch (define a function pointer for each sensor)
- // Can probably make the whole sensor thing a lot nicer with a linked list of sensors...
- // (Then to add more sensors to the software, someone just writes an appropriate read function and calls Sensor_Add(...) at init)
- // (I will do this. Don't do it before I get a chance, I don't trust you :P)
- switch (s->id)
- {
- //TODO: Strain gauges should have their own critical section, rest of sensors probably don't need to be in a critical section
- case STRAIN0:
- success &= GPIO_Set(GPIO0_30, true);
- success &= ADC_Read(ADC0);
- success &= GPIO_Set(GPIO0_30, false);
- if (!success)
- Fatal("Error reading strain gauge 0");
- break;
- case STRAIN1:
- success &= GPIO_Set(GPIO1_28, true);
- success &= ADC_Read(ADC0);
- success &= GPIO_Set(GPIO1_28, false);
- if (!success)
- Fatal("Error reading strain gauge 1");
- break;
- case STRAIN2:
- success &= GPIO_Set(GPIO0_31, true);
- success &= ADC_Read(ADC0);
- success &= GPIO_Set(GPIO0_31, false);
- case STRAIN3:
- success &= GPIO_Set(GPIO1_16, true);
- success &= ADC_Read(ADC0);
- success &= GPIO_Set(GPIO1_16, false);
- if (!success)
- Fatal("Error reading strain gauge 2");
- break;
- case PRESSURE0:
- success &= ADC_Read(ADC1, &value);
- break;
- case PRESSURE1:
- success &= ADC_Read(ADC5, &value);
- break;
- case PRESSURE_FEEDBACK:
- success &= ADC_Read(ADC4, &value);
- break;
- case MICROPHONE:
- success &= ADC_Read(ADC2, &value);
- break;
- case ENCLOSURE:
- {
- bool why_do_i_need_to_do_this = false;
- success &= GPIO_Read(GPIO0_31, &why_do_i_need_to_do_this);
- value = (int)why_do_i_need_to_do_this;
- break;
- }
- case DILATOMETER:
- {
- // Will definitely cause issues included in the same critical section as ADC reads
- // (since it will be the longest sensor to sample, everything else will have to keep waiting on it)
- value = 0;
- break;
- }
-
- }
-
- d->value = (double)(value); //TODO: Calibration? Or do calibration in GUI
-
- pthread_mutex_unlock(&mutex); //TODO: Reduce the critical section
-
-
- // Perform sanity check based on Sensor's ID and the DataPoint
- Sensor_CheckData(s->id, d->value);
-
- // Update latest DataPoint if necessary
-
- if (result)
- {
- s->newest_data.time_stamp = d->time_stamp;
- s->newest_data.value = d->value;
- }
-
-#ifdef _BBB
- //Not all cases have usleep, easiest here.
- //TODO: May want to add a control option to adjust the sampling rate for each sensor?
- // Also, we can get a more accurate sampling rate if instead of a fixed sleep, we calculate how long to sleep each time.
- usleep(100000);
-#endif
-
- /*
- if (success)
- Log(LOGDEBUG, "Successfully read sensor %d (for once)", s->id);
- else
- Log(LOGDEBUG, "Failed to read sensor %d (again)", s->id);
- */
- return result && success;
-}
-