You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
822 lines
26 KiB
822 lines
26 KiB
/* |
|
Lead developer: Andrew Tridgell |
|
|
|
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger, Tom Pittenger |
|
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon |
|
|
|
Please contribute your ideas! See https://ardupilot.org/dev for details |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#include "Plane.h" |
|
|
|
#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority) |
|
|
|
|
|
/* |
|
scheduler table - all regular tasks should be listed here. |
|
|
|
All entries in this table must be ordered by priority. |
|
|
|
This table is interleaved with the table presnet in each of the |
|
vehicles to determine the order in which tasks are run. Convenience |
|
methods SCHED_TASK and SCHED_TASK_CLASS are provided to build |
|
entries in this structure: |
|
|
|
SCHED_TASK arguments: |
|
- name of static function to call |
|
- rate (in Hertz) at which the function should be called |
|
- expected time (in MicroSeconds) that the function should take to run |
|
- priority (0 through 255, lower number meaning higher priority) |
|
|
|
SCHED_TASK_CLASS arguments: |
|
- class name of method to be called |
|
- instance on which to call the method |
|
- method to call on that instance |
|
- rate (in Hertz) at which the method should be called |
|
- expected time (in MicroSeconds) that the method should take to run |
|
- priority (0 through 255, lower number meaning higher priority) |
|
|
|
*/ |
|
const AP_Scheduler::Task Plane::scheduler_tasks[] = { |
|
// Units: Hz us |
|
SCHED_TASK(ahrs_update, 400, 400, 3), |
|
SCHED_TASK(read_radio, 50, 100, 6), |
|
SCHED_TASK(check_short_failsafe, 50, 100, 9), |
|
SCHED_TASK(update_speed_height, 50, 200, 12), |
|
SCHED_TASK(update_control_mode, 400, 100, 15), |
|
SCHED_TASK(stabilize, 400, 100, 18), |
|
SCHED_TASK(set_servos, 400, 100, 21), |
|
SCHED_TASK(update_throttle_hover, 100, 90, 24), |
|
SCHED_TASK(read_control_switch, 7, 100, 27), |
|
SCHED_TASK(update_GPS_50Hz, 50, 300, 30), |
|
SCHED_TASK(update_GPS_10Hz, 10, 400, 33), |
|
SCHED_TASK(navigate, 10, 150, 36), |
|
SCHED_TASK(update_compass, 10, 200, 39), |
|
SCHED_TASK(calc_airspeed_errors, 10, 100, 42), |
|
SCHED_TASK(update_alt, 10, 200, 45), |
|
SCHED_TASK(adjust_altitude_target, 10, 200, 48), |
|
#if ADVANCED_FAILSAFE == ENABLED |
|
SCHED_TASK(afs_fs_check, 10, 100, 51), |
|
#endif |
|
SCHED_TASK(ekf_check, 10, 75, 54), |
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57), |
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60), |
|
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63), |
|
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66), |
|
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69), |
|
SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72), |
|
SCHED_TASK(read_rangefinder, 50, 100, 78), |
|
#if AP_ICENGINE_ENABLED |
|
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), |
|
#endif |
|
SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50, 84), |
|
#if AP_OPTICALFLOW_ENABLED |
|
SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50, 87), |
|
#endif |
|
SCHED_TASK(one_second_loop, 1, 400, 90), |
|
SCHED_TASK(three_hz_loop, 3, 75, 93), |
|
SCHED_TASK(check_long_failsafe, 3, 400, 96), |
|
SCHED_TASK_CLASS(AP_RPM, &plane.rpm_sensor, update, 10, 100, 99), |
|
#if AP_AIRSPEED_AUTOCAL_ENABLE |
|
SCHED_TASK(airspeed_ratio_update, 1, 100, 102), |
|
#endif // AP_AIRSPEED_AUTOCAL_ENABLE |
|
#if HAL_MOUNT_ENABLED |
|
SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105), |
|
#endif // HAL_MOUNT_ENABLED |
|
#if CAMERA == ENABLED |
|
SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108), |
|
#endif // CAMERA == ENABLED |
|
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111), |
|
SCHED_TASK(compass_save, 0.1, 200, 114), |
|
SCHED_TASK(Log_Write_FullRate, 400, 300, 117), |
|
SCHED_TASK(update_logging10, 10, 300, 120), |
|
SCHED_TASK(update_logging25, 25, 300, 123), |
|
#if HAL_SOARING_ENABLED |
|
SCHED_TASK(update_soaring, 50, 400, 126), |
|
#endif |
|
SCHED_TASK(parachute_check, 10, 200, 129), |
|
#if AP_TERRAIN_AVAILABLE |
|
SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132), |
|
#endif // AP_TERRAIN_AVAILABLE |
|
SCHED_TASK(update_is_flying_5Hz, 5, 100, 135), |
|
#if LOGGING_ENABLED == ENABLED |
|
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138), |
|
#endif |
|
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141), |
|
#if HAL_ADSB_ENABLED |
|
SCHED_TASK(avoidance_adsb_update, 10, 100, 144), |
|
#endif |
|
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147), |
|
#if HAL_BUTTON_ENABLED |
|
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150), |
|
#endif |
|
#if STATS_ENABLED == ENABLED |
|
SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153), |
|
#endif |
|
#if GRIPPER_ENABLED == ENABLED |
|
SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75, 156), |
|
#endif |
|
#if LANDING_GEAR_ENABLED == ENABLED |
|
SCHED_TASK(landing_gear_update, 5, 50, 159), |
|
#endif |
|
}; |
|
|
|
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, |
|
uint8_t &task_count, |
|
uint32_t &log_bit) |
|
{ |
|
tasks = &scheduler_tasks[0]; |
|
task_count = ARRAY_SIZE(scheduler_tasks); |
|
log_bit = MASK_LOG_PM; |
|
} |
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
constexpr int8_t Plane::_failsafe_priorities[7]; |
|
#else |
|
constexpr int8_t Plane::_failsafe_priorities[6]; |
|
#endif |
|
|
|
// update AHRS system |
|
void Plane::ahrs_update() |
|
{ |
|
arming.update_soft_armed(); |
|
|
|
ahrs.update(); |
|
|
|
if (should_log(MASK_LOG_IMU)) { |
|
AP::ins().Write_IMU(); |
|
} |
|
|
|
// calculate a scaled roll limit based on current pitch |
|
roll_limit_cd = aparm.roll_limit_cd; |
|
pitch_limit_min_cd = aparm.pitch_limit_min_cd; |
|
|
|
bool rotate_limits = true; |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.tailsitter.active()) { |
|
rotate_limits = false; |
|
} |
|
#endif |
|
if (rotate_limits) { |
|
roll_limit_cd *= ahrs.cos_pitch(); |
|
pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); |
|
} |
|
|
|
// updated the summed gyro used for ground steering and |
|
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth |
|
// frame yaw rate |
|
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; |
|
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); |
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
// check if we have had a yaw reset from the EKF |
|
quadplane.check_yaw_reset(); |
|
|
|
// update inertial_nav for quadplane |
|
quadplane.inertial_nav.update(); |
|
#endif |
|
|
|
if (should_log(MASK_LOG_VIDEO_STABILISATION)) { |
|
ahrs.write_video_stabilisation(); |
|
} |
|
} |
|
|
|
/* |
|
update 50Hz speed/height controller |
|
*/ |
|
void Plane::update_speed_height(void) |
|
{ |
|
if (control_mode->does_auto_throttle()) { |
|
// Call TECS 50Hz update. Note that we call this regardless of |
|
// throttle suppressed, as this needs to be running for |
|
// takeoff detection |
|
TECS_controller.update_50hz(); |
|
} |
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode() || |
|
quadplane.in_assisted_flight()) { |
|
quadplane.update_throttle_mix(); |
|
} |
|
#endif |
|
} |
|
|
|
|
|
/* |
|
read and update compass |
|
*/ |
|
void Plane::update_compass(void) |
|
{ |
|
compass.read(); |
|
} |
|
|
|
/* |
|
do 10Hz logging |
|
*/ |
|
void Plane::update_logging10(void) |
|
{ |
|
bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST)); |
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) { |
|
Log_Write_Attitude(); |
|
ahrs.Write_AOA_SSA(); |
|
} else if (log_faster) { |
|
ahrs.Write_AOA_SSA(); |
|
} |
|
} |
|
|
|
/* |
|
do 25Hz logging |
|
*/ |
|
void Plane::update_logging25(void) |
|
{ |
|
// MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz |
|
// highest rate selected wins |
|
bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE); |
|
if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) { |
|
Log_Write_Attitude(); |
|
} |
|
|
|
if (should_log(MASK_LOG_CTUN)) { |
|
Log_Write_Control_Tuning(); |
|
AP::ins().write_notch_log_messages(); |
|
#if HAL_GYROFFT_ENABLED |
|
gyro_fft.write_log_messages(); |
|
#endif |
|
} |
|
|
|
if (should_log(MASK_LOG_NTUN)) { |
|
Log_Write_Nav_Tuning(); |
|
Log_Write_Guided(); |
|
} |
|
|
|
if (should_log(MASK_LOG_RC)) |
|
Log_Write_RC(); |
|
|
|
if (should_log(MASK_LOG_IMU)) |
|
AP::ins().Write_Vibration(); |
|
} |
|
|
|
|
|
/* |
|
check for AFS failsafe check |
|
*/ |
|
#if ADVANCED_FAILSAFE == ENABLED |
|
void Plane::afs_fs_check(void) |
|
{ |
|
// perform AFS failsafe checks |
|
#if AP_FENCE_ENABLED |
|
const bool fence_breached = fence.get_breaches() != 0; |
|
#else |
|
const bool fence_breached = false; |
|
#endif |
|
afs.check(fence_breached, failsafe.AFS_last_valid_rc_ms); |
|
} |
|
#endif |
|
|
|
#if HAL_WITH_IO_MCU |
|
#include <AP_IOMCU/AP_IOMCU.h> |
|
extern AP_IOMCU iomcu; |
|
#endif |
|
|
|
void Plane::one_second_loop() |
|
{ |
|
// make it possible to change control channel ordering at runtime |
|
set_control_channels(); |
|
|
|
#if HAL_WITH_IO_MCU |
|
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); |
|
#endif |
|
|
|
#if HAL_ADSB_ENABLED |
|
adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s |
|
adsb.set_max_speed(aparm.airspeed_max); |
|
#endif |
|
|
|
if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) { |
|
// use average of min and max airspeed as default airspeed fusion with high variance |
|
ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2), |
|
(float)((aparm.airspeed_max - aparm.airspeed_min)/2)); |
|
} |
|
|
|
// sync MAVLink system ID |
|
mavlink_system.sysid = g.sysid_this_mav; |
|
|
|
SRV_Channels::enable_aux_servos(); |
|
|
|
// update notify flags |
|
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); |
|
AP_Notify::flags.pre_arm_gps_check = true; |
|
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO; |
|
|
|
#if AP_TERRAIN_AVAILABLE |
|
if (should_log(MASK_LOG_GPS)) { |
|
terrain.log_terrain_data(); |
|
} |
|
#endif |
|
|
|
// update home position if NOT armed and gps position has |
|
// changed. Update every 5s at most |
|
if (!arming.is_armed() && |
|
gps.last_message_time_ms() - last_home_update_ms > 5000 && |
|
gps.status() >= AP_GPS::GPS_OK_FIX_3D) { |
|
last_home_update_ms = gps.last_message_time_ms(); |
|
update_home(); |
|
|
|
// reset the landing altitude correction |
|
landing.alt_offset = 0; |
|
} |
|
|
|
// this ensures G_Dt is correct, catching startup issues with constructors |
|
// calling the scheduler methods |
|
if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) || |
|
!is_equal(G_Dt, scheduler.get_loop_period_s())) { |
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); |
|
} |
|
} |
|
|
|
void Plane::three_hz_loop() |
|
{ |
|
#if AP_FENCE_ENABLED |
|
fence_check(); |
|
#endif |
|
} |
|
|
|
void Plane::compass_save() |
|
{ |
|
if (AP::compass().available() && |
|
compass.get_learn_type() >= Compass::LEARN_INTERNAL && |
|
!hal.util->get_soft_armed()) { |
|
/* |
|
only save offsets when disarmed |
|
*/ |
|
compass.save_offsets(); |
|
} |
|
} |
|
|
|
#if AP_AIRSPEED_AUTOCAL_ENABLE |
|
/* |
|
once a second update the airspeed calibration ratio |
|
*/ |
|
void Plane::airspeed_ratio_update(void) |
|
{ |
|
if (!airspeed.enabled() || |
|
gps.status() < AP_GPS::GPS_OK_FIX_3D || |
|
gps.ground_speed() < 4) { |
|
// don't calibrate when not moving |
|
return; |
|
} |
|
if (airspeed.get_airspeed() < aparm.airspeed_min && |
|
gps.ground_speed() < (uint32_t)aparm.airspeed_min) { |
|
// don't calibrate when flying below the minimum airspeed. We |
|
// check both airspeed and ground speed to catch cases where |
|
// the airspeed ratio is way too low, which could lead to it |
|
// never coming up again |
|
return; |
|
} |
|
if (labs(ahrs.roll_sensor) > roll_limit_cd || |
|
ahrs.pitch_sensor > aparm.pitch_limit_max_cd || |
|
ahrs.pitch_sensor < pitch_limit_min_cd) { |
|
// don't calibrate when going beyond normal flight envelope |
|
return; |
|
} |
|
const Vector3f &vg = gps.velocity(); |
|
airspeed.update_calibration(vg, aparm.airspeed_max); |
|
} |
|
#endif // AP_AIRSPEED_AUTOCAL_ENABLE |
|
|
|
/* |
|
read the GPS and update position |
|
*/ |
|
void Plane::update_GPS_50Hz(void) |
|
{ |
|
gps.update(); |
|
|
|
// get position from AHRS |
|
have_position = ahrs.get_location(current_loc); |
|
ahrs.get_relative_position_D_home(relative_altitude); |
|
relative_altitude *= -1.0f; |
|
} |
|
|
|
/* |
|
read update GPS position - 10Hz update |
|
*/ |
|
void Plane::update_GPS_10Hz(void) |
|
{ |
|
static uint32_t last_gps_msg_ms; |
|
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { |
|
last_gps_msg_ms = gps.last_message_time_ms(); |
|
|
|
if (ground_start_count > 1) { |
|
ground_start_count--; |
|
} else if (ground_start_count == 1) { |
|
// We countdown N number of good GPS fixes |
|
// so that the altitude is more accurate |
|
// ------------------------------------- |
|
if (current_loc.lat == 0 && current_loc.lng == 0) { |
|
ground_start_count = 5; |
|
|
|
} else if (!hal.util->was_watchdog_reset()) { |
|
if (!set_home_persistently(gps.location())) { |
|
// silently ignore failure... |
|
} |
|
|
|
next_WP_loc = prev_WP_loc = home; |
|
|
|
ground_start_count = 0; |
|
} |
|
} |
|
|
|
// update wind estimate |
|
ahrs.estimate_wind(); |
|
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) { |
|
// lost 3D fix, start again |
|
ground_start_count = 5; |
|
} |
|
|
|
calc_gndspeed_undershoot(); |
|
} |
|
|
|
/* |
|
main control mode dependent update code |
|
*/ |
|
void Plane::update_control_mode(void) |
|
{ |
|
if (control_mode != &mode_auto) { |
|
// hold_course is only used in takeoff and landing |
|
steer_state.hold_course_cd = -1; |
|
} |
|
|
|
update_fly_forward(); |
|
|
|
control_mode->update(); |
|
} |
|
|
|
|
|
void Plane::update_fly_forward(void) |
|
{ |
|
// ensure we are fly-forward when we are flying as a pure fixed |
|
// wing aircraft. This helps the EKF produce better state |
|
// estimates as it can make stronger assumptions |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.available() && |
|
quadplane.tailsitter.is_in_fw_flight()) { |
|
ahrs.set_fly_forward(true); |
|
return; |
|
} |
|
|
|
if (quadplane.in_vtol_mode() || |
|
quadplane.in_assisted_flight()) { |
|
ahrs.set_fly_forward(false); |
|
return; |
|
} |
|
#endif |
|
|
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { |
|
ahrs.set_fly_forward(landing.is_flying_forward()); |
|
return; |
|
} |
|
|
|
ahrs.set_fly_forward(true); |
|
} |
|
|
|
/* |
|
set the flight stage |
|
*/ |
|
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) |
|
{ |
|
if (fs == flight_stage) { |
|
return; |
|
} |
|
|
|
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND); |
|
|
|
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { |
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", |
|
int(auto_state.takeoff_altitude_rel_cm/100)); |
|
} |
|
|
|
flight_stage = fs; |
|
Log_Write_Status(); |
|
} |
|
|
|
void Plane::update_alt() |
|
{ |
|
barometer.update(); |
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.available()) { |
|
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio()); |
|
} |
|
#endif |
|
|
|
// calculate the sink rate. |
|
float sink_rate; |
|
Vector3f vel; |
|
if (ahrs.get_velocity_NED(vel)) { |
|
sink_rate = vel.z; |
|
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) { |
|
sink_rate = gps.velocity().z; |
|
} else { |
|
sink_rate = -barometer.get_climb_rate(); |
|
} |
|
|
|
// low pass the sink rate to take some of the noise out |
|
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; |
|
#if PARACHUTE == ENABLED |
|
parachute.set_sink_rate(auto_state.sink_rate); |
|
#endif |
|
|
|
update_flight_stage(); |
|
|
|
if (control_mode->does_auto_throttle() && !throttle_suppressed) { |
|
|
|
float distance_beyond_land_wp = 0; |
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { |
|
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc); |
|
} |
|
|
|
float target_alt = relative_target_altitude_cm(); |
|
|
|
if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) { |
|
// ensure we do the initial climb in RTL. We add an extra |
|
// 10m in the demanded height to push TECS to climb |
|
// quickly |
|
target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100; |
|
} |
|
|
|
TECS_controller.update_pitch_throttle(target_alt, |
|
target_airspeed_cm, |
|
flight_stage, |
|
distance_beyond_land_wp, |
|
get_takeoff_pitch_min_cd(), |
|
throttle_nudge, |
|
tecs_hgt_afe(), |
|
aerodynamic_load_factor); |
|
} |
|
} |
|
|
|
/* |
|
recalculate the flight_stage |
|
*/ |
|
void Plane::update_flight_stage(void) |
|
{ |
|
// Update the speed & height controller states |
|
if (control_mode->does_auto_throttle() && !throttle_suppressed) { |
|
if (control_mode == &mode_auto) { |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_auto()) { |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); |
|
return; |
|
} |
|
#endif |
|
if (auto_state.takeoff_complete == false) { |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF); |
|
return; |
|
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { |
|
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { |
|
// abort mode is sticky, it must complete while executing NAV_LAND |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); |
|
} else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 && |
|
landing.request_go_around()) { |
|
gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); |
|
} else { |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); |
|
} |
|
return; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_assisted_flight()) { |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); |
|
return; |
|
} |
|
#endif |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); |
|
} else if (control_mode != &mode_takeoff) { |
|
// If not in AUTO then assume normal operation for normal TECS operation. |
|
// This prevents TECS from being stuck in the wrong stage if you switch from |
|
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff. |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); |
|
} |
|
return; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode() || |
|
quadplane.in_assisted_flight()) { |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); |
|
return; |
|
} |
|
#endif |
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); |
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
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::Required::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 (arming.disarm(AP_Arming::Method::AUTOLANDED)) { |
|
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed"); |
|
} |
|
} |
|
} |
|
} |
|
|
|
|
|
|
|
/* |
|
the height above field elevation that we pass to TECS |
|
*/ |
|
float Plane::tecs_hgt_afe(void) |
|
{ |
|
/* |
|
pass the height above field elevation as the height above |
|
the ground when in landing, which means that TECS gets the |
|
rangefinder information and thus can know when the flare is |
|
coming. |
|
*/ |
|
float hgt_afe; |
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { |
|
hgt_afe = height_above_target(); |
|
hgt_afe -= rangefinder_correction(); |
|
} else { |
|
// when in normal flight we pass the hgt_afe as relative |
|
// altitude to home |
|
hgt_afe = relative_altitude; |
|
} |
|
return hgt_afe; |
|
} |
|
|
|
// vehicle specific waypoint info helpers |
|
bool Plane::get_wp_distance_m(float &distance) const |
|
{ |
|
// see GCS_MAVLINK_Plane::send_nav_controller_output() |
|
if (control_mode == &mode_manual) { |
|
return false; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode()) { |
|
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() * 0.01 : 0; |
|
return true; |
|
} |
|
#endif |
|
distance = auto_state.wp_distance; |
|
return true; |
|
} |
|
|
|
bool Plane::get_wp_bearing_deg(float &bearing) const |
|
{ |
|
// see GCS_MAVLINK_Plane::send_nav_controller_output() |
|
if (control_mode == &mode_manual) { |
|
return false; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode()) { |
|
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0; |
|
return true; |
|
} |
|
#endif |
|
bearing = nav_controller->target_bearing_cd() * 0.01; |
|
return true; |
|
} |
|
|
|
bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const |
|
{ |
|
// see GCS_MAVLINK_Plane::send_nav_controller_output() |
|
if (control_mode == &mode_manual) { |
|
return false; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode()) { |
|
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0; |
|
return true; |
|
} |
|
#endif |
|
xtrack_error = nav_controller->crosstrack_error(); |
|
return true; |
|
} |
|
|
|
#if AP_SCRIPTING_ENABLED |
|
// set target location (for use by scripting) |
|
bool Plane::set_target_location(const Location &target_loc) |
|
{ |
|
Location loc{target_loc}; |
|
|
|
if (plane.control_mode != &plane.mode_guided) { |
|
// only accept position updates when in GUIDED mode |
|
return false; |
|
} |
|
// add home alt if needed |
|
if (loc.relative_alt) { |
|
loc.alt += plane.home.alt; |
|
loc.relative_alt = 0; |
|
} |
|
plane.set_guided_WP(loc); |
|
return true; |
|
} |
|
|
|
// set target location (for use by scripting) |
|
bool Plane::get_target_location(Location& target_loc) |
|
{ |
|
switch (control_mode->mode_number()) { |
|
case Mode::Number::RTL: |
|
case Mode::Number::AVOID_ADSB: |
|
case Mode::Number::GUIDED: |
|
case Mode::Number::AUTO: |
|
case Mode::Number::LOITER: |
|
case Mode::Number::TAKEOFF: |
|
#if HAL_QUADPLANE_ENABLED |
|
case Mode::Number::QLOITER: |
|
case Mode::Number::QLAND: |
|
case Mode::Number::QRTL: |
|
#endif |
|
target_loc = next_WP_loc; |
|
return true; |
|
break; |
|
default: |
|
break; |
|
} |
|
return false; |
|
} |
|
|
|
/* |
|
update_target_location() works in all auto navigation modes |
|
*/ |
|
bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) |
|
{ |
|
if (!old_loc.same_latlon_as(next_WP_loc)) { |
|
return false; |
|
} |
|
ftype alt_diff; |
|
if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) || |
|
!is_zero(alt_diff)) { |
|
return false; |
|
} |
|
next_WP_loc = new_loc; |
|
next_WP_loc.change_alt_frame(old_loc.get_alt_frame()); |
|
|
|
return true; |
|
} |
|
|
|
// allow for velocity matching in VTOL |
|
bool Plane::set_velocity_match(const Vector2f &velocity) |
|
{ |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) { |
|
quadplane.poscontrol.velocity_match = velocity; |
|
quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis(); |
|
return true; |
|
} |
|
#endif |
|
return false; |
|
} |
|
|
|
#endif // AP_SCRIPTING_ENABLED |
|
|
|
#if OSD_ENABLED |
|
// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL |
|
void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const |
|
{ |
|
pitch = ahrs.pitch; |
|
roll = ahrs.roll; |
|
#if HAL_QUADPLANE_ENABLED |
|
if (quadplane.show_vtol_view()) { |
|
return; |
|
} |
|
#endif |
|
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD |
|
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; |
|
return; |
|
} |
|
#if HAL_QUADPLANE_ENABLED |
|
pitch = quadplane.ahrs_view->pitch; |
|
roll = quadplane.ahrs_view->roll; |
|
#endif |
|
} |
|
#endif |
|
|
|
AP_HAL_MAIN_CALLBACKS(&plane);
|
|
|