2015-05-13 03:09:36 -03:00
|
|
|
/*
|
|
|
|
Lead developer: Andrew Tridgell
|
|
|
|
|
2017-11-06 08:46:32 -04:00
|
|
|
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
|
2015-05-13 03:09:36 -03:00
|
|
|
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
|
|
|
|
|
2019-12-08 17:22:11 -04:00
|
|
|
Please contribute your ideas! See https://dev.ardupilot.org for details
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
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"
|
|
|
|
|
2015-11-30 15:59:28 -04:00
|
|
|
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros)
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
scheduler table - all regular tasks are listed here, along with how
|
2016-02-12 16:14:38 -04:00
|
|
|
often they should be called (in Hz) and the maximum time
|
2015-05-13 03:09:36 -03:00
|
|
|
they are expected to take (in microseconds)
|
|
|
|
*/
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Scheduler::Task Plane::scheduler_tasks[] = {
|
2016-02-12 16:14:38 -04:00
|
|
|
// Units: Hz us
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(ahrs_update, 400, 400),
|
|
|
|
SCHED_TASK(read_radio, 50, 100),
|
|
|
|
SCHED_TASK(check_short_failsafe, 50, 100),
|
|
|
|
SCHED_TASK(update_speed_height, 50, 200),
|
2019-01-15 13:46:13 -04:00
|
|
|
SCHED_TASK(update_control_mode, 400, 100),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(stabilize, 400, 100),
|
|
|
|
SCHED_TASK(set_servos, 400, 100),
|
2018-04-10 19:07:24 -03:00
|
|
|
SCHED_TASK(update_throttle_hover, 100, 90),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(read_control_switch, 7, 100),
|
|
|
|
SCHED_TASK(update_GPS_50Hz, 50, 300),
|
|
|
|
SCHED_TASK(update_GPS_10Hz, 10, 400),
|
|
|
|
SCHED_TASK(navigate, 10, 150),
|
|
|
|
SCHED_TASK(update_compass, 10, 200),
|
|
|
|
SCHED_TASK(read_airspeed, 10, 100),
|
|
|
|
SCHED_TASK(update_alt, 10, 200),
|
|
|
|
SCHED_TASK(adjust_altitude_target, 10, 200),
|
2019-01-08 21:05:40 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2016-07-22 04:45:05 -03:00
|
|
|
SCHED_TASK(afs_fs_check, 10, 100),
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif
|
2020-05-04 20:12:14 -03:00
|
|
|
SCHED_TASK(ekf_check, 10, 75),
|
2018-12-06 18:57:31 -04:00
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500),
|
2020-05-22 21:31:03 -03:00
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750),
|
2018-11-22 22:54:22 -04:00
|
|
|
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150),
|
2017-11-09 18:34:12 -04:00
|
|
|
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
|
2018-06-26 18:47:03 -03:00
|
|
|
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150),
|
2019-04-11 08:03:30 -03:00
|
|
|
SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(read_rangefinder, 50, 100),
|
2018-07-03 06:16:33 -03:00
|
|
|
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
|
2019-03-25 06:24:08 -03:00
|
|
|
SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(accel_cal_update, 10, 50),
|
2015-05-13 03:09:36 -03:00
|
|
|
#if OPTFLOW == ENABLED
|
2018-09-02 09:26:19 -03:00
|
|
|
SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50),
|
2015-05-13 03:09:36 -03:00
|
|
|
#endif
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(one_second_loop, 1, 400),
|
|
|
|
SCHED_TASK(check_long_failsafe, 3, 400),
|
|
|
|
SCHED_TASK(rpm_update, 10, 100),
|
2020-11-24 10:13:10 -04:00
|
|
|
#if AP_AIRSPEED_AUTOCAL_ENABLE
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(airspeed_ratio_update, 1, 100),
|
2020-11-24 10:13:10 -04:00
|
|
|
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
|
2020-07-24 14:23:23 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2018-06-26 18:47:03 -03:00
|
|
|
SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100),
|
2020-07-24 14:23:23 -03:00
|
|
|
#endif // HAL_MOUNT_ENABLED
|
2018-06-26 18:47:03 -03:00
|
|
|
#if CAMERA == ENABLED
|
2020-06-22 17:33:55 -03:00
|
|
|
SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100),
|
2018-06-26 18:47:03 -03:00
|
|
|
#endif // CAMERA == ENABLED
|
2018-02-12 17:08:28 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100),
|
2016-04-30 02:05:44 -03:00
|
|
|
SCHED_TASK(compass_save, 0.1, 200),
|
2016-05-01 19:51:13 -03:00
|
|
|
SCHED_TASK(Log_Write_Fast, 25, 300),
|
2017-04-08 01:02:23 -03:00
|
|
|
SCHED_TASK(update_logging1, 25, 300),
|
|
|
|
SCHED_TASK(update_logging2, 25, 300),
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2017-02-26 19:18:38 -04:00
|
|
|
SCHED_TASK(update_soaring, 50, 400),
|
2018-07-29 19:57:55 -03:00
|
|
|
#endif
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(parachute_check, 10, 200),
|
2018-06-26 18:47:03 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200),
|
|
|
|
#endif // AP_TERRAIN_AVAILABLE
|
2015-11-30 15:59:28 -04:00
|
|
|
SCHED_TASK(update_is_flying_5Hz, 5, 100),
|
2018-04-20 03:42:37 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2019-01-18 00:23:42 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400),
|
2018-04-20 03:42:37 -03:00
|
|
|
#endif
|
2018-06-26 18:47:03 -03:00
|
|
|
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50),
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-08-12 18:28:21 -03:00
|
|
|
SCHED_TASK(avoidance_adsb_update, 10, 100),
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2018-11-22 01:39:21 -04:00
|
|
|
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200),
|
2019-11-27 18:46:58 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100),
|
2018-02-28 08:22:16 -04:00
|
|
|
#if STATS_ENABLED == ENABLED
|
2018-06-26 18:47:03 -03:00
|
|
|
SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100),
|
2018-02-28 08:22:16 -04:00
|
|
|
#endif
|
2018-02-22 05:13:38 -04:00
|
|
|
#if GRIPPER_ENABLED == ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75),
|
|
|
|
#endif
|
2018-06-10 03:33:06 -03:00
|
|
|
#if LANDING_GEAR_ENABLED == ENABLED
|
2018-11-09 00:40:20 -04:00
|
|
|
SCHED_TASK(landing_gear_update, 5, 50),
|
2018-06-10 03:33:06 -03:00
|
|
|
#endif
|
2019-11-09 04:55:05 -04:00
|
|
|
#if EFI_ENABLED
|
2019-11-17 01:06:34 -04:00
|
|
|
SCHED_TASK(efi_update, 10, 200),
|
2019-11-09 04:55:05 -04:00
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
};
|
|
|
|
|
2020-01-16 06:31:56 -04:00
|
|
|
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit)
|
2015-05-13 03:09:36 -03:00
|
|
|
{
|
2020-01-16 06:31:56 -04:00
|
|
|
tasks = &scheduler_tasks[0];
|
|
|
|
task_count = ARRAY_SIZE(scheduler_tasks);
|
|
|
|
log_bit = MASK_LOG_PM;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
2020-01-16 06:31:56 -04:00
|
|
|
constexpr int8_t Plane::_failsafe_priorities[7];
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// update AHRS system
|
|
|
|
void Plane::ahrs_update()
|
|
|
|
{
|
2019-05-03 20:35:12 -03:00
|
|
|
arming.update_soft_armed();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2015-05-13 03:09:36 -03:00
|
|
|
if (g.hil_mode == 1) {
|
|
|
|
// update hil before AHRS update
|
2018-11-21 23:04:44 -04:00
|
|
|
gcs().update_receive();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
ahrs.update();
|
|
|
|
|
2015-07-13 08:34:33 -03:00
|
|
|
if (should_log(MASK_LOG_IMU)) {
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_IMU();
|
2015-07-13 08:34:33 -03:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// calculate a scaled roll limit based on current pitch
|
2017-02-11 04:12:56 -04:00
|
|
|
roll_limit_cd = aparm.roll_limit_cd;
|
|
|
|
pitch_limit_min_cd = aparm.pitch_limit_min_cd;
|
|
|
|
|
|
|
|
if (!quadplane.tailsitter_active()) {
|
|
|
|
roll_limit_cd *= ahrs.cos_pitch();
|
|
|
|
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// 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);
|
2015-12-26 03:45:42 -04:00
|
|
|
|
2019-01-13 00:24:51 -04:00
|
|
|
// check if we have had a yaw reset from the EKF
|
|
|
|
quadplane.check_yaw_reset();
|
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
// update inertial_nav for quadplane
|
2019-06-28 18:31:44 -03:00
|
|
|
quadplane.inertial_nav.update();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update 50Hz speed/height controller
|
|
|
|
*/
|
|
|
|
void Plane::update_speed_height(void)
|
|
|
|
{
|
2020-12-23 01:25:35 -04:00
|
|
|
if (control_mode->does_auto_throttle()) {
|
2015-05-13 03:09:36 -03:00
|
|
|
// Call TECS 50Hz update. Note that we call this regardless of
|
|
|
|
// throttle suppressed, as this needs to be running for
|
|
|
|
// takeoff detection
|
2016-04-21 03:44:34 -03:00
|
|
|
SpdHgt_Controller->update_50hz();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2019-01-26 19:30:53 -04:00
|
|
|
|
|
|
|
if (quadplane.in_vtol_mode() ||
|
|
|
|
quadplane.in_assisted_flight()) {
|
2020-02-21 02:51:36 -04:00
|
|
|
quadplane.update_throttle_mix();
|
2019-01-26 19:30:53 -04:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
read and update compass
|
|
|
|
*/
|
|
|
|
void Plane::update_compass(void)
|
|
|
|
{
|
2019-03-24 00:27:03 -03:00
|
|
|
if (AP::compass().enabled() && compass.read()) {
|
2015-05-13 03:09:36 -03:00
|
|
|
ahrs.set_compass(&compass);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
do 10Hz logging
|
|
|
|
*/
|
|
|
|
void Plane::update_logging1(void)
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
Log_Write_Attitude();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_IMU();
|
2017-04-09 08:16:50 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED))
|
2021-01-09 04:06:40 -04:00
|
|
|
ahrs.Write_AOA_SSA();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
do 10Hz logging - part2
|
|
|
|
*/
|
|
|
|
void Plane::update_logging2(void)
|
|
|
|
{
|
2020-01-01 16:37:35 -04:00
|
|
|
if (should_log(MASK_LOG_CTUN)) {
|
2015-05-13 03:09:36 -03:00
|
|
|
Log_Write_Control_Tuning();
|
2020-01-01 16:37:35 -04:00
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
gyro_fft.write_log_messages();
|
2020-05-29 13:31:46 -03:00
|
|
|
#else
|
|
|
|
write_notch_log_messages();
|
2020-01-01 16:37:35 -04:00
|
|
|
#endif
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2020-05-16 01:02:01 -03:00
|
|
|
if (should_log(MASK_LOG_NTUN)) {
|
2015-05-13 03:09:36 -03:00
|
|
|
Log_Write_Nav_Tuning();
|
2020-05-16 01:02:01 -03:00
|
|
|
Log_Write_Guided();
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_RC))
|
|
|
|
Log_Write_RC();
|
2015-06-12 04:00:06 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_IMU))
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_Vibration();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
2016-07-22 04:45:05 -03:00
|
|
|
check for AFS failsafe check
|
2015-05-13 03:09:36 -03:00
|
|
|
*/
|
2019-01-08 21:05:40 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2016-07-22 04:45:05 -03:00
|
|
|
void Plane::afs_fs_check(void)
|
2015-05-13 03:09:36 -03:00
|
|
|
{
|
2016-07-22 04:45:05 -03:00
|
|
|
// perform AFS failsafe checks
|
2016-07-22 05:14:53 -03:00
|
|
|
afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2018-10-30 21:08:07 -03:00
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
#include <AP_IOMCU/AP_IOMCU.h>
|
|
|
|
extern AP_IOMCU iomcu;
|
|
|
|
#endif
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::one_second_loop()
|
|
|
|
{
|
|
|
|
// make it possible to change control channel ordering at runtime
|
|
|
|
set_control_channels();
|
|
|
|
|
2018-10-30 21:08:07 -03:00
|
|
|
#if HAL_WITH_IO_MCU
|
2018-10-31 01:10:07 -03:00
|
|
|
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
|
2018-10-30 21:08:07 -03:00
|
|
|
#endif
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// make it possible to change orientation at runtime
|
2019-02-17 00:54:01 -04:00
|
|
|
ahrs.update_orientation();
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-08-07 21:48:36 -03:00
|
|
|
adsb.set_stall_speed_cm(aparm.airspeed_min);
|
2018-05-22 14:34:26 -03:00
|
|
|
adsb.set_max_speed(aparm.airspeed_max);
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2020-03-10 03:50:05 -03:00
|
|
|
ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2));
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// sync MAVLink system ID
|
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
2018-08-22 04:17:41 -03:00
|
|
|
SRV_Channels::enable_aux_servos();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// update notify flags
|
|
|
|
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
|
|
|
|
AP_Notify::flags.pre_arm_gps_check = true;
|
2019-03-06 23:12:56 -04:00
|
|
|
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
if (should_log(MASK_LOG_GPS)) {
|
2019-01-17 19:17:28 -04:00
|
|
|
terrain.log_terrain_data();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2019-03-31 21:18:16 -03:00
|
|
|
// update home position if NOT armed and gps position has
|
2016-10-13 20:58:51 -03:00
|
|
|
// changed. Update every 5s at most
|
2017-11-15 23:35:10 -04:00
|
|
|
if (!arming.is_armed() &&
|
2016-10-13 20:58:51 -03:00
|
|
|
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
|
2016-11-16 21:12:26 -04:00
|
|
|
landing.alt_offset = 0;
|
2016-10-13 20:58:51 -03:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::compass_save()
|
|
|
|
{
|
2019-03-24 00:27:03 -03:00
|
|
|
if (AP::compass().enabled() &&
|
2016-04-30 02:05:44 -03:00
|
|
|
compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
|
|
|
|
!hal.util->get_soft_armed()) {
|
|
|
|
/*
|
|
|
|
only save offsets when disarmed
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
compass.save_offsets();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-11-09 04:55:05 -04:00
|
|
|
void Plane::efi_update(void)
|
|
|
|
{
|
|
|
|
#if EFI_ENABLED
|
|
|
|
g2.efi.update();
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2020-11-24 10:13:10 -04:00
|
|
|
#if AP_AIRSPEED_AUTOCAL_ENABLE
|
2015-05-13 03:09:36 -03:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
2016-08-07 21:48:36 -03:00
|
|
|
if (airspeed.get_airspeed() < aparm.airspeed_min &&
|
|
|
|
gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
|
2015-05-13 03:09:36 -03:00
|
|
|
// 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;
|
|
|
|
}
|
2015-08-27 03:22:44 -03:00
|
|
|
if (labs(ahrs.roll_sensor) > roll_limit_cd ||
|
2015-05-13 03:09:36 -03:00
|
|
|
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();
|
2016-08-08 00:28:35 -03:00
|
|
|
airspeed.update_calibration(vg, aparm.airspeed_max);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2020-11-24 10:13:10 -04:00
|
|
|
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
read the GPS and update position
|
|
|
|
*/
|
|
|
|
void Plane::update_GPS_50Hz(void)
|
|
|
|
{
|
2019-04-05 04:52:33 -03:00
|
|
|
gps.update();
|
|
|
|
|
2016-04-21 03:25:18 -03:00
|
|
|
// get position from AHRS
|
|
|
|
have_position = ahrs.get_position(current_loc);
|
2017-01-30 15:48:22 -04:00
|
|
|
ahrs.get_relative_position_D_home(relative_altitude);
|
|
|
|
relative_altitude *= -1.0f;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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
|
|
|
|
// -------------------------------------
|
2016-10-04 08:39:30 -03:00
|
|
|
if (current_loc.lat == 0 && current_loc.lng == 0) {
|
2015-05-13 03:09:36 -03:00
|
|
|
ground_start_count = 5;
|
|
|
|
|
|
|
|
} else {
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!set_home_persistently(gps.location())) {
|
|
|
|
// silently ignore failure...
|
|
|
|
}
|
2018-05-18 00:06:46 -03:00
|
|
|
|
|
|
|
next_WP_loc = prev_WP_loc = home;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
ground_start_count = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// see if we've breached the geo-fence
|
|
|
|
geofence_check(false);
|
|
|
|
|
|
|
|
// update wind estimate
|
|
|
|
ahrs.estimate_wind();
|
2016-06-21 22:49:29 -03:00
|
|
|
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
|
|
|
|
// lost 3D fix, start again
|
|
|
|
ground_start_count = 5;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
calc_gndspeed_undershoot();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2019-01-15 13:46:13 -04:00
|
|
|
main control mode dependent update code
|
2015-05-13 03:09:36 -03:00
|
|
|
*/
|
2019-01-15 13:46:13 -04:00
|
|
|
void Plane::update_control_mode(void)
|
2015-05-13 03:09:36 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
Mode *effective_mode = control_mode;
|
|
|
|
if (control_mode == &mode_auto && g.auto_fbw_steer == 42) {
|
|
|
|
effective_mode = &mode_fbwa;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
if (effective_mode != &mode_auto) {
|
2015-05-13 03:09:36 -03:00
|
|
|
// hold_course is only used in takeoff and landing
|
|
|
|
steer_state.hold_course_cd = -1;
|
|
|
|
}
|
|
|
|
|
2017-04-30 09:00:21 -03:00
|
|
|
// 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 (quadplane.in_vtol_mode() ||
|
|
|
|
quadplane.in_assisted_flight()) {
|
2015-12-26 06:40:40 -04:00
|
|
|
ahrs.set_fly_forward(false);
|
2017-05-15 15:36:16 -03:00
|
|
|
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
|
|
|
ahrs.set_fly_forward(landing.is_flying_forward());
|
2015-12-26 06:40:40 -04:00
|
|
|
} else {
|
|
|
|
ahrs.set_fly_forward(true);
|
|
|
|
}
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
effective_mode->update();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
set the flight stage
|
|
|
|
*/
|
2016-12-13 22:20:57 -04:00
|
|
|
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
|
2015-05-13 03:09:36 -03:00
|
|
|
{
|
2015-05-09 13:36:40 -03:00
|
|
|
if (fs == flight_stage) {
|
|
|
|
return;
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2017-01-11 02:18:09 -04:00
|
|
|
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
|
2017-01-11 01:29:03 -04:00
|
|
|
|
2017-01-11 01:31:11 -04:00
|
|
|
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
|
2019-08-30 06:57:30 -03:00
|
|
|
int(auto_state.takeoff_altitude_rel_cm/100));
|
2017-01-11 01:31:11 -04:00
|
|
|
}
|
2015-05-09 13:36:40 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
flight_stage = fs;
|
2017-10-23 21:39:45 -03:00
|
|
|
Log_Write_Status();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::update_alt()
|
|
|
|
{
|
|
|
|
barometer.update();
|
|
|
|
|
2020-05-14 03:12:24 -03:00
|
|
|
if (quadplane.available()) {
|
|
|
|
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
|
|
|
|
}
|
|
|
|
|
2015-06-13 06:11:49 -03:00
|
|
|
// 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;
|
2019-02-11 10:20:38 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
parachute.set_sink_rate(auto_state.sink_rate);
|
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
geofence_check(true);
|
|
|
|
|
|
|
|
update_flight_stage();
|
2016-01-20 02:21:07 -04:00
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
|
2016-02-23 18:42:59 -04:00
|
|
|
|
2016-04-19 21:11:00 -03:00
|
|
|
float distance_beyond_land_wp = 0;
|
2019-04-12 05:23:04 -03:00
|
|
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
|
2019-02-24 20:11:45 -04:00
|
|
|
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
|
2016-02-23 18:42:59 -04:00
|
|
|
}
|
2020-05-14 21:12:02 -03:00
|
|
|
|
|
|
|
float target_alt = relative_target_altitude_cm();
|
|
|
|
|
|
|
|
if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
|
|
|
|
// 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 + (g2.rtl_climb_min+10)*100);
|
|
|
|
}
|
|
|
|
|
|
|
|
SpdHgt_Controller->update_pitch_throttle(target_alt,
|
2016-01-20 02:21:07 -04:00
|
|
|
target_airspeed_cm,
|
2016-04-19 21:11:00 -03:00
|
|
|
flight_stage,
|
2016-02-12 15:39:27 -04:00
|
|
|
distance_beyond_land_wp,
|
2016-03-09 18:22:53 -04:00
|
|
|
get_takeoff_pitch_min_cd(),
|
2016-01-20 02:21:07 -04:00
|
|
|
throttle_nudge,
|
|
|
|
tecs_hgt_afe(),
|
2018-10-18 09:51:21 -03:00
|
|
|
aerodynamic_load_factor);
|
2016-01-20 02:21:07 -04:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
recalculate the flight_stage
|
|
|
|
*/
|
|
|
|
void Plane::update_flight_stage(void)
|
|
|
|
{
|
|
|
|
// Update the speed & height controller states
|
2020-12-23 01:25:35 -04:00
|
|
|
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
|
2019-01-15 13:46:13 -04:00
|
|
|
if (control_mode == &mode_auto) {
|
2016-01-01 06:39:36 -04:00
|
|
|
if (quadplane.in_vtol_auto()) {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
|
2016-01-01 02:36:41 -04:00
|
|
|
} else if (auto_state.takeoff_complete == false) {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
|
2015-05-13 03:09:36 -03:00
|
|
|
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
|
2016-12-13 23:18:48 -04:00
|
|
|
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
|
2015-05-09 13:36:40 -03:00
|
|
|
// abort mode is sticky, it must complete while executing NAV_LAND
|
2016-12-13 23:18:48 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
|
2018-11-09 18:38:43 -04:00
|
|
|
} else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
|
2017-07-15 04:07:10 -03:00
|
|
|
landing.request_go_around()) {
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
|
2016-12-13 23:18:48 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
|
2017-01-10 15:47:31 -04:00
|
|
|
} else {
|
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
|
|
|
|
}
|
2016-01-01 02:36:41 -04:00
|
|
|
} else if (quadplane.in_assisted_flight()) {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
|
2015-05-13 03:09:36 -03:00
|
|
|
} else {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2019-05-04 06:56:07 -03:00
|
|
|
} else if (control_mode != &mode_takeoff) {
|
2016-04-19 21:11:00 -03:00
|
|
|
// 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.
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2016-03-09 03:20:41 -04:00
|
|
|
} else if (quadplane.in_vtol_mode() ||
|
2016-01-01 03:18:53 -04:00
|
|
|
quadplane.in_assisted_flight()) {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
|
2016-01-01 03:18:53 -04:00
|
|
|
} else {
|
2016-12-13 22:20:57 -04:00
|
|
|
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2016-11-16 21:24:51 -04:00
|
|
|
/*
|
|
|
|
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
|
2017-01-11 15:06:32 -04:00
|
|
|
|
|
|
|
only called from AP_Landing, when the landing library is ready to disarm
|
2016-11-16 21:24:51 -04:00
|
|
|
*/
|
|
|
|
void Plane::disarm_if_autoland_complete()
|
|
|
|
{
|
2016-11-23 06:18:21 -04:00
|
|
|
if (landing.get_disarm_delay() > 0 &&
|
2016-11-16 21:24:51 -04:00
|
|
|
!is_flying() &&
|
2019-03-06 23:12:56 -04:00
|
|
|
arming.arming_required() != AP_Arming::Required::NO &&
|
2016-11-16 21:24:51 -04:00
|
|
|
arming.is_armed()) {
|
|
|
|
/* we have auto disarm enabled. See if enough time has passed */
|
2016-11-23 06:18:21 -04:00
|
|
|
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
|
2020-02-21 09:09:58 -04:00
|
|
|
if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
|
2016-11-16 21:24:51 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
2017-01-11 01:29:03 -04:00
|
|
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
2016-11-16 21:24:51 -04:00
|
|
|
hgt_afe = height_above_target();
|
|
|
|
hgt_afe -= rangefinder_correction();
|
|
|
|
} else {
|
|
|
|
// when in normal flight we pass the hgt_afe as relative
|
|
|
|
// altitude to home
|
2017-01-30 15:48:22 -04:00
|
|
|
hgt_afe = relative_altitude;
|
2016-11-16 21:24:51 -04:00
|
|
|
}
|
|
|
|
return hgt_afe;
|
|
|
|
}
|
|
|
|
|
2020-12-05 14:44:48 -04:00
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Plane::get_wp_distance_m(float &distance) const
|
2018-07-25 20:47:06 -03:00
|
|
|
{
|
2020-12-05 14:44:48 -04:00
|
|
|
// see GCS_MAVLINK_Plane::send_nav_controller_output()
|
|
|
|
if (control_mode == &mode_manual) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (quadplane.in_vtol_mode()) {
|
|
|
|
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() : 0;
|
|
|
|
} else {
|
|
|
|
distance = auto_state.wp_distance;
|
|
|
|
}
|
|
|
|
return true;
|
2018-07-25 20:47:06 -03:00
|
|
|
}
|
2020-12-05 14:44:48 -04:00
|
|
|
|
|
|
|
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 (quadplane.in_vtol_mode()) {
|
|
|
|
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
|
|
|
|
} else {
|
|
|
|
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 (quadplane.in_vtol_mode()) {
|
|
|
|
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
|
|
|
|
} else {
|
|
|
|
xtrack_error = nav_controller->crosstrack_error();
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-07-25 20:47:06 -03:00
|
|
|
|
2020-03-06 06:11:09 -04:00
|
|
|
// set target location (for use by scripting)
|
|
|
|
bool Plane::set_target_location(const Location& target_loc)
|
|
|
|
{
|
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
|
|
|
// only accept position updates when in GUIDED mode
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
plane.guided_WP_loc = target_loc;
|
|
|
|
// add home alt if needed
|
|
|
|
if (plane.guided_WP_loc.relative_alt) {
|
|
|
|
plane.guided_WP_loc.alt += plane.home.alt;
|
|
|
|
plane.guided_WP_loc.relative_alt = 0;
|
|
|
|
}
|
|
|
|
plane.set_guided_WP();
|
|
|
|
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::QLOITER:
|
|
|
|
case Mode::Number::QLAND:
|
|
|
|
case Mode::Number::QRTL:
|
|
|
|
target_loc = next_WP_loc;
|
|
|
|
return true;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-10-19 15:28:58 -03:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&plane);
|