/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
Disable throttle if following conditions are met:
* 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
* AND
* 2 - Our reported altitude is within 10 meters of the home altitude.
* 3 - Our reported speed is under 5 meters per second.
* 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached
* OR
* 5 - Home location is not set
*/
bool Plane::suppress_throttle(void)
{
if (!throttle_suppressed) {
// we've previously met a condition for unsupressing the throttle
return false;
}
if (!auto_throttle_mode) {
// the user controls the throttle
throttle_suppressed = false;
return false;
}
if (control_mode==AUTO && g.auto_fbw_steer) {
// user has throttle control
return false;
}
if (control_mode==AUTO &&
auto_state.takeoff_complete == false) {
if (auto_takeoff_check()) {
// we're in auto takeoff
throttle_suppressed = false;
return false;
}
// keep throttle suppressed
return true;
}
if (relative_altitude_abs_cm() >= 1000) {
// we're more than 10m from the home altitude
throttle_suppressed = false;
gcs_send_text_fmt(PSTR("Throttle unsuppressed - altitude %.2f"),
(double)(relative_altitude_abs_cm()*0.01f));
return false;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
gps.ground_speed() >= 5) {
// if we have an airspeed sensor, then check it too, and
// require 5m/s. This prevents throttle up due to spiky GPS
// groundspeed with bad GPS reception
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) {
// we're moving at more than 5 m/s
gcs_send_text_fmt(PSTR("Throttle unsuppressed - speed %.2f airspeed %.2f"),
(double)gps.ground_speed(),
(double)airspeed.get_airspeed());
throttle_suppressed = false;
return false;
}
}
// throttle remains suppressed
return true;
}
bool Plane::verify_takeoff()
{
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5;
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
auto_state.takeoff_speed_time_ms = millis();
}
if (auto_state.takeoff_speed_time_ms != 0 &&
millis() - auto_state.takeoff_speed_time_ms >= 2000) {
// once we reach sufficient speed for good GPS course
// estimation we save our current GPS ground course
// corrected for summed yaw to set the take off
// course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitrary
// compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
takeoff_course = wrap_PI(takeoff_course);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)",
steer_state.hold_course_cd,
(double)gps.ground_speed(),
(double)degrees(steer_state.locked_course_err));
}
}
if (steer_state.hold_course_cd != -1) {
// call navigation controller for heading hold
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
nav_controller->update_level_flight();
}
// see if we have reached takeoff altitude
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
(double)(relative_alt_cm*0.01f));
steer_state.hold_course_cd = -1;
auto_state.takeoff_complete = true;
next_WP_loc = prev_WP_loc = current_loc;
plane.complete_auto_takeoff();
// don't cross-track on completion of takeoff, as otherwise we
// can end up doing too sharp a turn
auto_state.next_wp_no_crosstrack = true;
return true;
} else {
return false;
}
}
void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
g.speed_cruise.set(cmd.content.speed.target_ms);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", (double)g.speed_cruise.get());
}
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
g.throttle_cruise.set(cmd.content.speed.throttle_pct);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", g.throttle_cruise.get());
}
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
switch (fs) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
auto_state.land_in_progress = true;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
auto_state.land_in_progress = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
auto_state.land_in_progress = true;
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_VTOL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_state.land_in_progress = false;
break;
}
flight_stage = fs;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
void Plane::control_failsafe(uint16_t pwm)
{
if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
// control inputs to the trim value and throttle to min
channel_roll->radio_in = channel_roll->radio_trim;
channel_pitch->radio_in = channel_pitch->radio_trim;
channel_rudder->radio_in = channel_rudder->radio_trim;
// note that we don't set channel_throttle->radio_in to radio_trim,
// as that would cause throttle failsafe to not activate
channel_roll->control_in = 0;
channel_pitch->control_in = 0;
channel_rudder->control_in = 0;
channel_throttle->control_in = 0;
}
if(g.throttle_fs_enabled == 0)
return;
if (g.throttle_fs_enabled) {
if (rc_failsafe_active()) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafe.ch3_counter++;
if (failsafe.ch3_counter == 10) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS ON %u", (unsigned)pwm);
failsafe.ch3_failsafe = true;
AP_Notify::flags.failsafe_radio = true;
}
if (failsafe.ch3_counter > 10) {
failsafe.ch3_counter = 10;
}
}else if(failsafe.ch3_counter > 0) {
// we are no longer in failsafe condition
// but we need to recover quickly
failsafe.ch3_counter--;
if (failsafe.ch3_counter > 3) {
failsafe.ch3_counter = 3;
}
if (failsafe.ch3_counter == 1) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS OFF %u", (unsigned)pwm);
} else if(failsafe.ch3_counter == 0) {
failsafe.ch3_failsafe = false;
AP_Notify::flags.failsafe_radio = false;
}
}
}
}
/*
* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
*/
int16_t Plane::get_takeoff_pitch_min_cd(void)
{
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) {
return auto_state.takeoff_pitch_cd;
}
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
int32_t remaining_height_to_target_cm = (auto_state.takeoff_altitude_rel_cm - relative_alt_cm);
// seconds to target alt method
if (g.takeoff_pitch_limit_reduction_sec > 0) {
// if height-below-target has been initialized then use it to create and apply a scaler to the pitch_min
if (auto_state.height_below_takeoff_to_level_off_cm != 0) {
float scalar = remaining_height_to_target_cm / (float)auto_state.height_below_takeoff_to_level_off_cm;
return auto_state.takeoff_pitch_cd * scalar;
}
// are we entering the region where we want to start leveling off before we reach takeoff alt?
if (auto_state.sink_rate < -0.1f) {
float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
if (sec_to_target > 0 &&
relative_alt_cm >= 1000 &&
sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
// make a note of that altitude to use it as a start height for scaling
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100);
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
}
}
}
return auto_state.takeoff_pitch_cd;
}
// exit_mission_callback - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Plane::exit_mission_callback()
{
if (control_mode == AUTO) {
set_mode(RTL, MODE_REASON_MISSION_END);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
}
}
// exit_mission - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Rover::exit_mission()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
set_mode(HOLD);
}
}
// manual_init - initialise manual controller
bool Sub::manual_init(bool ignore_checks)
{
// Reuse the stabilize_init
bool success = stabilize_init(ignore_checks);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "MANUAL flight mode initialized!");
return success;
}
// exit_mission - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Rover::exit_mission()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("No commands. Can't set AUTO - setting HOLD"));
set_mode(HOLD);
}
}
/*
verify a LOITER_TO_ALT command. This involves checking we have
reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once.
*/
bool Plane::verify_loiter_to_alt()
{
bool result = false;
update_loiter(mission.get_current_nav_cmd().p1);
// condition_value == 0 means alt has never been reached
if (condition_value == 0) {
// primary goal, loiter to alt
if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
// primary goal completed, initialize secondary heading goal
if (loiter.unable_to_acheive_target_alt) {
gcs_send_text_fmt(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", current_loc.alt/100);
}
condition_value = 1;
result = verify_loiter_heading(true);
}
} else {
// secondary goal, loiter to heading
result = verify_loiter_heading(false);
}
if (result) {
gcs_send_text(MAV_SEVERITY_INFO,"Loiter to alt complete");
}
return result;
}
bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
// Check if we need to loiter at this waypoint
if (loiter_time_max > 0) {
if (loiter_time == 0) { // check if we are just starting loiter
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
(unsigned)cmd.index,
(unsigned)loiter_time_max);
// record the current time i.e. start timer
loiter_time = millis();
}
// Check if we have loiter long enough
if (((millis() - loiter_time) / 1000) < loiter_time_max) {
return false;
}
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Distance %um",
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
}
return true;
}
// have we gone past the waypoint?
// We should always go through the waypoint i.e. the above code
// first before we go past it.
if (location_passed_point(current_loc, prev_WP, next_WP)) {
// check if we have gone further past the wp then last time and output new message if we have
if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
distance_past_wp = get_distance(current_loc, next_WP);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i. Distance %um",
(unsigned)cmd.index,
(unsigned)distance_past_wp);
}
// Check if we need to loiter at this waypoint
if (loiter_time_max > 0) {
if (((millis() - loiter_time) / 1000) < loiter_time_max) {
return false;
}
}
distance_past_wp = 0;
return true;
}
return false;
}
/*
Restart a landing by first checking for a DO_LAND_START and
jump there. Otherwise decrement waypoint so we would re-start
from the top with same glide slope. Return true if successful.
*/
bool Plane::restart_landing_sequence()
{
if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
return false;
}
uint16_t do_land_start_index = mission.get_landing_sequence_start();
uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
bool success = false;
uint16_t current_index = mission.get_current_nav_index();
AP_Mission::Mission_Command cmd;
if (mission.read_cmd_from_storage(current_index+1,cmd) &&
cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT &&
(cmd.p1 == 0 || cmd.p1 == 1) &&
mission.set_current_cmd(current_index+1))
{
// if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100);
success = true;
}
else if (do_land_start_index != 0 &&
mission.set_current_cmd(do_land_start_index))
{
// look for a DO_LAND_START and use that index
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
success = true;
}
else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
mission.set_current_cmd(prev_cmd_with_wp_index))
{
// if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
// repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
success = true;
} else {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
success = false;
}
if (success) {
// exit landing stages if we're no longer executing NAV_LAND
update_flight_stage();
}
return success;
}
void Plane::failsafe_long_on_event(enum failsafe_state fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, ");
// If the GCS is locked up we allow control to revert to RC
hal.rcin->clear_overrides();
failsafe.state = fstype;
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
case CIRCLE:
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else {
set_mode(RTL);
}
break;
case QSTABILIZE:
case QLOITER:
set_mode(QHOVER);
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else if (g.long_fs_action == 1) {
set_mode(RTL);
}
break;
case RTL:
case QHOVER:
default:
break;
}
if (fstype == FAILSAFE_GCS) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat");
}
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason)
{
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.ch3_timer_ms = millis();
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on, ");
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A, reason);
} else {
set_mode(CIRCLE, reason);
}
break;
case QSTABILIZE:
case QLOITER:
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QLAND, reason);
break;
case AUTO:
case AVOID_ADSB:
case GUIDED:
case LOITER:
if(g.short_fs_action != 0) {
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A, reason);
} else {
set_mode(CIRCLE, reason);
}
}
break;
case CIRCLE:
case RTL:
case QLAND:
case QRTL:
default:
break;
}
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
return true;
}
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP));
return true;
}
return false;
}
/*
update navigation for normal mission waypoints. Return true when the
waypoint is complete
*/
bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
steer_state.hold_course_cd = -1;
if (auto_state.no_crosstrack) {
nav_controller->update_waypoint(current_loc, next_WP_loc);
} else {
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
}
// see if the user has specified a maximum distance to waypoint
if (g.waypoint_max_radius > 0 &&
auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
// this is needed to ensure completion of the waypoint
prev_WP_loc = current_loc;
}
return false;
}
float acceptance_distance = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle);
if (cmd.p1 > 0) {
// allow user to override acceptance radius
acceptance_distance = cmd.p1;
}
if (auto_state.wp_distance <= acceptance_distance) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP_loc));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP_loc));
return true;
}
return false;
}
void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.content.speed.speed_type)
{
case 0: // Airspeed
if (cmd.content.speed.target_ms > 0) {
g.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100);
gcs_send_text_fmt(PSTR("Set airspeed %u m/s"), (unsigned)cmd.content.speed.target_ms);
}
break;
case 1: // Ground speed
gcs_send_text_fmt(PSTR("Set groundspeed %u"), (unsigned)cmd.content.speed.target_ms);
g.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100);
break;
}
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
gcs_send_text_fmt(PSTR("Set throttle %u"), (unsigned)cmd.content.speed.throttle_pct);
aparm.throttle_cruise.set(cmd.content.speed.throttle_pct);
}
}
// do_nav_delay - Delay the next navigation command
void Copter::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
nav_delay_time_start = millis();
if (cmd.content.nav_delay.seconds > 0) {
// relative delay
nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
nav_delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
}
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000));
}
void Plane::log_perf_info()
{
if (scheduler.debug() != 0) {
gcs_send_text_fmt(PSTR("G_Dt_max=%lu G_Dt_min=%lu\n"),
(unsigned long)G_Dt_max,
(unsigned long)G_Dt_min);
}
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
G_Dt_max = 0;
G_Dt_min = 0;
resetPerfData();
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
switch (fs) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted via throttle. Climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
break;
}
flight_stage = fs;
}
/*
called to set/unset a failsafe event.
*/
void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
{
uint8_t old_bits = failsafe.bits;
if (on) {
failsafe.bits |= failsafe_type;
} else {
failsafe.bits &= ~failsafe_type;
}
if (old_bits == 0 && failsafe.bits != 0) {
// a failsafe event has started
failsafe.start_time = millis();
}
if (failsafe.triggered != 0 && failsafe.bits == 0) {
// a failsafe event has ended
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
}
failsafe.triggered &= failsafe.bits;
if (failsafe.triggered == 0 &&
failsafe.bits != 0 &&
millis() - failsafe.start_time > g.fs_timeout*1000 &&
control_mode != RTL &&
control_mode != HOLD) {
failsafe.triggered = failsafe.bits;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned)failsafe.triggered);
switch (g.fs_action) {
case 0:
break;
case 1:
set_mode(RTL);
break;
case 2:
set_mode(HOLD);
break;
}
}
}
void Copter::perf_update(void)
{
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
if (scheduler.debug()) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu\n",
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(),
(unsigned long)perf_info_get_min_time());
}
perf_info_reset();
pmTest1 = 0;
}
void Plane::low_battery_event(void)
{
if (failsafe.low_battery) {
return;
}
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (!landing.in_progress) {
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}
// exit_mission_callback - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Plane::exit_mission_callback()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("Returning to Home"));
memset(&auto_rtl_command, 0, sizeof(auto_rtl_command));
auto_rtl_command.content.location =
rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
setup_terrain_target_alt(auto_rtl_command.content.location);
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
start_command(auto_rtl_command);
}
}
void Plane::low_battery_event(void)
{
if (failsafe.low_battery) {
return;
}
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"),
(double)battery.voltage(), (double)battery.current_total_mah());
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}
// failsafe_gcs_check - check for ground station failsafe
void Sub::failsafe_gcs_check()
{
// return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) {
return;
}
uint32_t tnow = AP_HAL::millis();
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
if (tnow < failsafe.last_heartbeat_ms + FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
failsafe.gcs = false;
return;
}
//////////////////////////////
// GCS heartbeat has timed out
//////////////////////////////
// Send a warning every 30 seconds
if (tnow > failsafe.last_gcs_warn_ms + 30000) {
failsafe.last_gcs_warn_ms = tnow;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MYGCS: %d, heartbeat lost", g.sysid_my_gcs);
}
// do nothing if we have already triggered the failsafe action, or if the motors are disarmed
if (failsafe.gcs || !motors.armed()) {
return;
}
// update state, log to dataflash
failsafe.gcs = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// handle failsafe action
if (g.failsafe_gcs == FS_GCS_DISARM) {
init_disarm_motors();
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE);
}
}
void Plane::low_battery_event(void)
{
if (failsafe.low_battery) {
return;
}
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}
void Plane::failsafe_short_on_event(enum failsafe_state fstype)
{
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.ch3_timer_ms = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else {
set_mode(CIRCLE);
}
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.short_fs_action != 0) {
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
if(g.short_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else {
set_mode(CIRCLE);
}
}
break;
case CIRCLE:
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
void Plane::log_perf_info()
{
if (scheduler.debug() != 0) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u\n",
(unsigned)perf.num_long,
(unsigned)perf.mainLoop_count,
(unsigned)perf.G_Dt_max,
(unsigned)perf.G_Dt_min,
(unsigned)(DataFlash.num_dropped() - perf.last_log_dropped));
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
resetPerfData();
}
