diff options
-rw-r--r-- | src/modules/sensors/sensors.cpp | 343 |
1 files changed, 212 insertions, 131 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 87d7da537..cf4cafb87 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -35,9 +35,9 @@ * @file sensors.cpp * Sensor readout process. * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Lorenz Meier <lorenz@px4.io> + * @author Julian Oes <julian@px4.io> + * @author Thomas Gubler <thomas@px4.io> */ #include <nuttx/config.h> @@ -151,6 +151,8 @@ #endif static const int ERROR = -1; +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" + /** * Sensor app start / stop handling function * @@ -266,12 +268,6 @@ private: float dz[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; - float gyro_offset[3]; - float gyro_scale[3]; - float mag_offset[3]; - float mag_scale[3]; - float accel_offset[3]; - float accel_scale[3]; float diff_pres_offset_pa; float diff_pres_analog_scale; @@ -334,12 +330,6 @@ private: param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t gyro_offset[3]; - param_t gyro_scale[3]; - param_t accel_offset[3]; - param_t accel_scale[3]; - param_t mag_offset[3]; - param_t mag_scale[3]; param_t diff_pres_offset_pa; param_t diff_pres_analog_scale; @@ -618,31 +608,6 @@ Sensors::Sensors() : _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); - /* gyro offsets */ - _parameter_handles.gyro_offset[0] = param_find("CAL_GYRO0_XOFF"); - _parameter_handles.gyro_offset[1] = param_find("CAL_GYRO0_YOFF"); - _parameter_handles.gyro_offset[2] = param_find("CAL_GYRO0_ZOFF"); - _parameter_handles.gyro_scale[0] = param_find("CAL_GYRO0_XSCALE"); - _parameter_handles.gyro_scale[1] = param_find("CAL_GYRO0_YSCALE"); - _parameter_handles.gyro_scale[2] = param_find("CAL_GYRO0_ZSCALE"); - - /* accel offsets */ - _parameter_handles.accel_offset[0] = param_find("CAL_ACC0_XOFF"); - _parameter_handles.accel_offset[1] = param_find("CAL_ACC0_YOFF"); - _parameter_handles.accel_offset[2] = param_find("CAL_ACC0_ZOFF"); - _parameter_handles.accel_scale[0] = param_find("CAL_ACC0_XSCALE"); - _parameter_handles.accel_scale[1] = param_find("CAL_ACC0_YSCALE"); - _parameter_handles.accel_scale[2] = param_find("CAL_ACC0_ZSCALE"); - - /* mag offsets */ - _parameter_handles.mag_offset[0] = param_find("CAL_MAG0_XOFF"); - _parameter_handles.mag_offset[1] = param_find("CAL_MAG0_YOFF"); - _parameter_handles.mag_offset[2] = param_find("CAL_MAG0_ZOFF"); - - _parameter_handles.mag_scale[0] = param_find("CAL_MAG0_XSCALE"); - _parameter_handles.mag_scale[1] = param_find("CAL_MAG0_YSCALE"); - _parameter_handles.mag_scale[2] = param_find("CAL_MAG0_ZSCALE"); - /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); @@ -839,31 +804,6 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } - /* gyro offsets */ - param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); - param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); - param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2])); - param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0])); - param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1])); - param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2])); - - /* accel offsets */ - param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0])); - param_get(_parameter_handles.accel_offset[1], &(_parameters.accel_offset[1])); - param_get(_parameter_handles.accel_offset[2], &(_parameters.accel_offset[2])); - param_get(_parameter_handles.accel_scale[0], &(_parameters.accel_scale[0])); - param_get(_parameter_handles.accel_scale[1], &(_parameters.accel_scale[1])); - param_get(_parameter_handles.accel_scale[2], &(_parameters.accel_scale[2])); - - /* mag offsets */ - param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0])); - param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1])); - param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2])); - /* mag scaling */ - param_get(_parameter_handles.mag_scale[0], &(_parameters.mag_scale[0])); - param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); - param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); - /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); @@ -884,7 +824,7 @@ Sensors::parameters_update() /* set px4flow rotation */ int flowfd; - flowfd = open(PX4FLOW_DEVICE_PATH, 0); + flowfd = open(PX4FLOW0_DEVICE_PATH, 0); if (flowfd >= 0) { int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); @@ -916,10 +856,10 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); int barofd; - barofd = open(BARO_DEVICE_PATH, 0); + barofd = open(BARO0_DEVICE_PATH, 0); if (barofd < 0) { - warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); + warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); return ERROR; } else { @@ -942,10 +882,10 @@ Sensors::accel_init() { int fd; - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL0_DEVICE_PATH, 0); if (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH); + warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); return ERROR; } else { @@ -967,10 +907,10 @@ Sensors::gyro_init() { int fd; - fd = open(GYRO_DEVICE_PATH, 0); + fd = open(GYRO0_DEVICE_PATH, 0); if (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH); + warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); return ERROR; } else { @@ -992,10 +932,10 @@ Sensors::mag_init() int fd; int ret; - fd = open(MAG_DEVICE_PATH, 0); + fd = open(MAG0_DEVICE_PATH, 0); if (fd < 0) { - warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH); + warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); return ERROR; } @@ -1047,10 +987,10 @@ Sensors::baro_init() { int fd; - fd = open(BARO_DEVICE_PATH, 0); + fd = open(BARO0_DEVICE_PATH, 0); if (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH); + warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); return ERROR; } @@ -1066,10 +1006,10 @@ int Sensors::adc_init() { - _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + _fd_adc = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { - warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH); + warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); return ERROR; } @@ -1099,6 +1039,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[2] = accel_report.z_raw; raw.accelerometer_timestamp = accel_report.timestamp; + raw.accelerometer_errcount = accel_report.error_count; } orb_check(_accel1_sub, &accel_updated); @@ -1120,6 +1061,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer1_raw[2] = accel_report.z_raw; raw.accelerometer1_timestamp = accel_report.timestamp; + raw.accelerometer1_errcount = accel_report.error_count; } orb_check(_accel2_sub, &accel_updated); @@ -1141,6 +1083,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer2_raw[2] = accel_report.z_raw; raw.accelerometer2_timestamp = accel_report.timestamp; + raw.accelerometer2_errcount = accel_report.error_count; } } @@ -1167,6 +1110,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[2] = gyro_report.z_raw; raw.timestamp = gyro_report.timestamp; + raw.gyro_errcount = gyro_report.error_count; } orb_check(_gyro1_sub, &gyro_updated); @@ -1188,6 +1132,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro1_raw[2] = gyro_report.z_raw; raw.gyro1_timestamp = gyro_report.timestamp; + raw.gyro1_errcount = gyro_report.error_count; } orb_check(_gyro2_sub, &gyro_updated); @@ -1209,6 +1154,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro2_raw[2] = gyro_report.z_raw; raw.gyro2_timestamp = gyro_report.timestamp; + raw.gyro2_errcount = gyro_report.error_count; } } @@ -1243,6 +1189,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[2] = mag_report.z_raw; raw.magnetometer_timestamp = mag_report.timestamp; + raw.magnetometer_errcount = mag_report.error_count; } orb_check(_mag1_sub, &mag_updated); @@ -1257,6 +1204,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer1_raw[2] = mag_report.z_raw; raw.magnetometer1_timestamp = mag_report.timestamp; + raw.magnetometer1_errcount = mag_report.error_count; } orb_check(_mag2_sub, &mag_updated); @@ -1271,6 +1219,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer2_raw[2] = mag_report.z_raw; raw.magnetometer2_timestamp = mag_report.timestamp; + raw.magnetometer2_errcount = mag_report.error_count; } } @@ -1387,56 +1336,187 @@ Sensors::parameter_update_poll(bool forced) /* update parameters */ parameters_update(); - /* update sensor offsets */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - _parameters.gyro_offset[0], - _parameters.gyro_scale[0], - _parameters.gyro_offset[1], - _parameters.gyro_scale[1], - _parameters.gyro_offset[2], - _parameters.gyro_scale[2], - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { - warn("WARNING: failed to set scale / offsets for gyro"); - } + /* set offset parameters to new values */ + bool failed; + int res; + char str[30]; - close(fd); + /* run through all gyro sensors */ + for (unsigned s = 0; s < 3; s++) { + + failed = false; + res = ERROR; + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + + + (void)sprintf(str, "CAL_GYRO%u_ID", s); + int device_id; + failed |= (OK != param_get(param_find(str), &device_id)); + + if (failed) { + close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct gyro_scale gscale = {}; + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + /* apply new scaling and offsets */ + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + } + break; + } + } + + close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - _parameters.accel_offset[0], - _parameters.accel_scale[0], - _parameters.accel_offset[1], - _parameters.accel_scale[1], - _parameters.accel_offset[2], - _parameters.accel_scale[2], - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { - warn("WARNING: failed to set scale / offsets for accel"); + if (res != OK) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } } - close(fd); + /* run through all accel sensors */ + for (unsigned s = 0; s < 3; s++) { + + failed = false; + res = ERROR; + (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + + (void)sprintf(str, "CAL_ACC%u_ID", s); + int device_id; + failed |= (OK != param_get(param_find(str), &device_id)); + + if (failed) { + close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct accel_scale gscale = {}; + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + /* apply new scaling and offsets */ + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + } + break; + } + } - fd = open(MAG_DEVICE_PATH, 0); - struct mag_scale mscale = { - _parameters.mag_offset[0], - _parameters.mag_scale[0], - _parameters.mag_offset[1], - _parameters.mag_scale[1], - _parameters.mag_offset[2], - _parameters.mag_scale[2], - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { - warn("WARNING: failed to set scale / offsets for mag"); + close(fd); + + if (res != OK) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } } - close(fd); + /* run through all mag sensors */ + for (unsigned s = 0; s < 3; s++) { + + failed = false; + res = ERROR; + (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + + (void)sprintf(str, "CAL_MAG%u_ID", s); + int device_id; + failed |= (OK != param_get(param_find(str), &device_id)); + + if (failed) { + close(fd); + continue; + } - fd = open(AIRSPEED_DEVICE_PATH, 0); + /* if the calibration is for this device, apply it */ + if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct mag_scale gscale = {}; + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + failed |= (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + failed |= (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + /* apply new scaling and offsets */ + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + } + break; + } + } + + close(fd); + + if (res != OK) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } + } + + int fd = open(AIRSPEED0_DEVICE_PATH, 0); /* this sensor is optional, abort without error */ @@ -1452,16 +1532,6 @@ Sensors::parameter_update_poll(bool forced) close(fd); } - -#if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], - (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], - (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); - printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100)); - fflush(stdout); - usleep(5000); -#endif } } @@ -2033,6 +2103,17 @@ Sensors::task_main() mag_poll(raw); baro_poll(raw); + /* work out if main gyro timed out and fail over to alternate gyro */ + if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + + /* if the secondary failed as well, go to the tertiary */ + if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { + fds[0].fd = _gyro2_sub; + } else { + fds[0].fd = _gyro1_sub; + } + } + /* check battery voltage */ adc_poll(raw); @@ -2049,7 +2130,7 @@ Sensors::task_main() perf_end(_loop_perf); } - warnx("[sensors] exiting."); + warnx("exiting."); exit_immediate: _sensors_task = -1; |