diff options
Diffstat (limited to 'apps/sensors/sensors.cpp')
-rw-r--r-- | apps/sensors/sensors.cpp | 75 |
1 files changed, 48 insertions, 27 deletions
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 77dc0aa31..217b05e9d 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -653,9 +653,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw) accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_counter++; } else { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + raw.accelerometer_counter++; + } } raw.accelerometer_m_s2[0] = accel_report.x; @@ -665,8 +672,6 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - - raw.accelerometer_counter++; } void @@ -692,50 +697,66 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f; } else { - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + bool gyro_updated; + orb_check(_gyro_sub, &gyro_updated); - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; + if (gyro_updated) { + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_counter++; + raw.gyro_rad_s[0] = gyro_report.x; + raw.gyro_rad_s[1] = gyro_report.y; + raw.gyro_rad_s[2] = gyro_report.z; + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.gyro_counter++; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - struct mag_report mag_report; + bool mag_updated; + orb_check(_mag_sub, &mag_updated); - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + if (mag_updated) { + struct mag_report mag_report; - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; - - raw.magnetometer_counter++; + raw.magnetometer_ga[0] = mag_report.x; + raw.magnetometer_ga[1] = mag_report.y; + raw.magnetometer_ga[2] = mag_report.z; + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_counter++; + } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - struct baro_report baro_report; + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); - raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_counter++; + } } void |