// terrain failsafe action
void Sub::failsafe_terrain_on_event()
{
failsafe.terrain = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
// If rangefinder is enabled, we can recover from this failsafe
if (!rangefinder_state.enabled || !auto_terrain_recover_start()) {
failsafe_terrain_act();
}
}
// Attempt recovery from terrain failsafe
// If recovery is successful resume mission
// If recovery fails revert to failsafe action
void Sub::auto_terrain_recover_run()
{
float target_climb_rate = 0;
static uint32_t rangefinder_recovery_ms = 0;
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::RangeFinder_OutOfRangeLow:
target_climb_rate = wp_nav.get_speed_up();
rangefinder_recovery_ms = 0;
break;
case RangeFinder::RangeFinder_OutOfRangeHigh:
target_climb_rate = wp_nav.get_speed_down();
rangefinder_recovery_ms = 0;
break;
case RangeFinder::RangeFinder_Good: // exit on success (recovered rangefinder data)
target_climb_rate = 0; // Attempt to hold current depth
if (rangefinder_state.alt_healthy) {
// Start timer as soon as rangefinder is healthy
if (rangefinder_recovery_ms == 0) {
rangefinder_recovery_ms = AP_HAL::millis();
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // Reset alt hold targets
}
// 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) {
gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
failsafe_terrain_set_status(true); // Reset failsafe timers
failsafe.terrain = false; // Clear flag
auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
mission.resume(); // Resume mission
rangefinder_recovery_ms = 0; // Reset for subsequent recoveries
}
}
break;
// Not connected, or no data
default:
// Terrain failsafe recovery has failed, terrain data is not available
// and rangefinder is not connected, or has stopped responding
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
failsafe_terrain_act();
rangefinder_recovery_ms = 0;
return;
}
// exit on failure (timeout)
if (AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {
// Recovery has failed, revert to failsafe action
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
failsafe_terrain_act();
}
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
///////////////////////
// update xy targets //
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
/////////////////////
// update z target //
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, true);
pos_control.update_z_controller();
////////////////////////////
// update angular targets //
float target_roll = 0;
float target_pitch = 0;
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
float target_yaw_rate = 0;
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
}
