diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-09 16:30:00 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-09 16:30:00 +0100 |
| commit | 0baca3ee316ca70fd18bf2cd5128685220e57634 (patch) | |
| tree | 2ff24279c864e44289ec2f5e356791b577b1bab2 /apps/multirotor_att_control | |
| parent | a866fb2f2c3791f0d357a30f1e2ce33f2f984af7 (diff) | |
| download | px4-firmware-0baca3ee316ca70fd18bf2cd5128685220e57634.tar.gz px4-firmware-0baca3ee316ca70fd18bf2cd5128685220e57634.tar.bz2 px4-firmware-0baca3ee316ca70fd18bf2cd5128685220e57634.zip | |
Better, platform independent failsafe handling
Diffstat (limited to 'apps/multirotor_att_control')
| -rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 20 |
1 files changed, 18 insertions, 2 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 3b05bfa25..262367370 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -184,7 +184,13 @@ mc_thread_main(int argc, char *argv[]) rates_sp.roll = offboard_sp.p1; rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; + if (state.offboard_control_signal_lost) { + /* give up in rate mode on signal loss */ + rates_sp.thrust = 0.0f; + } else { + /* no signal loss, normal throttle */ + rates_sp.thrust = offboard_sp.p4; + } // printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -192,7 +198,17 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; + + /* check if control signal is lost */ + if (state.offboard_control_signal_lost) { + /* set failsafe thrust */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.thrust = (failsafe_throttle < 0.6f) : failsafe_throttle ? 0.5f; + } else { + /* no signal loss, normal throttle */ + att_sp.thrust = offboard_sp.p4; + } + // printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ |