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