diff options
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 62 |
1 files changed, 32 insertions, 30 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 46eb7cc85..dc5bd95c6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -211,8 +211,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ @@ -470,8 +470,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), _mag_is_external(false), _battery_discharged(0), _battery_current_timestamp(0) @@ -639,41 +637,43 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + const char *paramerr = "FAIL PARM LOAD"; + /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("Failed getting roll chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("Failed getting pitch chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("Failed getting yaw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("Failed getting throttle chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("Failed getting return sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx("Failed getting assisted sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx("Failed getting mission sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + warnx(paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -745,12 +745,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("Failed updating voltage scaling param"); + warnx(paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("Failed updating current scaling param"); + warnx(paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -983,7 +983,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; @@ -1416,24 +1416,26 @@ Sensors::rc_poll() } } + /* mode switch input */ if (_rc.function[MODE] >= 0) { manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ if (_rc.function[MISSION] >= 0) { manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - /* land switch input */ + /* return switch input */ if (_rc.function[RETURN] >= 0) { manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } @@ -1648,17 +1650,17 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(0, "sensors task already running"); + errx(0, "already running"); sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) - errx(1, "sensors task alloc failed"); + errx(1, "alloc failed"); if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - err(1, "sensors task start failed"); + err(1, "start failed"); } exit(0); @@ -1666,7 +1668,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) - errx(1, "sensors task not running"); + errx(1, "not running"); delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1675,10 +1677,10 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - errx(0, "task is running"); + errx(0, "is running"); } else { - errx(1, "task is not running"); + errx(1, "not running"); } } |