From a4724acf82e48d914bfe6397340454505e14decf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 17:26:42 +0200 Subject: autostart 10016_3dr_iris: yaw PID parameters updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index f11aa704e..084dff140 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -18,9 +18,9 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAW_P 2.5 + param set MC_YAWRATE_P 0.25 + param set MC_YAWRATE_I 0.25 param set MC_YAWRATE_D 0.0 param set BAT_V_SCALING 0.00989 -- cgit v1.2.3 From e72a7a7dd78cde4a93d42a53328553e71560fc27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:14 +0200 Subject: fw att: robustify main loop against non finite numbers and limit error output rate --- src/modules/fw_att_control/fw_att_control_main.cpp | 42 ++++++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1f3e9f098..cafce4417 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main() while (!_task_should_exit) { + static int loop_counter = 0; + /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); @@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - warnx("Did not get a valid R\n"); + if (loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } } /* Run attitude controllers */ @@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + _roll_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("roll_u %.4f", roll_u); + } } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); + _pitch_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); + } } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", (double)yaw_u); + _yaw_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double)yaw_u); + } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", (double)throttle_sp); + if (loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double)throttle_sp); + } } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + if (loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + } } /* @@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main() } + loop_counter++; perf_end(_loop_perf); } -- cgit v1.2.3 From 7c165689ce0d33a65554f62a3438c2da30404642 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:37 +0200 Subject: fw att: pitch ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 9584924cc..a3f5199b1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() : float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { - + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + warnx("not controlling pitch"); + return _rate_setpoint; + } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -123,6 +127,13 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); -- cgit v1.2.3 From 52596be98c77497d67615435fdbd8e403cae618f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:50 +0200 Subject: fw att: roll ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 2e86c72dc..6ad00049d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() : float ECL_RollController::control_attitude(float roll_setpoint, float roll) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll_setpoint) && isfinite(roll))) { + return _rate_setpoint; + } /* Calculate error */ float roll_error = roll_setpoint - roll; @@ -86,6 +90,13 @@ float ECL_RollController::control_bodyrate(float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && + isfinite(airspeed_min) && isfinite(airspeed_max) && + isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); @@ -122,8 +133,8 @@ float ECL_RollController::control_bodyrate(float pitch, float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ + * anti-windup: do not allow integrator to increase if actuator is at limit + */ if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); -- cgit v1.2.3 From 9a35bac2adc1e803dea7cdb6a2db277f111724e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:16:10 +0200 Subject: fw att: yaw ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 255776765..d43e0314e 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,6 +66,12 @@ float ECL_YawController::control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && + isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && + isfinite(pitch_rate_setpoint))) { + return _rate_setpoint; + } // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; @@ -103,6 +109,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float pitch_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); -- cgit v1.2.3 From b9b81beb17eb449921f11f46bc419056dce03852 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 May 2014 21:49:00 +0200 Subject: fw att: add performance counter --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 4 ++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 4 ++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 8 ++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 ++++ src/modules/fw_att_control/fw_att_control_main.cpp | 19 +++++++++++++++++-- 7 files changed, 53 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a3f5199b1..0a909d02f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -63,12 +63,19 @@ ECL_PitchController::ECL_PitchController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); +} + +ECL_PitchController::~ECL_PitchController() +{ + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -131,6 +138,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 30a82a86a..39b9f9d03 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); + ~ECL_PitchController(); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); @@ -126,6 +129,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 6ad00049d..82903ef5a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -61,12 +61,19 @@ ECL_RollController::ECL_RollController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); +} + +ECL_RollController::~ECL_RollController() +{ + perf_free(_nonfinite_input_perf); } float ECL_RollController::control_attitude(float roll_setpoint, float roll) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(roll_setpoint) && isfinite(roll))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -94,6 +101,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 92c64b95f..0799dbe03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,12 +51,15 @@ #include #include +#include class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); + ~ECL_RollController(); + float control_attitude(float roll_setpoint, float roll); float control_bodyrate(float pitch, @@ -117,6 +120,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d43e0314e..e53ffc644 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -60,6 +60,12 @@ ECL_YawController::ECL_YawController() : _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f) { + perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); +} + +ECL_YawController::~ECL_YawController() +{ + perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -70,6 +76,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { + perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -113,6 +120,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { + perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 03f3202d0..a360c14b8 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,12 +50,15 @@ #include #include +#include class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); + ~ECL_YawController(); + float control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint); @@ -118,6 +121,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; + perf_counter_t _nonfinite_input_perf; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 59854e734..178b590ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -134,6 +134,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ @@ -310,6 +312,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), + _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false) { @@ -387,6 +391,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() } while (_control_task != -1); } + perf_free(_loop_perf); + perf_free(_nonfinite_input_perf); + perf_free(_nonfinite_output_perf); + att_control::g_control = nullptr; } @@ -674,10 +682,12 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (!isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; - + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } } else { airspeed = _airspeed.true_airspeed_m_s; } @@ -778,6 +788,8 @@ FixedwingAttitudeControl::task_main() _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } @@ -790,6 +802,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," @@ -813,6 +826,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); if (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -826,6 +840,7 @@ FixedwingAttitudeControl::task_main() } } } else { + perf_count(_nonfinite_input_perf); if (loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } -- cgit v1.2.3 From 04a25fcf1402feab10ede27fe0fe710b85b95941 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 May 2014 13:58:12 +0200 Subject: makefiles: check for correct arm-none-eabi-gcc version --- makefiles/toolchain_gnu-arm-eabi.mk | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b519e0e7a..d4689c63e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -48,6 +48,16 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump +# Check if the right version of the toolchain is available +# +CROSSDEV_VER_SUPPORTED = 4.7.4 +CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) + +ifneq ($(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED)) +endif + + # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup MAXOPTIMIZATION ?= -O3 @@ -76,7 +86,7 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = # Pick the right set of flags for the architecture. # @@ -265,7 +275,7 @@ define SYM_TO_BIN $(Q) $(OBJCOPY) -O binary $1 $2 endef -# Take the raw binary $1 and make it into an object file $2. +# Take the raw binary $1 and make it into an object file $2. # The symbol $3 points to the beginning of the file, and $3_len # gives its length. # -- cgit v1.2.3 From 577dc879d3e6e287b03c743e7205375f13bb3081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 12:27:13 +0200 Subject: Removed obsolete flow control example --- .../flow_position_control_main.c | 613 --------------------- .../flow_position_control_params.c | 124 ----- .../flow_position_control_params.h | 100 ---- src/examples/flow_position_control/module.mk | 41 -- 4 files changed, 878 deletions(-) delete mode 100644 src/examples/flow_position_control/flow_position_control_main.c delete mode 100644 src/examples/flow_position_control/flow_position_control_params.c delete mode 100644 src/examples/flow_position_control/flow_position_control_params.h delete mode 100644 src/examples/flow_position_control/module.mk diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c deleted file mode 100644 index 391e40ac1..000000000 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ /dev/null @@ -1,613 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file flow_position_control.c - * - * Optical flow position controller - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flow_position_control_params.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int flow_position_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int flow_position_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn_cmd(). - */ -int flow_position_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { - printf("flow position control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("flow_position_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 4096, - flow_position_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) - { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) - { - if (thread_running) - printf("\tflow position control app is running\n"); - else - printf("\tflow position control app not started\n"); - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int -flow_position_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - thread_running = true; - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[fpc] started"); - - uint32_t counter = 0; - const float time_scale = powf(10.0f,-6.0f); - - /* structures */ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct filtered_bottom_flow_s filtered_flow; - memset(&filtered_flow, 0, sizeof(filtered_flow)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_bodyframe_speed_setpoint_s speed_sp; - memset(&speed_sp, 0, sizeof(speed_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); - int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - - orb_advert_t speed_sp_pub; - bool speed_setpoint_adverted = false; - - /* parameters init*/ - struct flow_position_control_params params; - struct flow_position_control_param_handles param_handles; - parameters_init(¶m_handles); - parameters_update(¶m_handles, ¶ms); - - /* init flow sum setpoint */ - float flow_sp_sumx = 0.0f; - float flow_sp_sumy = 0.0f; - - /* init yaw setpoint */ - float yaw_sp = 0.0f; - - /* init height setpoint */ - float height_sp = params.height_min; - - /* height controller states */ - bool start_phase = true; - bool landing_initialized = false; - float landing_thrust_start = 0.0f; - - /* states */ - float integrated_h_error = 0.0f; - float last_local_pos_z = 0.0f; - bool update_flow_sp_sumx = false; - bool update_flow_sp_sumy = false; - uint64_t last_time = 0.0f; - float dt = 0.0f; // s - - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); - - static bool sensors_ready = false; - static bool status_changed = false; - - while (!thread_should_exit) - { - /* wait for first attitude msg to be sure all data are available */ - if (sensors_ready) - { - /* polling */ - struct pollfd fds[2] = { - { .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator - { .fd = parameter_update_sub, .events = POLLIN } - - }; - - /* wait for a position update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ -// printf("[flow position control] no filtered flow updates\n"); - } - else - { - /* parameter update available? */ - if (fds[1].revents & POLLIN) - { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - parameters_update(¶m_handles, ¶ms); - mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); - } - - /* only run controller if position/speed changed */ - if (fds[0].revents & POLLIN) - { - perf_begin(mc_loop_perf); - - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - /* get a local copy of filtered bottom flow */ - orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); - /* get a local copy of control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - if (control_mode.flag_control_velocity_enabled) - { - float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 - float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 - float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 - - if(status_changed == false) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); - - status_changed = true; - - /* calc dt */ - if(last_time == 0) - { - last_time = hrt_absolute_time(); - continue; - } - dt = ((float) (hrt_absolute_time() - last_time)) * time_scale; - last_time = hrt_absolute_time(); - - /* update flow sum setpoint */ - if (update_flow_sp_sumx) - { - flow_sp_sumx = filtered_flow.sumx; - update_flow_sp_sumx = false; - } - if (update_flow_sp_sumy) - { - flow_sp_sumy = filtered_flow.sumy; - update_flow_sp_sumy = false; - } - - /* calc new bodyframe speed setpoints */ - float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; - float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d; - float speed_limit_height_factor = height_sp; // the settings are for 1 meter - - /* overwrite with rc input if there is any */ - if(isfinite(manual_pitch) && isfinite(manual_roll)) - { - if(fabsf(manual_pitch) > params.manual_threshold) - { - speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor; - update_flow_sp_sumx = true; - } - - if(fabsf(manual_roll) > params.manual_threshold) - { - speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor; - update_flow_sp_sumy = true; - } - } - - /* limit speed setpoints */ - if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) && - (speed_body_x >= -params.limit_speed_x * speed_limit_height_factor)) - { - speed_sp.vx = speed_body_x; - } - else - { - if(speed_body_x > params.limit_speed_x * speed_limit_height_factor) - speed_sp.vx = params.limit_speed_x * speed_limit_height_factor; - if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor) - speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor; - } - - if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) && - (speed_body_y >= -params.limit_speed_y * speed_limit_height_factor)) - { - speed_sp.vy = speed_body_y; - } - else - { - if(speed_body_y > params.limit_speed_y * speed_limit_height_factor) - speed_sp.vy = params.limit_speed_y * speed_limit_height_factor; - if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor) - speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor; - } - - /* manual yaw change */ - if(isfinite(manual_yaw) && isfinite(manual.throttle)) - { - if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f) - { - yaw_sp += manual_yaw * params.limit_yaw_step; - - /* modulo for rotation -pi +pi */ - if(yaw_sp < -M_PI_F) - yaw_sp = yaw_sp + M_TWOPI_F; - else if(yaw_sp > M_PI_F) - yaw_sp = yaw_sp - M_TWOPI_F; - } - } - - /* forward yaw setpoint */ - speed_sp.yaw_sp = yaw_sp; - - - /* manual height control - * 0-20%: thrust linear down - * 20%-40%: down - * 40%-60%: stabilize altitude - * 60-100%: up - */ - float thrust_control = 0.0f; - - if (isfinite(manual.throttle)) - { - if (start_phase) - { - /* control start thrust with stick input */ - if (manual.throttle < 0.4f) - { - /* first 40% for up to feedforward */ - thrust_control = manual.throttle / 0.4f * params.thrust_feedforward; - } - else - { - /* second 60% for up to feedforward + 10% */ - thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward; - } - - /* exit start phase if setpoint is reached */ - if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower) - { - start_phase = false; - /* switch to stabilize */ - thrust_control = params.thrust_feedforward; - } - } - else - { - if (manual.throttle < 0.2f) - { - /* landing initialization */ - if (!landing_initialized) - { - /* consider last thrust control to avoid steps */ - landing_thrust_start = speed_sp.thrust_sp; - landing_initialized = true; - } - - /* set current height as setpoint to avoid steps */ - if (-local_pos.z > params.height_min) - height_sp = -local_pos.z; - else - height_sp = params.height_min; - - /* lower 20% stick range controls thrust down */ - thrust_control = manual.throttle / 0.2f * landing_thrust_start; - - /* assume ground position here */ - if (thrust_control < 0.1f) - { - /* reset integral if on ground */ - integrated_h_error = 0.0f; - /* switch to start phase */ - start_phase = true; - /* reset height setpoint */ - height_sp = params.height_min; - } - } - else - { - /* stabilized mode */ - landing_initialized = false; - - /* calc new thrust with PID */ - float height_error = (local_pos.z - (-height_sp)); - - /* update height setpoint if needed*/ - if (manual.throttle < 0.4f) - { - /* down */ - if (height_sp > params.height_min + params.height_rate && - fabsf(height_error) < params.limit_height_error) - height_sp -= params.height_rate * dt; - } - - if (manual.throttle > 0.6f) - { - /* up */ - if (height_sp < params.height_max && - fabsf(height_error) < params.limit_height_error) - height_sp += params.height_rate * dt; - } - - /* instead of speed limitation, limit height error (downwards) */ - if(height_error > params.limit_height_error) - height_error = params.limit_height_error; - else if(height_error < -params.limit_height_error) - height_error = -params.limit_height_error; - - integrated_h_error = integrated_h_error + height_error; - float integrated_thrust_addition = integrated_h_error * params.height_i; - - if(integrated_thrust_addition > params.limit_thrust_int) - integrated_thrust_addition = params.limit_thrust_int; - if(integrated_thrust_addition < -params.limit_thrust_int) - integrated_thrust_addition = -params.limit_thrust_int; - - float height_speed = last_local_pos_z - local_pos.z; - float thrust_diff = height_error * params.height_p - height_speed * params.height_d; - - thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition; - - /* add attitude component - * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM - */ -// // TODO problem with attitude -// if (att.R_valid && att.R[2][2] > 0) -// thrust_control = thrust_control / att.R[2][2]; - - /* set thrust lower limit */ - if(thrust_control < params.limit_thrust_lower) - thrust_control = params.limit_thrust_lower; - } - } - - /* set thrust upper limit */ - if(thrust_control > params.limit_thrust_upper) - thrust_control = params.limit_thrust_upper; - } - /* store actual height for speed estimation */ - last_local_pos_z = local_pos.z; - - speed_sp.thrust_sp = thrust_control; //manual.throttle; - speed_sp.timestamp = hrt_absolute_time(); - - /* publish new speed setpoint */ - if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp)) - { - - if(speed_setpoint_adverted) - { - orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp); - } - else - { - speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp); - speed_setpoint_adverted = true; - } - } - else - { - warnx("NaN in flow position controller!"); - } - } - else - { - /* in manual or stabilized state just reset speed and flow sum setpoint */ - //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); - if(status_changed == true) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); - - status_changed = false; - speed_sp.vx = 0.0f; - speed_sp.vy = 0.0f; - flow_sp_sumx = filtered_flow.sumx; - flow_sp_sumy = filtered_flow.sumy; - if(isfinite(att.yaw)) - { - yaw_sp = att.yaw; - speed_sp.yaw_sp = att.yaw; - } - if(isfinite(manual.throttle)) - speed_sp.thrust_sp = manual.throttle; - } - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - perf_end(mc_loop_perf); - } - } - - counter++; - } - else - { - /* sensors not ready waiting for first attitude msg */ - - /* polling */ - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - /* wait for a flow msg, check for exit condition every 5 s */ - int ret = poll(fds, 1, 5000); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ - mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN) - { - sensors_ready = true; - mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); - } - } - } - } - - mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); - - thread_running = false; - - close(parameter_update_sub); - close(vehicle_attitude_sub); - close(vehicle_local_position_sub); - close(armed_sub); - close(control_mode_sub); - close(manual_control_setpoint_sub); - close(speed_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - return 0; -} - diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c deleted file mode 100644 index eb1473647..000000000 --- a/src/examples/flow_position_control/flow_position_control_params.c +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_control_params.c - */ - -#include "flow_position_control_params.h" - -/* controller parameters */ - -// Position control P gain -PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); -// Position control D / damping gain -PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); -// Altitude control P gain -PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); -// Altitude control I (integrator) gain -PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); -// Altitude control D gain -PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); -// Altitude control rate limiter -PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); -// Altitude control minimum altitude -PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); -// Altitude control maximum altitude (higher than 1.5m is untested) -PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); -// Altitude control feed forward throttle - adjust to the -// throttle position (0..1) where the copter hovers in manual flight -PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight -PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); -PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); -PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f); -PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f); -PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f); -PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f); -PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f); -PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f); - - -int parameters_init(struct flow_position_control_param_handles *h) -{ - /* PID parameters */ - h->pos_p = param_find("FPC_POS_P"); - h->pos_d = param_find("FPC_POS_D"); - h->height_p = param_find("FPC_H_P"); - h->height_i = param_find("FPC_H_I"); - h->height_d = param_find("FPC_H_D"); - h->height_rate = param_find("FPC_H_RATE"); - h->height_min = param_find("FPC_H_MIN"); - h->height_max = param_find("FPC_H_MAX"); - h->thrust_feedforward = param_find("FPC_T_FFWD"); - h->limit_speed_x = param_find("FPC_L_S_X"); - h->limit_speed_y = param_find("FPC_L_S_Y"); - h->limit_height_error = param_find("FPC_L_H_ERR"); - h->limit_thrust_int = param_find("FPC_L_TH_I"); - h->limit_thrust_upper = param_find("FPC_L_TH_U"); - h->limit_thrust_lower = param_find("FPC_L_TH_L"); - h->limit_yaw_step = param_find("FPC_L_YAW_STEP"); - h->manual_threshold = param_find("FPC_MAN_THR"); - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p) -{ - param_get(h->pos_p, &(p->pos_p)); - param_get(h->pos_d, &(p->pos_d)); - param_get(h->height_p, &(p->height_p)); - param_get(h->height_i, &(p->height_i)); - param_get(h->height_d, &(p->height_d)); - param_get(h->height_rate, &(p->height_rate)); - param_get(h->height_min, &(p->height_min)); - param_get(h->height_max, &(p->height_max)); - param_get(h->thrust_feedforward, &(p->thrust_feedforward)); - param_get(h->limit_speed_x, &(p->limit_speed_x)); - param_get(h->limit_speed_y, &(p->limit_speed_y)); - param_get(h->limit_height_error, &(p->limit_height_error)); - param_get(h->limit_thrust_int, &(p->limit_thrust_int)); - param_get(h->limit_thrust_upper, &(p->limit_thrust_upper)); - param_get(h->limit_thrust_lower, &(p->limit_thrust_lower)); - param_get(h->limit_yaw_step, &(p->limit_yaw_step)); - param_get(h->manual_threshold, &(p->manual_threshold)); - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h deleted file mode 100644 index d0c8fc722..000000000 --- a/src/examples/flow_position_control/flow_position_control_params.h +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_position_control_params.h - * - * Parameters for position controller - */ - -#include - -struct flow_position_control_params { - float pos_p; - float pos_d; - float height_p; - float height_i; - float height_d; - float height_rate; - float height_min; - float height_max; - float thrust_feedforward; - float limit_speed_x; - float limit_speed_y; - float limit_height_error; - float limit_thrust_int; - float limit_thrust_upper; - float limit_thrust_lower; - float limit_yaw_step; - float manual_threshold; - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct flow_position_control_param_handles { - param_t pos_p; - param_t pos_d; - param_t height_p; - param_t height_i; - param_t height_d; - param_t height_rate; - param_t height_min; - param_t height_max; - param_t thrust_feedforward; - param_t limit_speed_x; - param_t limit_speed_y; - param_t limit_height_error; - param_t limit_thrust_int; - param_t limit_thrust_upper; - param_t limit_thrust_lower; - param_t limit_yaw_step; - param_t manual_threshold; - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct flow_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p); diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk deleted file mode 100644 index b10dc490a..000000000 --- a/src/examples/flow_position_control/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build multirotor position control -# - -MODULE_COMMAND = flow_position_control - -SRCS = flow_position_control_main.c \ - flow_position_control_params.c -- cgit v1.2.3 From e710e2a2d1875e4bec2d42e7d4054b11a1195260 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 12:38:29 +0200 Subject: Allow any GCC 4.7 subversion --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d4689c63e..808b635bb 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 +CROSSDEV_VER_SUPPORTED = 4.7 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifneq ($(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED)) +ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) endif -- cgit v1.2.3 From 7255aafcb5102ec448543e3ead80a29d182dfee1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:00:22 +0200 Subject: Better docs in GPS topic --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 794c3f8bc..d79e9685f 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -85,7 +85,7 @@ struct vehicle_gps_position_s { uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; -- cgit v1.2.3 From 19989b4314c4918ce8f6e1731ba389757f95ae62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:00:42 +0200 Subject: Better docs on log messages --- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 85ef4da39..3ccffd5b2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -321,25 +321,25 @@ struct log_VICN_s { /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ #define LOG_GS0A_MSG 26 struct log_GS0A_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ #define LOG_GS0B_MSG 27 struct log_GS0B_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ #define LOG_GS1A_MSG 28 struct log_GS1A_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ #define LOG_GS1B_MSG 29 struct log_GS1B_s { - uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ -- cgit v1.2.3 From e71c386547badfb4bf2cde0e58f345ea569fc1d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:01:20 +0200 Subject: Always log both GPS SNR sets. Assign array IDs by PRN to get a per-satellite unique mapping --- src/modules/sdlog2/sdlog2.c | 29 +++++++++++++---------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ff68b5ad6..873f04828 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1068,27 +1068,24 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ - unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - - for (unsigned i = 0; i < max_sats_a; i++) { - log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + for (unsigned i = 0; i < gps_msg_max_snr; i++) { + if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0A.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + } } LOGBUFFER_WRITE_AND_COUNT(GS0A); - /* do we need a 2nd set? */ - if (gps_msg_max_snr > log_max_snr) { - log_msg.msg_type = LOG_GS0B_MSG; - memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); - /* fill set B - deduct the count we already have taken care of */ - gps_msg_max_snr -= log_max_snr; - unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr; - - for (unsigned i = 0; i < max_sats_b; i++) { - /* count from zero, but obey offset of log_max_snr consumed units */ - log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i]; + log_msg.msg_type = LOG_GS0B_MSG; + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ + for (unsigned i = 0; i < gps_msg_max_snr; i++) { + if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0B.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; } - LOGBUFFER_WRITE_AND_COUNT(GS0B); } + LOGBUFFER_WRITE_AND_COUNT(GS0B); } } -- cgit v1.2.3 From b3d6dcb2e5a1f66c42d575f13cbc5a7eef16db27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:14:02 +0200 Subject: Pre-emptively increase the log buffer - after the last cleanup we got again plenty of RAM --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 517e073af..25f31a7e0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,6 +11,6 @@ then sdlog2 start -r 50 -a -b 4 -t else echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -a -b 16 -t + sdlog2 start -r 200 -a -b 22 -t fi fi -- cgit v1.2.3 From 98d5ed5e7357659951a067eb8e396be9c2c59a58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:27 +0200 Subject: sdlog2: Fix GPS sat offset math --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 873f04828..b6b039d26 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1069,9 +1069,13 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ for (unsigned i = 0; i < gps_msg_max_snr; i++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + int satindex = buf_gps_pos.satellite_prn[i] - 1; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); @@ -1080,9 +1084,14 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); /* fill set B */ for (unsigned i = 0; i < gps_msg_max_snr; i++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + /* get second bank of satellites, thus deduct bank size from index */ + int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); -- cgit v1.2.3 From 5f752cdb7cf6c74e34b3ac79b9220c70172791e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:48 +0200 Subject: commander: Better audio indications on arming reject reasons --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c26200048..0463cf8f8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[]) sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); + print_reject_arm("#audio: NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f1a353d5b..87309b238 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Allow if HIL_STATE_ON if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); } valid_transition = false; @@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); valid_transition = false; break; -- cgit v1.2.3 From 951bd70ffcb4a1d420347534e5b999021a412be0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:57:25 +0200 Subject: ubx driver: Fix / shorten printfs --- src/drivers/gps/ubx.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 8a2afecb7..f4a065c19 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate) send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: configuration failed: RATE"); + warnx("CFG FAIL: RATE"); return 1; } @@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate) send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: configuration failed: NAV5"); + warnx("CFG FAIL: NAV5"); return 1; } @@ -194,35 +194,35 @@ UBX::configure(unsigned &baudrate) configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV POSLLH"); + warnx("MSG CFG FAIL: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); + warnx("MSG CFG FAIL: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SOL"); + warnx("MSG CFG FAIL: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV VELNED"); + warnx("MSG CFG FAIL: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SVINFO"); + warnx("MSG CFG FAIL: NAV SVINFO"); return 1; } @@ -274,7 +274,7 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ - warnx("ubx: poll error"); + warnx("poll error"); return -1; } else if (ret == 0) { @@ -310,7 +310,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { - warnx("ubx: timeout - no useful messages"); + warnx("timeout - no useful messages"); return -1; } } @@ -383,7 +383,7 @@ UBX::parse_char(uint8_t b) return 1; // message received successfully } else { - warnx("ubx: checksum wrong"); + warnx("checksum wrong"); decode_init(); return -1; } @@ -392,7 +392,7 @@ UBX::parse_char(uint8_t b) _rx_count++; } else { - warnx("ubx: buffer full"); + warnx("buffer full"); decode_init(); return -1; } -- cgit v1.2.3 From a359a1b9408c7e4827c7dcbf7c27bf76ea071662 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:24 +0200 Subject: Fixed audio output to provide decent user feedback --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0463cf8f8..5c0628a16 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); + /* this needs additional hints to the user - so let other messages pass and be spoken */ + mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From 4f26a8b7c83636f294b500558d8f7c72ed0cc310 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:51 +0200 Subject: gps topic: Added snr and jamming fields --- src/modules/uORB/topics/vehicle_gps_position.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index d79e9685f..5924a324d 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -68,6 +68,9 @@ struct vehicle_gps_position_s { float eph_m; /**< GPS HDOP horizontal dilution of position in m */ float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + unsigned noise_per_ms; /**< */ + unsigned jamming_indicator; /**< */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ float vel_m_s; /**< GPS ground speed (m/s) */ float vel_n_m_s; /**< GPS ground speed in m/s */ -- cgit v1.2.3 From 7bf1f82f615b05bb37b8c4c45891bbde96745a4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:03:13 +0200 Subject: sdlog2: Logging GPS snr and jamming fields --- src/modules/sdlog2/sdlog2.c | 23 ++++++++++++++++++----- src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b6b039d26..197b65231 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -113,8 +113,8 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ -static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ +static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -641,7 +641,7 @@ int write_formats(int fd) int written = 0; /* fill message format packet for each format and write it */ - for (int i = 0; i < log_formats_num; i++) { + for (unsigned i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; written += write(fd, &log_msg_format, sizeof(log_msg_format)); } @@ -1046,6 +1046,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { + + float snr_mean = 0.0f; + + for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { + snr_mean += buf_gps_pos.satellite_snr[i]; + } + + snr_mean /= buf_gps_pos.satellites_visible; + log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1058,6 +1067,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); if (_extended_logging) { @@ -1073,7 +1086,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int satindex = buf_gps_pos.satellite_prn[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < log_max_snr)) { + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } @@ -1089,7 +1102,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ - if ((satindex >= 0) && (satindex < log_max_snr)) { + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3ccffd5b2..f4d88f079 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -139,6 +139,10 @@ struct log_GPS_s { float vel_e; float vel_d; float cog; + uint8_t sats; + uint16_t snr_mean; + uint16_t noise_per_ms; + uint16_t jamming_indicator; }; /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ @@ -375,7 +379,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), -- cgit v1.2.3 From c4d79b84b4eb3b4d255706f29972cd3f31689749 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:08:33 +0200 Subject: bugfixes in UBX driver, emit new SNR, Jamming and noise count indices --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/gps/gps_helper.cpp | 4 ++-- src/drivers/gps/gps_helper.h | 8 ++++++-- src/drivers/gps/ubx.cpp | 25 +++++++++++++++++++++++++ src/drivers/gps/ubx.h | 28 ++++++++++++++++++++++++++-- 5 files changed, 62 insertions(+), 9 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6195cd6ea..5342ccf78 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -448,10 +448,10 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, - _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2360ff39b..3b92f1bf4 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate() return _rate_vel; } -float +void GPS_Helper::reset_update_rates() { _rate_count_vel = 0; @@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates() _interval_rate_start = hrt_absolute_time(); } -float +void GPS_Helper::store_update_rates() { _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index cfb9e0d43..d14a95afe 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -46,13 +46,17 @@ class GPS_Helper { public: + + GPS_Helper() {}; + virtual ~GPS_Helper() {}; + virtual int configure(unsigned &baud) = 0; virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); float get_position_update_rate(); float get_velocity_update_rate(); - float reset_update_rates(); - float store_update_rates(); + void reset_update_rates(); + void store_update_rates(); protected: uint8_t _rate_count_lat_lon; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f4a065c19..19cf5beec 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -226,6 +226,13 @@ UBX::configure(unsigned &baudrate) return 1; } + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } + _configured = true; return 0; } @@ -566,6 +573,24 @@ UBX::handle_message() break; } + case UBX_CLASS_MON: { + switch (_message_id) { + case UBX_MESSAGE_MON_HW: { + + struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + + _gps_position->noise_per_ms = p->noisePerMS; + _gps_position->jamming_indicator = p->jamInd; + + ret = 1; + break; + } + + default: + break; + } + } + default: break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 79a904f4a..5cf47b60b 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -56,6 +56,7 @@ //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_MON 0x0A /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 @@ -72,6 +73,8 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_MON_HW 0x09 + #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -210,6 +213,27 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; +struct gps_bin_mon_hw_packet { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t __reserved1; + uint32_t usedMask; + uint8_t VP[25]; + uint8_t jamInd; + uint16_t __reserved3; + uint32_t pinIrq; + uint32_t pulLH; + uint32_t pullL; +}; + + //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ // int16_t week; /**< Measurement GPS week number */ @@ -319,7 +343,7 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer +#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer class UBX : public GPS_Helper { @@ -383,7 +407,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_last; + hrt_abstime _disable_cmd_last; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From fb41f752fdc486cca83f74cb6e4d5ec416bdbd89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 16:29:35 +0200 Subject: Fixed Easystar mixer (reported by Mark Whitehorn) --- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index db0e40fc2..3ab2ac3d1 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -5,4 +5,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER easystar.mix +set MIXER easystar -- cgit v1.2.3