diff options
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 181 |
1 files changed, 88 insertions, 93 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 867d6c339..87d7da537 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -512,7 +512,7 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), -/* subscriptions */ + /* subscriptions */ _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), @@ -530,7 +530,7 @@ Sensors::Sensors() : _rc_parameter_map_sub(-1), _manual_control_sub(-1), -/* publications */ + /* publications */ _sensor_pub(-1), _manual_control_pub(-1), _actuator_group_3_pub(-1), @@ -539,7 +539,7 @@ Sensors::Sensors() : _airspeed_pub(-1), _diff_pres_pub(-1), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _mag_is_external(false), @@ -619,29 +619,29 @@ Sensors::Sensors() : _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); /* gyro offsets */ - _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); - _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); - _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); - _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); - _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); - _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); + _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("SENS_ACC_XOFF"); - _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF"); - _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF"); - _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE"); - _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE"); - _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE"); + _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("SENS_MAG_XOFF"); - _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); - _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); + _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("SENS_MAG_XSCALE"); - _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); - _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + _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"); @@ -786,9 +786,11 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -883,15 +885,18 @@ Sensors::parameters_update() /* set px4flow rotation */ int flowfd; flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { - int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); - if (flowret) { - warnx("flow rotation could not be set"); + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + + if (flowret) { + warnx("flow rotation could not be set"); close(flowfd); return ERROR; - } + } + close(flowfd); - } + } get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -902,9 +907,9 @@ Sensors::parameters_update() /** fine tune board offset on parameter update **/ math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); _board_rotation = _board_rotation * board_rotation_offset; @@ -912,17 +917,20 @@ Sensors::parameters_update() param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); int barofd; barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); return ERROR; } else { int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); close(barofd); return ERROR; } + close(barofd); } @@ -942,28 +950,11 @@ Sensors::accel_init() } else { - // XXX do the check more elegantly - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the accel internal sampling rate to default rate */ + ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); - /* set the accel internal sampling rate up to at leat 1000Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 1000); - - /* set the driver to poll at 1000Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 1000); - -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE - - /* set the accel internal sampling rate up to at leat 800Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 800); - - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); - -#else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE - -#endif + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); } @@ -984,31 +975,12 @@ Sensors::gyro_init() } else { - // XXX do the check more elegantly + /* set the gyro internal sampling rate to default rate */ + ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the driver to poll at default rate */ + ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { - ioctl(fd, GYROIOCSSAMPLERATE, 800); - } - - /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { - ioctl(fd, SENSORIOCSPOLLRATE, 800); - } - -#else - - /* set the gyro internal sampling rate up to at least 760Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 760); - - /* set the driver to poll at 760Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 760); - -#endif - - close(fd); } return OK; @@ -1348,14 +1320,17 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; /* don't risk to feed negative airspeed into the system */ - _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); - _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1479,8 +1454,10 @@ Sensors::parameter_update_poll(bool forced) } #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("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); @@ -1496,6 +1473,7 @@ Sensors::rc_parameter_map_poll(bool forced) if (map_updated) { orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { @@ -1508,21 +1486,24 @@ Sensors::rc_parameter_map_poll(bool forced) /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); } } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", - i, - _rc_parameter_map.param_id[i], - (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i], - (double)_rc_parameter_map.value_min[i], - (double)_rc_parameter_map.value_max[i] - ); + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); } } } @@ -1616,7 +1597,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ @@ -1709,6 +1691,7 @@ void Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) @@ -1717,14 +1700,14 @@ Sensors::set_params_from_rc() continue; } - float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { param_rc_values[i] = rc_val; float param_val = math::constrain( - _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, - _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); param_set(_parameter_handles.rc_param[i], ¶m_val); } } @@ -1808,10 +1791,12 @@ Sensors::rc_poll() * DO NOT REMOVE OR ALTER STEP 1! */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( + _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( + _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ @@ -1899,6 +1884,7 @@ Sensors::rc_poll() /* Update parameters from RC Channels (tuning with RC) if activated */ static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { set_params_from_rc(); last_rc_to_param_map_time = hrt_absolute_time(); @@ -1920,22 +1906,31 @@ Sensors::task_main() /* start individual sensors */ int ret; ret = accel_init(); + if (ret) { goto exit_immediate; } + ret = gyro_init(); + if (ret) { goto exit_immediate; } + ret = mag_init(); + if (ret) { goto exit_immediate; } + ret = baro_init(); + if (ret) { goto exit_immediate; } + ret = adc_init(); + if (ret) { goto exit_immediate; } @@ -2075,7 +2070,7 @@ Sensors::start() nullptr); /* wait until the task is up and running or has failed */ - while(_sensors_task > 0 && _task_should_exit) { + while (_sensors_task > 0 && _task_should_exit) { usleep(100); } |