Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

WIP: Fix FMU MC failsafe on RC loss #5388

Closed
wants to merge 5 commits into from
Closed

WIP: Fix FMU MC failsafe on RC loss #5388

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0b5ffd6
always enter failsafe if in manual mode and RC lost
kd0aij Aug 26, 2016
ed2565c
fix logic error which prevented mixing when RC lost
kd0aij Aug 26, 2016
8fdb5f9
add block for handling flight termination nav state
kd0aij Aug 26, 2016
757dea5
run astyle
kd0aij Nov 11, 2016
a1d835b
force lockdown on RC loss if flight termination enabled
kd0aij Nov 11, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
133 changes: 67 additions & 66 deletions src/drivers/px4fmu/fmu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1045,96 +1045,97 @@ PX4FMU::cycle()
}
}

/* can we mix? */
if (_mixers != nullptr) {
}

size_t num_outputs;
_cycle_timestamp = hrt_absolute_time();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did _cycle_timestamp not get updated in the RC lost case? If so that's a good catch!

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah, I didn't realize the cause was the timestamp not being updated, maybe the mixing code should go back in the if block

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah what was your change then? I was trying to find out what you fixed there.

Copy link
Contributor Author

@kd0aij kd0aij Aug 26, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My change forces the output mixer to run even when the actuator topic is not being published.

The problem "Also, px4fmu quit running the output mixer when RC was lost." is due to the fact that the mixer was being run only on updates to the actuator topic(s). When commander asserts armed.force_failsafe, it appears that actuator values are no longer being published. How is this supposed to work?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The solution to that is to either also watch the armed topic or enforce a minimum rate. Obviously the output is the combination of actuator and armed and so both need to be watched.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Then I guess my FMU bugfix is correct, since it guarantees that actuator_outputs are published on every cycle.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this is correct like you changed it.


switch (_mode) {
case MODE_1PWM:
num_outputs = 1;
break;
/* can we mix? */
if (_mixers != nullptr) {

case MODE_2PWM:
case MODE_2PWM2CAP:
num_outputs = 2;
break;
size_t num_outputs;

case MODE_3PWM:
case MODE_3PWM1CAP:
num_outputs = 3;
break;
switch (_mode) {
case MODE_1PWM:
num_outputs = 1;
break;

case MODE_4PWM:
num_outputs = 4;
break;
case MODE_2PWM:
case MODE_2PWM2CAP:
num_outputs = 2;
break;

case MODE_6PWM:
num_outputs = 6;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
num_outputs = 3;
break;

case MODE_8PWM:
num_outputs = 8;
break;
case MODE_4PWM:
num_outputs = 4;
break;

default:
num_outputs = 0;
break;
}
case MODE_6PWM:
num_outputs = 6;
break;

hrt_abstime now = hrt_absolute_time();
float dt = (now - _time_last_mix) / 1e6f;
_time_last_mix = now;
case MODE_8PWM:
num_outputs = 8;
break;

if (dt < 0.0001f) {
dt = 0.0001f;
default:
num_outputs = 0;
break;
}

} else if (dt > 0.02f) {
dt = 0.02f;
}
hrt_abstime now = hrt_absolute_time();
float dt = (now - _time_last_mix) / 1e6f;
_time_last_mix = now;

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 (dt < 0.0001f) {
dt = 0.0001f;

/* do mixing */
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);
} else if (dt > 0.02f) {
dt = 0.02f;
}

/* 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;
}
}
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);
}

uint16_t pwm_limited[_max_actuators];
/* do mixing */
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);

/* 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);
/* 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];

/* 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];
}
}
/* 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);

/* 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]);
}
}

_cycle_timestamp = hrt_absolute_time();
publish_pwm_outputs(pwm_limited, num_outputs);
}

#ifdef GPIO_BTN_SAFETY

Expand Down
8 changes: 5 additions & 3 deletions src/modules/commander/commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2833,19 +2833,21 @@ 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*/) ||
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think we want termination if we lose RC and still have GPS. The question is probably what the meaning of gps_failure is or should be.

It could mean the following:

  1. GPS not available, or GPS available once and now lost.
    or:
  2. GPS available once and now lost.

Copy link
Contributor Author

@kd0aij kd0aij Aug 26, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you're in ACRO mode, flying manually and lose RC, currently you get "fly away"
But I agree, the problem must be that gps_failure is not being asserted when it should be.
What kind of testing has been done on this functionality, and is there any documentation available?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I usually try to test this in SITL. I simulate GPS lost using gpssim stop. Documentation, nope :(

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's hard to guess how things like commander are intended to work with over 5000 lines of code in one module and no documentation of design intent...

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see, the termination code is messed up and I'm guessing no one is really using it. I think it needs proper fixing.

(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;

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");
}
}
}
Expand Down
8 changes: 8 additions & 0 deletions src/modules/mc_att_control/mc_att_control_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should all be handled in the commander. And the output (fmu, px4io) drivers need to handle the armed.lockdown condition.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is how fw_att_control works; I was just copying it without knowing whether the design was correct or fully tested :)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where? I don't find armed or lockdown in the fw_pos and fw_att controllers.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I meant handling nav_termination: instead of trying to assert lockdown, we should deploy a parachute or (somehow) kill motors here

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I still think this is wrong because the flag_control_termination_enabled should be set by the commander which already has knowledge of the termination circuit breaker.


return OK;
}
Expand Down Expand Up @@ -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 */
Expand Down