mirror of https://github.com/ArduPilot/ardupilot
822 lines
26 KiB
C++
822 lines
26 KiB
C++
/*
|
|
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
|
|
#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);
|