diff options
Diffstat (limited to 'src/modules')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c | 2 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 20 |
3 files changed, 16 insertions, 14 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4154e3db4..3ff3d9922 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -40,6 +40,7 @@ */ #include "attitude_estimator_ekf_params.h" +#include <math.h> /* Extended Kalman Filter covariances */ @@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->yaw_off, &(p->yaw_off)); param_get(h->mag_decl, &(p->mag_decl)); + p->mag_decl *= M_PI / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8031a311e..ad8c1b4c0 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -837,21 +837,21 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx("%s%s", ekfname, str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx("%s%s", ekfname, str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { const char* str = "switching to dynamic state"; - warnx("%s%s", ekfname, str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } @@ -859,7 +859,7 @@ FixedwingEstimator::task_main() default: { const char* str = "unknown reset condition"; - warnx("%s%s", ekfname, str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); } } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a73599526..5315c5e38 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -627,31 +627,31 @@ Sensors::parameters_update() /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { @@ -659,7 +659,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -725,12 +725,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(paramerr); + warnx("%s", paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); |