diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c14afb322f..eb52ce8296 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,7 +167,8 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = + input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -227,7 +228,7 @@ private: orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ - + perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ @@ -366,7 +367,7 @@ private: int init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount); + uint32_t *priorities, uint32_t *errcount); /** * Update our local parameter cache. @@ -489,10 +490,10 @@ Sensors::Sensors() : _armed(false), /* subscriptions */ - _gyro_sub{-1, -1, -1}, - _accel_sub{-1, -1, -1}, - _mag_sub{-1, -1, -1}, - _baro_sub{-1, -1, -1}, + _gyro_sub{ -1, -1, -1}, + _accel_sub{ -1, -1, -1}, + _mag_sub{ -1, -1, -1}, + _baro_sub{ -1, -1, -1}, _gyro_count(0), _accel_count(0), _mag_count(0), @@ -588,7 +589,8 @@ Sensors::Sensors() : /* RC to parameter mapping for changing parameters with RC */ for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { char name[rc_parameter_map_s::PARAM_ID_LEN]; - snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", + i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -641,7 +643,7 @@ Sensors::Sensors() : (void)param_find("PWM_AUX_MAX"); (void)param_find("PWM_AUX_DISARMED"); (void)param_find("TRIG_MODE"); - + /* fetch initial parameter values */ parameters_update(); } @@ -825,19 +827,20 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("%s", paramerr); + } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.0082f; - #elif CONFIG_ARCH_BOARD_AEROCORE +#elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.00459340659f; - #else +#else /* ensure a missing default trips a low voltage lockdown */ _parameters.battery_voltage_scaling = 0.00001f; - #endif +#endif } /* scaling of ADC ticks to battery current */ @@ -1098,9 +1101,11 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; raw.gyro_timestamp[i] = gyro_report.timestamp; + if (i == 0) { raw.timestamp = gyro_report.timestamp; } + raw.gyro_errcount[i] = gyro_report.error_count; raw.gyro_temp[i] = gyro_report.temperature; } @@ -1183,8 +1188,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1265,7 +1270,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1297,15 +1302,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { config_ok = true; } } + break; } } @@ -1331,7 +1340,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1363,15 +1372,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { config_ok = true; } } + break; } } @@ -1404,7 +1417,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1444,6 +1457,7 @@ Sensors::parameter_update_poll(bool forced) /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); + } else { int32_t mag_rot; @@ -1488,15 +1502,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { config_ok = true; } } + break; } } @@ -1764,6 +1782,7 @@ Sensors::set_params_from_rc() } 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) { @@ -1895,24 +1914,30 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, + _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, + _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, + _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, + _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, + _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) { @@ -1964,7 +1989,7 @@ Sensors::task_main_trampoline(int argc, char *argv[]) int Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount) + uint32_t *priorities, uint32_t *errcount) { unsigned group_count = orb_group_count(meta); @@ -1975,7 +2000,7 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, for (unsigned i = 0; i < group_count; i++) { if (subs[i] < 0) { subs[i] = orb_subscribe_multi(meta, i); - orb_priority(subs[i], (int32_t*)&priorities[i]); + orb_priority(subs[i], (int32_t *)&priorities[i]); } } @@ -1988,27 +2013,40 @@ Sensors::task_main() /* start individual sensors */ int ret = 0; + do { /* create a scope to handle exit with break */ ret = accel_init(); - if (ret) break; + + if (ret) { break; } + ret = gyro_init(); - if (ret) break; + + if (ret) { break; } + ret = mag_init(); - if (ret) break; + + if (ret) { break; } + ret = baro_init(); - if (ret) break; + + if (ret) { break; } + ret = adc_init(); - if (ret) break; + + if (ret) { break; } + break; } while (0); if (ret) { warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >=0) { + + if (_fd_adc >= 0) { px4_close(_fd_adc); _fd_adc = -1; } + return; } @@ -2016,28 +2054,31 @@ Sensors::task_main() /* ensure no overflows can occur */ static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, - "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); /* * do subscriptions */ unsigned gcount_prev = _gyro_count; + unsigned mcount_prev = _mag_count; + unsigned acount_prev = _accel_count; + unsigned bcount_prev = _baro_count; _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); + &raw.gyro_priority[0], &raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); + &raw.baro_priority[0], &raw.baro_errcount[0]); if (gcount_prev != _gyro_count || mcount_prev != _mag_count || @@ -2130,6 +2171,7 @@ Sensors::task_main() /* if the secondary failed as well, go to the tertiary */ if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { fds[0].fd = _gyro_sub[2]; + } else { fds[0].fd = _gyro_sub[1]; } @@ -2150,16 +2192,16 @@ Sensors::task_main() */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); + &raw.gyro_priority[0], &raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); + &raw.baro_priority[0], &raw.baro_errcount[0]); _last_config_update = hrt_absolute_time(); @@ -2190,11 +2232,11 @@ Sensors::start() /* start the task */ _sensors_task = px4_task_spawn_cmd("sensors", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (px4_main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); /* wait until the task is up and running or has failed */ while (_sensors_task > 0 && _task_should_exit) {