-/**
- * 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", value, g_sensor_names[id],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", value, g_sensor_names[id],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)
-{
-
- // Set time stamp
- struct timeval t;
- gettimeofday(&t, NULL);
- d->time_stamp = TIMEVAL_DIFF(t, *Control_GetStartTime());
-
- static bool result = true;
-
-
- // Read value based on Sensor Id
- switch (s->id)
- {
- case 2:
- {
- static bool set = false;
- int raw_adc = 0;
- //GPIO_Set(GPIO0_30, true);
- ADC_Read(ADC0, &raw_adc);
- d->value = (double)raw_adc; //ADC #0 on the Beaglebone
- //Log(LOGDEBUG, "Got value %f from ADC0", d->value);
- //GPIO_Set(GPIO0_30, false);
- set = !set;
- //GPIO_Set(GPIO1_28, set);
-
- usleep(100000);
-
- break;
- }
-
- default:
- d->value = rand() % 2;
- usleep(1000000);
- break;
-
-
- case ANALOG_TEST0:
- {
- d->value = (double)(rand() % 100) / 100;
- break;
- }
- case ANALOG_TEST1:
- {
- static int count = 0;
- count %= 500;
- d->value = count++;
- break;
- }
-
- case ANALOG_FAIL0:
- d->value = 0;
- //d->value = (double)(rand() % 6) * -( rand() % 2) / ( rand() % 100 + 1);
- //Gives a value between -5 and 5
- break;
- case DIGITAL_TEST0:
- d->value = t.tv_sec % 2;
-
- break;
- case DIGITAL_TEST1:
- d->value = (t.tv_sec+1)%2;
- break;
- case DIGITAL_REALTEST:
- {
- d->value = 0; //d->value must be something... valgrind...
- // Can pass pin as argument, just using 20 as an example here
- // Although since pins will be fixed, can just define it here if we need to
- //d->value = pinRead(20); //Pin 20 on the Beaglebone
- break;
- }
- case DIGITAL_FAIL0:
- if( rand() % 100 > 98)
- d->value = 2;
- d->value = rand() % 2;
- //Gives 0 or 1 or a 2 every 1/100 times
- break;
- //default:
- // Fatal("Unknown sensor id: %d", s->id);
- // break;
- }
-
-
- // 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.
- usleep(1000000);
-#endif
- return result;
-}
-