From 0b5ffd691241bb4c02b3039a5acd260e621a888a Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 25 Aug 2016 18:04:08 -0600 Subject: [PATCH 1/5] always enter failsafe if in manual mode and RC lost --- src/modules/commander/commander.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 27aca67c868f..dc658e267a2a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2833,19 +2833,20 @@ int commander_thread_main(int argc, char *argv[]) internal_state.main_state == commander_state_s::MAIN_STATE_STAB || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status_flags.gps_failure) || + ((status.rc_signal_lost /*&& status_flags.gps_failure*/) || (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of RC signal loss and GPS failure"); + PX4_WARN("Flight termination because of RC signal loss"); // and GPS failure"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination"); +// mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination"); + mavlink_log_critical(&mavlink_log_pub, "RC lost: flight termination"); } } } From ed2565cce56c3eec2272670a2fcd5bc584a74d32 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 25 Aug 2016 18:04:44 -0600 Subject: [PATCH 2/5] fix logic error which prevented mixing when RC lost --- src/drivers/px4fmu/fmu.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 53d35c231b3b..f54c034d6af0 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1045,6 +1045,10 @@ PX4FMU::cycle() } } + } + + _cycle_timestamp = hrt_absolute_time(); + /* can we mix? */ if (_mixers != nullptr) { @@ -1132,9 +1136,6 @@ PX4FMU::cycle() publish_pwm_outputs(pwm_limited, num_outputs); } - } - - _cycle_timestamp = hrt_absolute_time(); #ifdef GPIO_BTN_SAFETY From 8fdb5f92d910a7bc76c05e6c71ae092ec8bf89f7 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 25 Aug 2016 18:05:23 -0600 Subject: [PATCH 3/5] add block for handling flight termination nav state --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b152f369533c..3664de4adabb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -150,6 +150,7 @@ class MulticopterAttitudeControl orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + bool _termination_enabled; /**< true if flight termination circuit breaker not set */ struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ @@ -539,6 +540,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + _termination_enabled = !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); return OK; } @@ -889,6 +891,12 @@ MulticopterAttitudeControl::task_main() vehicle_status_poll(); vehicle_motor_limits_poll(); + /* handle flight termination */ + if (_termination_enabled && _v_control_mode.flag_control_termination_enabled) { + // do something + } + + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ From 757dea5718caf314ea7ba8f416ee83fb0d584313 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 11 Nov 2016 09:21:06 -0700 Subject: [PATCH 4/5] run astyle --- src/drivers/px4fmu/fmu.cpp | 128 ++++++++++++++++++------------------- 1 file changed, 64 insertions(+), 64 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f54c034d6af0..1e4ad47daa50 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1049,94 +1049,94 @@ PX4FMU::cycle() _cycle_timestamp = hrt_absolute_time(); - /* can we mix? */ - if (_mixers != nullptr) { + /* can we mix? */ + if (_mixers != nullptr) { - size_t num_outputs; + size_t num_outputs; - switch (_mode) { - case MODE_1PWM: - num_outputs = 1; - break; + switch (_mode) { + case MODE_1PWM: + num_outputs = 1; + break; - case MODE_2PWM: - case MODE_2PWM2CAP: - num_outputs = 2; - break; + case MODE_2PWM: + case MODE_2PWM2CAP: + num_outputs = 2; + break; - case MODE_3PWM: - case MODE_3PWM1CAP: - num_outputs = 3; - break; + case MODE_3PWM: + case MODE_3PWM1CAP: + num_outputs = 3; + break; - case MODE_4PWM: - num_outputs = 4; - break; + case MODE_4PWM: + num_outputs = 4; + break; - case MODE_6PWM: - num_outputs = 6; - break; + case MODE_6PWM: + num_outputs = 6; + break; - case MODE_8PWM: - num_outputs = 8; - break; + case MODE_8PWM: + num_outputs = 8; + break; - default: - num_outputs = 0; - break; - } + default: + num_outputs = 0; + break; + } - hrt_abstime now = hrt_absolute_time(); - float dt = (now - _time_last_mix) / 1e6f; - _time_last_mix = now; + hrt_abstime now = hrt_absolute_time(); + float dt = (now - _time_last_mix) / 1e6f; + _time_last_mix = now; - if (dt < 0.0001f) { - dt = 0.0001f; + if (dt < 0.0001f) { + dt = 0.0001f; - } else if (dt > 0.02f) { - dt = 0.02f; - } + } else if (dt > 0.02f) { + dt = 0.02f; + } - if (_mot_t_max > FLT_EPSILON) { - // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator ouputs are in the range [-1,1] - float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; - _mixers->set_max_delta_out_once(delta_out_max); - } + if (_mot_t_max > FLT_EPSILON) { + // maximum value the ouputs of the multirotor mixer are allowed to change in this cycle + // factor 2 is needed because actuator ouputs are in the range [-1,1] + float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; + _mixers->set_max_delta_out_once(delta_out_max); + } - /* do mixing */ - float outputs[_max_actuators]; - num_outputs = _mixers->mix(outputs, num_outputs, NULL); + /* do mixing */ + float outputs[_max_actuators]; + num_outputs = _mixers->mix(outputs, num_outputs, NULL); - /* disable unused ports by setting their output to NaN */ - for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { - if (i >= num_outputs) { - outputs[i] = NAN_VALUE; - } + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { + if (i >= num_outputs) { + outputs[i] = NAN_VALUE; } + } - uint16_t pwm_limited[_max_actuators]; - - /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + uint16_t pwm_limited[_max_actuators]; + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, + _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); - /* overwrite outputs in case of lockdown with disarmed PWM values */ - if (_armed.lockdown) { - for (size_t i = 0; i < num_outputs; i++) { - pwm_limited[i] = _disarmed_pwm[i]; - } - } - /* output to the servos */ + /* overwrite outputs in case of lockdown with disarmed PWM values */ + if (_armed.lockdown) { for (size_t i = 0; i < num_outputs; i++) { - pwm_output_set(i, pwm_limited[i]); + pwm_limited[i] = _disarmed_pwm[i]; } + } - publish_pwm_outputs(pwm_limited, num_outputs); + /* output to the servos */ + for (size_t i = 0; i < num_outputs; i++) { + pwm_output_set(i, pwm_limited[i]); } + publish_pwm_outputs(pwm_limited, num_outputs); + } + #ifdef GPIO_BTN_SAFETY if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { From a1d835bf27b92c868daea6c77629c0f432b47fa5 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 11 Nov 2016 12:02:40 -0700 Subject: [PATCH 5/5] force lockdown on RC loss if flight termination enabled --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc658e267a2a..f88ced2d58cc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2836,6 +2836,7 @@ int commander_thread_main(int argc, char *argv[]) ((status.rc_signal_lost /*&& status_flags.gps_failure*/) || (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; + armed.lockdown = true; status_changed = true; static bool flight_termination_printed = false;