/**
* \brief This function is called when an enemy's sprite collides with a sprite of this entity.
* \param enemy the enemy
* \param enemy_sprite the enemy's sprite that overlaps the hero
* \param this_sprite the arrow sprite
*/
void Arrow::notify_collision_with_enemy(
Enemy& enemy, Sprite& enemy_sprite, Sprite& this_sprite) {
if (!overlaps(hero) && is_flying()) {
enemy.try_hurt(ATTACK_ARROW, *this, &enemy_sprite);
}
}
/*
setup landing gear state
*/
void Plane::set_landing_gear(void)
{
if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying()) {
AP_LandingGear::LandingGearCommand cmd = (AP_LandingGear::LandingGearCommand)gear.last_auto_cmd;
switch (flight_stage) {
case AP_Vehicle::FixedWing::FLIGHT_LAND:
cmd = AP_LandingGear::LandingGear_Deploy;
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
cmd = AP_LandingGear::LandingGear_Retract;
break;
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
if (quadplane.is_vtol_land(mission.get_current_nav_cmd().id)) {
cmd = AP_LandingGear::LandingGear_Deploy;
}
break;
default:
break;
}
if (cmd != gear.last_auto_cmd) {
gear.last_auto_cmd = (int8_t)cmd;
change_landing_gear(cmd);
}
}
}
/*
implement automatic persistent trim of control surfaces with
AUTO_TRIM=2, only available when SERVO_RNG_ENABLE=1 as otherwise it
would impact R/C transmitter calibration
*/
void Plane::servos_auto_trim(void)
{
// only in auto modes and FBWA
if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) {
return;
}
if (!hal.util->get_soft_armed()) {
return;
}
if (!is_flying()) {
return;
}
if (quadplane.in_assisted_flight() || quadplane.in_vtol_mode()) {
// can't auto-trim with quadplane motors running
return;
}
if (abs(nav_roll_cd) > 700 || abs(nav_pitch_cd) > 700) {
// only when close to level
return;
}
uint32_t now = AP_HAL::millis();
if (now - auto_trim.last_trim_check < 500) {
// check twice a second. We want slow trim update
return;
}
if (ahrs.groundspeed() < 8 || smoothed_airspeed < 8) {
// only when definitely moving
return;
}
// adjust trim on channels by a small amount according to I value
float roll_I = rollController.get_pid_info().I;
float pitch_I = pitchController.get_pid_info().I;
g2.servo_channels.adjust_trim(SRV_Channel::k_aileron, roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_elevator, pitch_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_elevon_left, pitch_I - roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_elevon_right, pitch_I + roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_vtail_left, pitch_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_vtail_right, pitch_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_left, roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_right, -roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerLeft1, pitch_I - roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerLeft2, pitch_I - roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerRight1, pitch_I + roll_I);
g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerRight2, pitch_I + roll_I);
auto_trim.last_trim_check = now;
if (now - auto_trim.last_trim_save > 10000) {
auto_trim.last_trim_save = now;
g2.servo_channels.save_trim();
}
}
/**
* \brief This function is called when a crystal detects a collision with this entity.
* \param crystal the crystal
* \param collision_mode the collision mode that detected the event
*/
void Arrow::notify_collision_with_crystal(Crystal& crystal, CollisionMode collision_mode) {
if (collision_mode == COLLISION_OVERLAPPING && is_flying()) {
crystal.activate(*this);
attach_to(crystal);
}
}
/**
* @brief This function is called when a crystal detects a collision with this entity.
* @param crystal the crystal
* @param collision_mode the collision mode that detected the event
*/
void Hookshot::notify_collision_with_crystal(Crystal& crystal, CollisionMode collision_mode) {
if (is_flying()) {
crystal.activate(*this);
if (!is_going_back()) {
go_back();
}
}
}
/**
* \brief This function is called when a switch detects a collision with this entity.
* \param sw the switch
* \param collision_mode the collision mode that detected the event
*/
void Arrow::notify_collision_with_switch(Switch& sw, CollisionMode collision_mode) {
if (sw.is_arrow_target() && is_stopped()) {
sw.try_activate(*this);
}
else if (sw.is_solid() && is_flying()) {
sw.try_activate();
attach_to(sw);
}
}
/**
* @brief This function is called when a switch detects a collision with this entity.
* @param sw the switch
* @param collision_mode the collision mode that detected the event
*/
void Hookshot::notify_collision_with_switch(Switch& sw, CollisionMode collision_mode) {
if (is_flying() && collision_mode == COLLISION_RECTANGLE) {
sw.try_activate();
if (!is_going_back()) {
go_back();
Sound::play("sword_tapping");
}
}
}
/**
* @brief Notifies this entity that it has just failed to change its position
* because of obstacles.
*/
void Hookshot::notify_obstacle_reached() {
if (is_flying()) {
if (!get_map().test_collision_with_border(
get_movement()->get_last_collision_box_on_obstacle())) {
// play a sound unless the obstacle is the map border
Sound::play("sword_tapping");
}
go_back();
}
}
/**
* @brief This function is called when a destructible item detects a non-pixel precise collision with this entity.
* @param destructible the destructible item
* @param collision_mode the collision mode that detected the event
*/
void Hookshot::notify_collision_with_destructible(Destructible& destructible, CollisionMode collision_mode) {
if (destructible.is_obstacle_for(*this) && is_flying()) {
if (destructible.can_explode()) {
destructible.explode();
go_back();
}
else {
attach_to(destructible);
}
}
}
/**
* \brief This function is called when a destructible item detects a non-pixel perfect collision with this entity.
* \param destructible the destructible item
* \param collision_mode the collision mode that detected the event
*/
void Arrow::notify_collision_with_destructible(
Destructible& destructible, CollisionMode collision_mode) {
if (destructible.is_obstacle_for(*this) && is_flying()) {
if (destructible.get_can_explode()) {
destructible.explode();
remove_from_map();
}
else {
attach_to(destructible);
}
}
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
only called from AP_Landing, when the landing library is ready to disarm
*/
void Plane::disarm_if_autoland_complete()
{
if (landing.get_disarm_delay() > 0 &&
!is_flying() &&
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
if (disarm_motors()) {
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
}
}
}
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
*/
void Plane::disarm_if_autoland_complete()
{
if (g.land_disarm_delay > 0 &&
auto_state.land_complete &&
!is_flying() &&
arming.arming_required() != AP_Arming::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
if (disarm_motors()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed"));
}
}
}
}
/*
implement automatic persistent trim of control surfaces with
AUTO_TRIM=2, only available when SERVO_RNG_ENABLE=1 as otherwise it
would impact R/C transmitter calibration
*/
void Plane::servos_auto_trim(void)
{
if (!g2.servo_channels.enabled()) {
// only possible with SERVO_RNG_ENABLE=1
return;
}
// only in auto modes and FBWA
if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) {
return;
}
if (!hal.util->get_soft_armed()) {
return;
}
if (!is_flying()) {
return;
}
if (quadplane.in_assisted_flight() || quadplane.in_vtol_mode()) {
// can't auto-trim with quadplane motors running
return;
}
if (abs(nav_roll_cd) > 700 || abs(nav_pitch_cd) > 700) {
// only when close to level
return;
}
uint32_t now = AP_HAL::millis();
if (now - auto_trim.last_trim_check < 500) {
// check twice a second. We want slow trim update
return;
}
if (ahrs.groundspeed() < 8 || smoothed_airspeed < 8) {
// only when definitely moving
return;
}
// adjust trim on channels by a small amount according to I value
g2.servo_channels.adjust_trim(channel_roll->get_ch_out(), rollController.get_pid_info().I);
g2.servo_channels.adjust_trim(channel_pitch->get_ch_out(), pitchController.get_pid_info().I);
auto_trim.last_trim_check = now;
if (now - auto_trim.last_trim_save > 10000) {
auto_trim.last_trim_save = now;
g2.servo_channels.save_trim();
}
}
/*
Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::determine_is_flying(void)
{
float aspeed;
bool isFlyingBool;
bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5);
// If we don't have a GPS lock then don't use GPS for this test
bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
gps.ground_speed() >= 5);
if (hal.util->get_soft_armed()) {
// when armed, we need overwhelming evidence that we ARE NOT flying
isFlyingBool = airspeedMovement || gpsMovement;
/*
make is_flying() more accurate for landing approach
*/
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
fabsf(auto_state.sink_rate) > 0.2f) {
isFlyingBool = true;
}
} else {
// when disarmed, we need overwhelming evidence that we ARE flying
isFlyingBool = airspeedMovement && gpsMovement;
}
// low-pass the result.
isFlyingProbability = (0.6f * isFlyingProbability) + (0.4f * (float)isFlyingBool);
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm
*/
if (is_flying()) {
auto_state.last_flying_ms = millis();
}
}
/*
parachute_manual_release - trigger the release of the parachute,
after performing some checks for pilot error checks if the vehicle
is landed
*/
bool Plane::parachute_manual_release()
{
// exit immediately if parachute is not enabled
if (!parachute.enabled() || parachute.released()) {
return false;
}
// do not release if vehicle is not flying
if (!is_flying()) {
// warn user of reason for failure
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Parachute: not flying"));
return false;
}
if (relative_altitude() < parachute.alt_min()) {
gcs_send_text_fmt(PSTR("Parachute: too low"));
return false;
}
// if we get this far release parachute
parachute_release();
return true;
}
/*
Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::update_is_flying_5Hz(void)
{
float aspeed;
bool is_flying_bool;
uint32_t now_ms = AP_HAL::millis();
uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500;
bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
(gps.ground_speed_cm() >= ground_speed_thresh_cm);
// airspeed at least 75% of stall speed?
bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));
if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
// short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
(gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now
(now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)
if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
// we've flown before, remove GPS constraints temporarily and only use airspeed
is_flying_bool = airspeed_movement; // moving through the air
} else {
// we've never flown yet, require good GPS movement
is_flying_bool = airspeed_movement || // moving through the air
gps_confirmed_movement; // locked and we're moving
}
if (control_mode == AUTO) {
/*
make is_flying() more accurate during various auto modes
*/
// Detect X-axis deceleration for probable ground impacts.
// Limit the max probability so it can decay faster. This
// will not change the is_flying state, anything above 0.1
// is "true", it just allows it to decay faster once we decide we
// aren't flying using the normal schemes
if (g.crash_accel_threshold == 0) {
crash_state.impact_detected = false;
} else if (ins.get_accel_peak_hold_neg_x() < -(g.crash_accel_threshold)) {
// large deceleration detected, lets lower confidence VERY quickly
crash_state.impact_detected = true;
crash_state.impact_timer_ms = now_ms;
if (isFlyingProbability > 0.2f) {
isFlyingProbability = 0.2f;
}
} else if (crash_state.impact_detected &&
(now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) {
// no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability
crash_state.impact_detected = false;
}
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (in_preLaunch_flight_stage()) {
// while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
// ensure we aren't showing a false positive.
is_flying_bool = false;
crash_state.is_crashed = false;
auto_state.started_flying_in_auto_ms = 0;
}
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// TODO: detect ground impacts
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (fabsf(auto_state.sink_rate) > 0.2f) {
is_flying_bool = true;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
if (auto_state.sink_rate < -0.5f) {
// steep climb
is_flying_bool = true;
}
break;
default:
break;
} // switch
}
} else {
// when disarmed assume not flying and need overwhelming evidence that we ARE flying
is_flying_bool = airspeed_movement && gps_confirmed_movement;
if ((control_mode == AUTO) &&
((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
is_flying_bool = false;
}
}
if (!crash_state.impact_detected || !is_flying_bool) {
// when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up.
// low-pass the result.
// coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
}
if (quadplane.is_flying()) {
is_flying_bool = true;
}
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm
*/
bool new_is_flying = is_flying();
// we are flying, note the time
if (new_is_flying) {
auto_state.last_flying_ms = now_ms;
if (!previous_is_flying) {
// just started flying in any mode
started_flying_ms = now_ms;
}
if ((control_mode == AUTO) &&
((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
// We just started flying, note that time also
auto_state.started_flying_in_auto_ms = now_ms;
}
}
previous_is_flying = new_is_flying;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO || !g.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
crash_state.is_crashed = false;
return;
}
uint32_t now_ms = AP_HAL::millis();
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying() && been_auto_flying)
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage()) {
crashed = true;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
crashed = true;
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However,
// a crash into a tree would be caught here.
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (!crash_state.checkedHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms + CRASH_DETECTION_DELAY_MS;
}
crash_state.checkedHardLanding = true;
break;
default:
break;
} // switch
} else {
crash_state.checkedHardLanding = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken");
} else {
gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken");
}
}
else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
auto_state.land_complete = true;
if (crashed_near_land_waypoint) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
} else {
gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
}
}
}
}
void drawable_unit::redraw_unit (display & disp) const
{
const display_context & dc = disp.get_disp_context();
const gamemap &map = dc.map();
const std::vector<team> &teams = dc.teams();
const team & viewing_team = teams[disp.viewing_team()];
unit_animation_component & ac = *anim_comp_;
if ( hidden_ || disp.is_blindfolded() || !is_visible_to_team(viewing_team,map, disp.show_everything()) )
{
ac.clear_haloes();
if(ac.anim_) {
ac.anim_->update_last_draw_time();
}
return;
}
if (!ac.anim_) {
ac.set_standing();
if (!ac.anim_) return;
}
if (ac.refreshing_) return;
ac.refreshing_ = true;
ac.anim_->update_last_draw_time();
frame_parameters params;
const t_translation::t_terrain terrain = map.get_terrain(loc_);
const terrain_type& terrain_info = map.get_terrain_info(terrain);
// do not set to 0 so we can distinguish the flying from the "not on submerge terrain"
// instead use -1.0 (as in "negative depth", it will be ignored by rendering)
params.submerge= is_flying() ? -1.0 : terrain_info.unit_submerge();
if (invisible(loc_) &&
params.highlight_ratio > 0.5) {
params.highlight_ratio = 0.5;
}
if (loc_ == disp.selected_hex() && params.highlight_ratio == 1.0) {
params.highlight_ratio = 1.5;
}
int height_adjust = static_cast<int>(terrain_info.unit_height_adjust() * disp.get_zoom_factor());
if (is_flying() && height_adjust < 0) {
height_adjust = 0;
}
params.y -= height_adjust;
params.halo_y -= height_adjust;
int red = 0,green = 0,blue = 0,tints = 0;
double blend_ratio = 0;
// Add future colored states here
if(get_state(STATE_POISONED)) {
green += 255;
blend_ratio += 0.25;
tints += 1;
}
if(get_state(STATE_SLOWED)) {
red += 191;
green += 191;
blue += 255;
blend_ratio += 0.25;
tints += 1;
}
if(tints > 0) {
params.blend_with = disp.rgb((red/tints),(green/tints),(blue/tints));
params.blend_ratio = ((blend_ratio/tints));
}
//hackish : see unit_frame::merge_parameters
// we use image_mod on the primary image
// and halo_mod on secondary images and all haloes
params.image_mod = image_mods();
params.halo_mod = TC_image_mods();
params.image= absolute_image();
if(get_state(STATE_PETRIFIED)) params.image_mod +="~GS()";
params.primary_frame = t_true;
const frame_parameters adjusted_params = ac.anim_->get_current_params(params);
const map_location dst = loc_.get_direction(facing_);
const int xsrc = disp.get_location_x(loc_);
const int ysrc = disp.get_location_y(loc_);
const int xdst = disp.get_location_x(dst);
const int ydst = disp.get_location_y(dst);
int d2 = disp.hex_size() / 2;
const int x = static_cast<int>(adjusted_params.offset * xdst + (1.0-adjusted_params.offset) * xsrc) + d2;
const int y = static_cast<int>(adjusted_params.offset * ydst + (1.0-adjusted_params.offset) * ysrc) + d2;
if(ac.unit_halo_ == halo::NO_HALO && !image_halo().empty()) {
ac.unit_halo_ = halo::add(0, 0, image_halo()+TC_image_mods(), map_location(-1, -1));
}
if(ac.unit_halo_ != halo::NO_HALO && image_halo().empty()) {
halo::remove(ac.unit_halo_);
ac.unit_halo_ = halo::NO_HALO;
} else if(ac.unit_halo_ != halo::NO_HALO) {
halo::set_location(ac.unit_halo_, x, y - height_adjust);
}
// We draw bars only if wanted, visible on the map view
bool draw_bars = ac.draw_bars_ ;
if (draw_bars) {
const int d = disp.hex_size();
SDL_Rect unit_rect = sdl::create_rect(xsrc, ysrc +adjusted_params.y, d, d);
draw_bars = sdl::rects_overlap(unit_rect, disp.map_outside_area());
}
surface ellipse_front(NULL);
surface ellipse_back(NULL);
int ellipse_floating = 0;
// Always show the ellipse for selected units
if(draw_bars && (preferences::show_side_colors() || disp.selected_hex() == loc_)) {
if(adjusted_params.submerge > 0.0) {
// The division by 2 seems to have no real meaning,
// It just works fine with the current center of ellipse
// and prevent a too large adjust if submerge = 1.0
ellipse_floating = static_cast<int>(adjusted_params.submerge * disp.hex_size() / 2);
}
std::string ellipse=image_ellipse();
if(ellipse.empty()){
ellipse="misc/ellipse";
}
if(ellipse != "none") {
// check if the unit has a ZoC or can recruit
const char* const nozoc = emit_zoc_ ? "" : "nozoc-";
const char* const leader = can_recruit() ? "leader-" : "";
const char* const selected = disp.selected_hex() == loc_ ? "selected-" : "";
// Load the ellipse parts recolored to match team color
char buf[100];
std::string tc=team::get_side_color_index(side_);
snprintf(buf,sizeof(buf),"%s-%s%s%stop.png~RC(ellipse_red>%s)",ellipse.c_str(),leader,nozoc,selected,tc.c_str());
ellipse_back.assign(image::get_image(image::locator(buf), image::SCALED_TO_ZOOM));
snprintf(buf,sizeof(buf),"%s-%s%s%sbottom.png~RC(ellipse_red>%s)",ellipse.c_str(),leader,nozoc,selected,tc.c_str());
ellipse_front.assign(image::get_image(image::locator(buf), image::SCALED_TO_ZOOM));
}
}
if (ellipse_back != NULL) {
//disp.drawing_buffer_add(display::LAYER_UNIT_BG, loc,
disp.drawing_buffer_add(display::LAYER_UNIT_FIRST, loc_,
xsrc, ysrc +adjusted_params.y-ellipse_floating, ellipse_back);
}
if (ellipse_front != NULL) {
//disp.drawing_buffer_add(display::LAYER_UNIT_FG, loc,
disp.drawing_buffer_add(display::LAYER_UNIT_FIRST, loc_,
xsrc, ysrc +adjusted_params.y-ellipse_floating, ellipse_front);
}
if(draw_bars) {
const image::locator* orb_img = NULL;
/*static*/ const image::locator partmoved_orb(game_config::images::orb + "~RC(magenta>" +
preferences::partial_color() + ")" );
/*static*/ const image::locator moved_orb(game_config::images::orb + "~RC(magenta>" +
preferences::moved_color() + ")" );
/*static*/ const image::locator ally_orb(game_config::images::orb + "~RC(magenta>" +
preferences::allied_color() + ")" );
/*static*/ const image::locator enemy_orb(game_config::images::orb + "~RC(magenta>" +
preferences::enemy_color() + ")" );
/*static*/ const image::locator unmoved_orb(game_config::images::orb + "~RC(magenta>" +
preferences::unmoved_color() + ")" );
const std::string* energy_file = &game_config::images::energy;
if(size_t(side()) != disp.viewing_team()+1) {
if(disp.team_valid() &&
viewing_team.is_enemy(side())) {
if (preferences::show_enemy_orb() && !get_state(STATE_PETRIFIED))
orb_img = &enemy_orb;
else
orb_img = NULL;
} else {
if (preferences::show_allied_orb())
orb_img = &ally_orb;
else orb_img = NULL;
}
} else {
if (preferences::show_moved_orb())
orb_img = &moved_orb;
else orb_img = NULL;
if(disp.playing_team() == disp.viewing_team() && !user_end_turn()) {
if (movement_left() == total_movement()) {
if (preferences::show_unmoved_orb())
orb_img = &unmoved_orb;
else orb_img = NULL;
} else if ( dc.unit_can_move(*this) ) {
if (preferences::show_partial_orb())
orb_img = &partmoved_orb;
else orb_img = NULL;
}
}
}
if (orb_img != NULL) {
surface orb(image::get_image(*orb_img,image::SCALED_TO_ZOOM));
disp.drawing_buffer_add(display::LAYER_UNIT_BAR,
loc_, xsrc, ysrc +adjusted_params.y, orb);
}
double unit_energy = 0.0;
if(max_hitpoints() > 0) {
unit_energy = double(hitpoints())/double(max_hitpoints());
}
const int bar_shift = static_cast<int>(-5*disp.get_zoom_factor());
const int hp_bar_height = static_cast<int>(max_hitpoints() * hp_bar_scaling_);
const fixed_t bar_alpha = (loc_ == disp.mouseover_hex() || loc_ == disp.selected_hex()) ? ftofxp(1.0): ftofxp(0.8);
disp.draw_bar(*energy_file, xsrc+bar_shift, ysrc +adjusted_params.y,
loc_, hp_bar_height, unit_energy,hp_color(), bar_alpha);
if(experience() > 0 && can_advance()) {
const double filled = double(experience())/double(max_experience());
const int xp_bar_height = static_cast<int>(max_experience() * xp_bar_scaling_ / std::max<int>(level_,1));
SDL_Color color=xp_color();
disp.draw_bar(*energy_file, xsrc, ysrc +adjusted_params.y,
loc_, xp_bar_height, filled, color, bar_alpha);
}
if (can_recruit()) {
surface crown(image::get_image(leader_crown(),image::SCALED_TO_ZOOM));
if(!crown.null()) {
//if(bar_alpha != ftofxp(1.0)) {
// crown = adjust_surface_alpha(crown, bar_alpha);
//}
disp.drawing_buffer_add(display::LAYER_UNIT_BAR,
loc_, xsrc, ysrc +adjusted_params.y, crown);
}
}
for(std::vector<std::string>::const_iterator ov = overlays().begin(); ov != overlays().end(); ++ov) {
const surface ov_img(image::get_image(*ov, image::SCALED_TO_ZOOM));
if(ov_img != NULL) {
disp.drawing_buffer_add(display::LAYER_UNIT_BAR,
loc_, xsrc, ysrc +adjusted_params.y, ov_img);
}
}
}
// Smooth unit movements from terrain of different elevation.
// Do this separately from above so that the health bar doesn't go up and down.
const t_translation::t_terrain terrain_dst = map.get_terrain(dst);
const terrain_type& terrain_dst_info = map.get_terrain_info(terrain_dst);
int height_adjust_unit = static_cast<int>((terrain_info.unit_height_adjust() * (1.0 - adjusted_params.offset) +
terrain_dst_info.unit_height_adjust() * adjusted_params.offset) *
disp.get_zoom_factor());
if (is_flying() && height_adjust_unit < 0) {
height_adjust_unit = 0;
}
params.y -= height_adjust_unit - height_adjust;
params.halo_y -= height_adjust_unit - height_adjust;
ac.anim_->redraw(params);
ac.refreshing_ = false;
}
/*
Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::update_is_flying_5Hz(void)
{
float aspeed;
bool is_flying_bool;
uint32_t now_ms = AP_HAL::millis();
uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500;
bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
(gps.ground_speed_cm() >= ground_speed_thresh_cm);
// airspeed at least 75% of stall speed?
bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));
if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
// short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
(gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now
(now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)
if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
// we've flown before, remove GPS constraints temporarily and only use airspeed
is_flying_bool = airspeed_movement; // moving through the air
} else {
// we've never flown yet, require good GPS movement
is_flying_bool = airspeed_movement || // moving through the air
gps_confirmed_movement; // locked and we're moving
}
if (control_mode == AUTO) {
/*
make is_flying() more accurate during various auto modes
*/
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
// while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
// ensure we aren't showing a false positive. If the throttle is suppressed
// we are definitely not flying, or at least for not much longer!
if (throttle_suppressed) {
is_flying_bool = false;
crash_state.is_crashed = false;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
case AP_SpdHgtControl::FLIGHT_NORMAL:
// TODO: detect ground impacts
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
// TODO: detect ground impacts
if (fabsf(auto_state.sink_rate) > 0.2f) {
is_flying_bool = true;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
break;
} // switch
}
} else {
// when disarmed assume not flying and need overwhelming evidence that we ARE flying
is_flying_bool = airspeed_movement && gps_confirmed_movement;
if ((control_mode == AUTO) &&
((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
is_flying_bool = false;
}
}
// low-pass the result.
// coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm
*/
bool new_is_flying = is_flying();
// we are flying, note the time
if (new_is_flying) {
auto_state.last_flying_ms = now_ms;
if (!previous_is_flying) {
// just started flying in any mode
started_flying_ms = now_ms;
}
if ((control_mode == AUTO) &&
((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
// We just started flying, note that time also
auto_state.started_flying_in_auto_ms = now_ms;
}
}
previous_is_flying = new_is_flying;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
/* 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
* OR
* 6- Landing does not want to allow throttle
*/
bool Plane::suppress_throttle(void)
{
#if PARACHUTE == ENABLED
if (auto_throttle_mode && parachute.release_initiated()) {
// throttle always suppressed in auto-throttle modes after parachute release initiated
throttle_suppressed = true;
return true;
}
#endif
if (landing.is_throttle_suppressed()) {
return true;
}
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 == 42) {
// user has throttle control
return false;
}
bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5);
if (control_mode==AUTO &&
auto_state.takeoff_complete == false) {
uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000;
if (is_flying() &&
millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode
adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home
labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch
gps_movement) { // definite gps movement
// we're already flying, do not suppress the throttle. We can get
// stuck in this condition if we reset a mission and cmd 1 is takeoff
// but we're currently flying around below the takeoff altitude
throttle_suppressed = false;
return false;
}
if (auto_takeoff_check()) {
// we're in auto takeoff
throttle_suppressed = false;
auto_state.baro_takeoff_alt = barometer.get_altitude();
return false;
}
// keep throttle suppressed
return true;
}
if (fabsf(relative_altitude) >= 10.0f) {
// we're more than 10m from the home altitude
throttle_suppressed = false;
return false;
}
if (gps_movement) {
// 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
throttle_suppressed = false;
return false;
}
}
if (quadplane.is_flying()) {
throttle_suppressed = false;
return false;
}
// throttle remains suppressed
return true;
}
/**
* @brief This function is called when a chest detects a collision with this entity.
* @param chest the chest
*/
void Hookshot::notify_collision_with_chest(Chest& chest) {
if (is_flying()) {
attach_to(chest);
}
}
/*
run output for tailsitters
*/
void QuadPlane::tailsitter_output(void)
{
if (!is_tailsitter()) {
return;
}
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
if (tailsitter.vectored_forward_gain > 0) {
// thrust vectoring in fixed wing flight
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
float tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain;
float tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain;
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 0);
}
if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) {
/*
during transitions to vtol mode set the throttle to the
hover throttle, and set the altitude controller
integrator to the same throttle level
*/
uint8_t throttle = motors->get_throttle_hover() * 100;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
pos_control->get_accel_z_pid().set_integrator(throttle*10);
}
return;
}
motors_output(false);
plane.pitchController.reset_I();
plane.rollController.reset_I();
if (hal.util->get_soft_armed()) {
// scale surfaces for throttle
tailsitter_speed_scaling();
}
if (tailsitter.vectored_hover_gain > 0) {
// thrust vectoring VTOL modes
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
/*
apply extra elevator when at high pitch errors, using a
power law. This allows the motors to point straight up for
takeoff without integrator windup
*/
int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0;
float extra_sign = extra_pitch > 0?1:-1;
float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500;
float tilt_left = extra_elevator + (elevator + aileron) * tailsitter.vectored_hover_gain;
float tilt_right = extra_elevator + (elevator - aileron) * tailsitter.vectored_hover_gain;
if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) {
// prevent integrator windup
motors->limit.roll_pitch = 1;
motors->limit.yaw = 1;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
}
if (tailsitter.input_mask_chan > 0 &&
tailsitter.input_mask > 0 &&
RC_Channels::get_radio_in(tailsitter.input_mask_chan-1) > 1700) {
// the user is learning to prop-hang
if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
}
if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
}
if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.channel_throttle->get_control_in_zero_dz());
}
if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) {
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());
}
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (channel_throttle->control_in != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode then disallow rudder arming/disarming
if (auto_throttle_mode ) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->control_in > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->control_in < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
{
AP_Arming::ArmingRudder arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) {
//parameter disallows rudder arming/disabling
return;
}
// if throttle is not down, then pilot cannot rudder arm/disarm
if (get_throttle_input() != 0){
rudder_arm_timer = 0;
return;
}
// if not in a manual throttle mode and not in CRUISE or FBWB
// modes then disallow rudder arming/disarming
if (auto_throttle_mode &&
(control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !is_flying()) {
// when armed and not flying, full left rudder starts disarming counter
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
/**
* @brief This function is called when a block detects a collision with this entity.
* @param block the block
*/
void Hookshot::notify_collision_with_block(Block& block) {
if (is_flying()) {
attach_to(block);
}
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO || !aparm.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
crash_state.is_crashed = false;
return;
}
uint32_t now_ms = AP_HAL::millis();
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying() && arming.is_armed())
{
if (landing.is_expecting_impact()) {
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (!crash_state.checkedHardLanding && // only check once
been_auto_flying &&
(labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
current_loc.get_distance(mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms;
crash_state.debounce_time_total_ms = 0; // no debounce
}
crash_state.checkedHardLanding = true;
} else if (landing.is_on_approach()) {
// when altitude gets low, we automatically flare so ground crashes
// most likely can not be triggered from here. However,
// a crash into a tree would be caught here.
if (been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
} else {
switch (flight_stage)
{
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
if (g.takeoff_throttle_min_accel > 0 &&
!throttle_suppressed) {
// if you have an acceleration holding back throttle, but you met the
// accel threshold but still not fying, then you either shook/hit the
// plane or it was a failed launch.
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage() && been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
default:
break;
} // switch
}
} else {
crash_state.checkedHardLanding = false;
}
// if we have no GPS lock and we don't have a functional airspeed
// sensor then don't do crash detection
if (gps.status() < AP_GPS::GPS_OK_FIX_3D && (!airspeed.use() || !airspeed.healthy())) {
crashed = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (aparm.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken");
} else {
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken");
}
}
else {
if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
if (crashed_near_land_waypoint) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
} else {
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
}
}
}
}
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
{
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
} else {
// use rangefinder to correct if possible
const float height = height_above_target() - rangefinder_correction();
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range);
}
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_LOITER_TO_ALT:
return verify_loiter_to_alt();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
return verify_continue_and_change_alt();
case MAV_CMD_NAV_ALTITUDE_WAIT:
return verify_altitude_wait(cmd);
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
return quadplane.verify_vtol_land();
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
return true;
#endif
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_INVERTED_FLIGHT:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_AUTOTUNE_ENABLE:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_VTOL_TRANSITION:
case MAV_CMD_DO_ENGINE_CONTROL:
return true;
default:
// error message
gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
}
/*
update navigation for landing. Called when on landing approach or
final flare
*/
bool Plane::verify_land()
{
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
// If a go around has been commanded, we are done landing. This will send
// the mission to the next mission item, which presumably is a mission
// segment with operations to perform when a landing is called off.
// If there are no commands after the land waypoint mission item then
// the plane will proceed to loiter about its home point.
if (auto_state.commanded_go_around) {
return true;
}
float height = height_above_target();
// use rangefinder to correct if possible
height -= rangefinder_correction();
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
by the calculated sink rate (if LAND_FLARE_SEC != 0)
3) we have gone past the landing point and don't have
rangefinder data (to prevent us keeping throttle on
after landing if we've had positive baro drift)
*/
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_in_range = rangefinder_state.in_range;
#else
bool rangefinder_in_range = false;
#endif
if (height <= g.land_flare_alt ||
(aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) ||
(!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) ||
(fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) {
if (!auto_state.land_complete) {
auto_state.post_landing_stats = true;
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
} else {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
(double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
}
}
auto_state.land_complete = true;
if (gps.ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
// target speeds too early.
g.airspeed_cruise_cm.load();
g.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
}
/*
when landing we keep the L1 navigation waypoint 200m ahead. This
prevents sudden turns if we overshoot the landing point
*/
struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc,
land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
// once landed and stationary, post some statistics
// this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
if (auto_state.post_landing_stats && !arming.is_armed()) {
auto_state.post_landing_stats = false;
gcs_send_text_fmt(PSTR("Distance from LAND point=%.2fm"), (double)get_distance(current_loc, next_WP_loc));
}
// check if we should auto-disarm after a confirmed landing
disarm_if_autoland_complete();
/*
we return false as a landing mission item never completes
we stay on this waypoint unless the GCS commands us to change
mission item or reset the mission, or a go-around is commanded
*/
return false;
}
/*
update navigation for landing. Called when on landing approach or
final flare
*/
bool Plane::verify_land()
{
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
// the altitude has been reached, restart the landing sequence
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
throttle_suppressed = false;
auto_state.land_complete = false;
auto_state.land_pre_flare = false;
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
// see if we have reached abort altitude
if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) {
next_WP_loc = current_loc;
mission.stop();
bool success = restart_landing_sequence();
mission.resume();
if (!success) {
// on a restart failure lets RTL or else the plane may fly away with nowhere to go!
set_mode(RTL, MODE_REASON_MISSION_END);
}
// make sure to return false so it leaves the mission index alone
}
return false;
}
float height = height_above_target();
// use rangefinder to correct if possible
height -= rangefinder_correction();
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
by the calculated sink rate (if LAND_FLARE_SEC != 0)
3) we have gone past the landing point and don't have
rangefinder data (to prevent us keeping throttle on
after landing if we've had positive baro drift)
*/
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_in_range = rangefinder_state.in_range;
#else
bool rangefinder_in_range = false;
#endif
// flare check:
// 1) below flare alt/sec requires approach stage check because if sec/alt are set too
// large, and we're on a hard turn to line up for approach, we'll prematurely flare by
// skipping approach phase and the extreme roll limits will make it hard to line up with runway
// 2) passed land point and don't have an accurate AGL
// 3) probably crashed (ensures motor gets turned off)
bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
bool below_flare_alt = (height <= g.land_flare_alt);
bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec);
bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying());
if ((on_approach_stage && below_flare_alt) ||
(on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) ||
(!rangefinder_in_range && auto_state.wp_proportion >= 1) ||
probably_crashed) {
if (!auto_state.land_complete) {
auto_state.post_landing_stats = true;
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
(double)height, (double)auto_state.sink_rate,
(double)gps.ground_speed(),
(double)get_distance(current_loc, next_WP_loc));
}
auto_state.land_complete = true;
update_flight_stage();
}
if (gps.ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
// target speeds too early.
g.airspeed_cruise_cm.load();
g.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
} else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt);
bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec);
if (reached_pre_flare_alt || reached_pre_flare_sec) {
auto_state.land_pre_flare = true;
update_flight_stage();
}
}
/*
when landing we keep the L1 navigation waypoint 200m ahead. This
prevents sudden turns if we overshoot the landing point
*/
struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc,
land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
// once landed and stationary, post some statistics
// this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
if (auto_state.post_landing_stats && !arming.is_armed()) {
auto_state.post_landing_stats = false;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
}
// check if we should auto-disarm after a confirmed landing
disarm_if_autoland_complete();
/*
we return false as a landing mission item never completes
we stay on this waypoint unless the GCS commands us to change
mission item, reset the mission, command a go-around or finish
a land_abort procedure.
*/
return false;
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
return;
}
uint32_t now_ms = hal.scheduler->millis();
bool auto_launch_detected;
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying())
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);
if (been_auto_flying || // failed hand launch
auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches
// has launched but is no longer flying. That's a crash on takeoff.
crashed = true;
}
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (been_auto_flying) {
crashed = true;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (been_auto_flying) {
crashed = true;
}
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However,
// a crash into a tree, for example, would be.
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (been_auto_flying &&
!crash_state.checkHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms + 2500;
}
crash_state.checkHardLanding = true;
break;
} // switch
} else {
crash_state.checkHardLanding = false;
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken"));
} else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken"));
}
}
else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
auto_state.land_complete = true;
if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected"));
} else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected"));
}
}
}
}
