2015-05-13 03:09:36 -03:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
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
|
|
|
|
|
2016-04-23 18:02:10 -03:00
|
|
|
Please contribute your ideas! See http://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),
|
|
|
|
SCHED_TASK(update_flight_mode, 400, 100),
|
|
|
|
SCHED_TASK(stabilize, 400, 100),
|
|
|
|
SCHED_TASK(set_servos, 400, 100),
|
|
|
|
SCHED_TASK(read_control_switch, 7, 100),
|
|
|
|
SCHED_TASK(gcs_retry_deferred, 50, 500),
|
|
|
|
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),
|
2016-07-22 04:45:05 -03:00
|
|
|
SCHED_TASK(afs_fs_check, 10, 100),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(gcs_update, 50, 500),
|
|
|
|
SCHED_TASK(gcs_data_stream_send, 50, 500),
|
|
|
|
SCHED_TASK(update_events, 50, 150),
|
|
|
|
SCHED_TASK(read_battery, 10, 300),
|
|
|
|
SCHED_TASK(compass_accumulate, 50, 200),
|
|
|
|
SCHED_TASK(barometer_accumulate, 50, 150),
|
2015-11-30 15:59:28 -04:00
|
|
|
SCHED_TASK(update_notify, 50, 300),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(read_rangefinder, 50, 100),
|
2016-07-23 04:37:42 -03:00
|
|
|
SCHED_TASK(ice_update, 10, 100),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(compass_cal_update, 50, 50),
|
|
|
|
SCHED_TASK(accel_cal_update, 10, 50),
|
2015-05-13 03:09:36 -03:00
|
|
|
#if OPTFLOW == ENABLED
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(update_optical_flow, 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(read_receiver_rssi, 10, 100),
|
|
|
|
SCHED_TASK(rpm_update, 10, 100),
|
|
|
|
SCHED_TASK(airspeed_ratio_update, 1, 100),
|
|
|
|
SCHED_TASK(update_mount, 50, 100),
|
|
|
|
SCHED_TASK(update_trigger, 50, 100),
|
|
|
|
SCHED_TASK(log_perf_info, 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),
|
2017-02-26 19:18:38 -04:00
|
|
|
SCHED_TASK(update_soaring, 50, 400),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(parachute_check, 10, 200),
|
|
|
|
SCHED_TASK(terrain_update, 10, 200),
|
2015-11-30 15:59:28 -04:00
|
|
|
SCHED_TASK(update_is_flying_5Hz, 5, 100),
|
2016-04-21 01:51:29 -03:00
|
|
|
SCHED_TASK(dataflash_periodic, 50, 400),
|
2016-08-12 18:28:21 -03:00
|
|
|
SCHED_TASK(avoidance_adsb_update, 10, 100),
|
2016-07-21 21:28:17 -03:00
|
|
|
SCHED_TASK(button_update, 5, 100),
|
2016-10-25 22:24:41 -03:00
|
|
|
SCHED_TASK(stats_update, 1, 100),
|
2015-05-13 03:09:36 -03:00
|
|
|
};
|
|
|
|
|
2016-10-25 22:24:41 -03:00
|
|
|
/*
|
|
|
|
update AP_Stats
|
|
|
|
*/
|
|
|
|
void Plane::stats_update(void)
|
|
|
|
{
|
|
|
|
g2.stats.update();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::setup()
|
|
|
|
{
|
|
|
|
cliSerial = hal.console;
|
|
|
|
|
|
|
|
// load the default values of variables listed in var_info[]
|
|
|
|
AP_Param::setup_sketch_defaults();
|
|
|
|
|
|
|
|
AP_Notify::flags.failsafe_battery = false;
|
|
|
|
|
2015-08-28 03:01:32 -03:00
|
|
|
rssi.init();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
init_ardupilot();
|
|
|
|
|
|
|
|
// initialise the main loop scheduler
|
2015-07-06 12:30:40 -03:00
|
|
|
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::loop()
|
|
|
|
{
|
2016-04-21 01:51:29 -03:00
|
|
|
uint32_t loop_us = 1000000UL / scheduler.get_loop_rate_hz();
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// wait for an INS sample
|
|
|
|
ins.wait_for_sample();
|
|
|
|
|
2015-05-13 19:05:32 -03:00
|
|
|
uint32_t timer = micros();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-04-20 22:50:13 -03:00
|
|
|
perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us;
|
|
|
|
G_Dt = perf.delta_us_fast_loop * 1.0e-6f;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-04-21 01:51:29 -03:00
|
|
|
if (perf.delta_us_fast_loop > loop_us + 500) {
|
|
|
|
perf.num_long++;
|
|
|
|
}
|
|
|
|
|
2016-04-20 22:50:13 -03:00
|
|
|
if (perf.delta_us_fast_loop > perf.G_Dt_max && perf.fast_loopTimer_us != 0) {
|
|
|
|
perf.G_Dt_max = perf.delta_us_fast_loop;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
2016-04-20 22:50:13 -03:00
|
|
|
if (perf.delta_us_fast_loop < perf.G_Dt_min || perf.G_Dt_min == 0) {
|
|
|
|
perf.G_Dt_min = perf.delta_us_fast_loop;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2016-04-20 22:50:13 -03:00
|
|
|
perf.fast_loopTimer_us = timer;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-04-20 22:50:13 -03:00
|
|
|
perf.mainLoop_count++;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// tell the scheduler one tick has passed
|
|
|
|
scheduler.tick();
|
|
|
|
|
|
|
|
// run all the tasks that are due to run. Note that we only
|
|
|
|
// have to call this once per loop, as the tasks are scheduled
|
|
|
|
// in multiples of the main loop tick. So if they don't run on
|
|
|
|
// the first call to the scheduler they won't run on a later
|
|
|
|
// call until scheduler.tick() is called again
|
2016-04-21 01:51:29 -03:00
|
|
|
scheduler.run(loop_us);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
2016-07-28 03:37:28 -03:00
|
|
|
void Plane::update_soft_armed()
|
|
|
|
{
|
|
|
|
hal.util->set_soft_armed(arming.is_armed() &&
|
|
|
|
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
|
|
|
DataFlash.set_vehicle_armed(hal.util->get_soft_armed());
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// update AHRS system
|
|
|
|
void Plane::ahrs_update()
|
|
|
|
{
|
2016-07-28 03:37:28 -03:00
|
|
|
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
|
|
|
|
gcs_update();
|
|
|
|
}
|
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)) {
|
2015-05-13 03:09:36 -03:00
|
|
|
Log_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
|
|
|
|
|
|
|
// update inertial_nav for quadplane
|
|
|
|
quadplane.inertial_nav.update(G_Dt);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update 50Hz speed/height controller
|
|
|
|
*/
|
|
|
|
void Plane::update_speed_height(void)
|
|
|
|
{
|
|
|
|
if (auto_throttle_mode) {
|
|
|
|
// 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
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
update camera mount
|
|
|
|
*/
|
|
|
|
void Plane::update_mount(void)
|
|
|
|
{
|
|
|
|
#if MOUNT == ENABLED
|
|
|
|
camera_mount.update();
|
|
|
|
#endif
|
2015-06-07 14:17:47 -03:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2015-06-07 14:17:47 -03:00
|
|
|
/*
|
|
|
|
update camera trigger
|
|
|
|
*/
|
|
|
|
void Plane::update_trigger(void)
|
|
|
|
{
|
2015-05-13 03:09:36 -03:00
|
|
|
#if CAMERA == ENABLED
|
2017-07-26 00:48:21 -03:00
|
|
|
camera.update_trigger();
|
2015-05-13 03:09:36 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
read and update compass
|
|
|
|
*/
|
|
|
|
void Plane::update_compass(void)
|
|
|
|
{
|
|
|
|
if (g.compass_enabled && compass.read()) {
|
|
|
|
ahrs.set_compass(&compass);
|
|
|
|
compass.learn_offsets();
|
2016-05-04 05:32:02 -03:00
|
|
|
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
|
2015-05-13 03:09:36 -03:00
|
|
|
DataFlash.Log_Write_Compass(compass);
|
|
|
|
}
|
|
|
|
} else {
|
2016-10-28 19:10:03 -03:00
|
|
|
ahrs.set_compass(nullptr);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
if the compass is enabled then try to accumulate a reading
|
|
|
|
*/
|
|
|
|
void Plane::compass_accumulate(void)
|
|
|
|
{
|
|
|
|
if (g.compass_enabled) {
|
|
|
|
compass.accumulate();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
try to accumulate a baro reading
|
|
|
|
*/
|
|
|
|
void Plane::barometer_accumulate(void)
|
|
|
|
{
|
|
|
|
barometer.accumulate();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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))
|
|
|
|
Log_Write_IMU();
|
2017-04-09 08:16:50 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED))
|
|
|
|
Log_Write_AOA_SSA();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
do 10Hz logging - part2
|
|
|
|
*/
|
|
|
|
void Plane::update_logging2(void)
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_CTUN))
|
|
|
|
Log_Write_Control_Tuning();
|
|
|
|
|
|
|
|
if (should_log(MASK_LOG_NTUN))
|
|
|
|
Log_Write_Nav_Tuning();
|
|
|
|
|
|
|
|
if (should_log(MASK_LOG_RC))
|
|
|
|
Log_Write_RC();
|
2015-06-12 04:00:06 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_IMU))
|
|
|
|
DataFlash.Log_Write_Vibration(ins);
|
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
|
|
|
*/
|
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
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
update aux servo mappings
|
|
|
|
*/
|
|
|
|
void Plane::update_aux(void)
|
|
|
|
{
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channels::enable_aux_servos();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::one_second_loop()
|
|
|
|
{
|
|
|
|
// send a heartbeat
|
2017-07-07 22:54:10 -03:00
|
|
|
gcs().send_message(MSG_HEARTBEAT);
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// make it possible to change control channel ordering at runtime
|
|
|
|
set_control_channels();
|
|
|
|
|
2016-09-10 19:19:22 -03:00
|
|
|
#if HAVE_PX4_MIXER
|
2016-06-05 19:47:55 -03:00
|
|
|
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
|
|
|
|
// if disarmed try to configure the mixer
|
|
|
|
setup_failsafe_mixing();
|
|
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// make it possible to change orientation at runtime
|
|
|
|
ahrs.set_orientation();
|
|
|
|
|
2016-08-07 21:48:36 -03:00
|
|
|
adsb.set_stall_speed_cm(aparm.airspeed_min);
|
2016-07-10 21:23:08 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// sync MAVLink system ID
|
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
|
|
|
update_aux();
|
|
|
|
|
|
|
|
// 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::NO;
|
|
|
|
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
if (should_log(MASK_LOG_GPS)) {
|
|
|
|
terrain.log_terrain_data(DataFlash);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2016-10-13 20:58:51 -03:00
|
|
|
// update home position if soft armed and gps position has
|
|
|
|
// changed. Update every 5s at most
|
|
|
|
if (!hal.util->get_soft_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
|
2016-11-16 21:12:26 -04:00
|
|
|
landing.alt_offset = 0;
|
2016-10-13 20:58:51 -03:00
|
|
|
}
|
2016-10-27 23:09:35 -03:00
|
|
|
|
|
|
|
// update error mask of sensors and subsystems. The mask uses the
|
|
|
|
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
|
|
|
|
// indicates that the sensor or subsystem is present but not
|
|
|
|
// functioning correctly
|
|
|
|
update_sensor_status_flags();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::log_perf_info()
|
|
|
|
{
|
|
|
|
if (scheduler.debug() != 0) {
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u",
|
2016-04-21 01:51:29 -03:00
|
|
|
(unsigned)perf.num_long,
|
|
|
|
(unsigned)perf.mainLoop_count,
|
2016-04-21 03:11:48 -03:00
|
|
|
(unsigned)perf.G_Dt_max,
|
|
|
|
(unsigned)perf.G_Dt_min,
|
|
|
|
(unsigned)(DataFlash.num_dropped() - perf.last_log_dropped));
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2015-11-03 09:46:40 -04:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_PM)) {
|
2015-05-13 03:09:36 -03:00
|
|
|
Log_Write_Performance();
|
2015-11-03 09:46:40 -04:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
resetPerfData();
|
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::compass_save()
|
|
|
|
{
|
2016-04-30 02:05:44 -03:00
|
|
|
if (g.compass_enabled &&
|
|
|
|
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();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::terrain_update(void)
|
|
|
|
{
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
terrain.update();
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2015-11-19 14:24:55 -04:00
|
|
|
|
2015-08-06 10:25:22 -03:00
|
|
|
void Plane::dataflash_periodic(void)
|
|
|
|
{
|
|
|
|
DataFlash.periodic_tasks();
|
|
|
|
}
|
|
|
|
|
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
|
|
|
gcs_send_airspeed_calibration(vg);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
read the GPS and update position
|
|
|
|
*/
|
|
|
|
void Plane::update_GPS_50Hz(void)
|
|
|
|
{
|
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;
|
2016-04-21 03:25:18 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
|
|
|
|
gps.update();
|
|
|
|
|
|
|
|
for (uint8_t i=0; i<gps.num_sensors(); i++) {
|
|
|
|
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
|
|
|
|
last_gps_reading[i] = gps.last_message_time_ms(i);
|
|
|
|
if (should_log(MASK_LOG_GPS)) {
|
|
|
|
Log_Write_GPS(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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 {
|
|
|
|
init_home();
|
|
|
|
|
|
|
|
// set system clock for log timestamps
|
2016-01-21 02:57:53 -04:00
|
|
|
uint64_t gps_timestamp = gps.time_epoch_usec();
|
|
|
|
|
|
|
|
hal.util->set_system_clock(gps_timestamp);
|
|
|
|
|
|
|
|
// update signing timestamp
|
|
|
|
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
if (g.compass_enabled) {
|
|
|
|
// Set compass declination automatically
|
|
|
|
const Location &loc = gps.location();
|
|
|
|
compass.set_initial_location(loc.lat, loc.lng);
|
|
|
|
}
|
|
|
|
ground_start_count = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// see if we've breached the geo-fence
|
|
|
|
geofence_check(false);
|
|
|
|
|
|
|
|
#if CAMERA == ENABLED
|
2017-07-26 00:48:21 -03:00
|
|
|
camera.update();
|
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// 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();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
main handling for AUTO mode
|
|
|
|
*/
|
|
|
|
void Plane::handle_auto_mode(void)
|
|
|
|
{
|
2016-04-22 04:59:00 -03:00
|
|
|
uint16_t nav_cmd_id;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-04-24 03:08:03 -03:00
|
|
|
if (mission.state() != AP_Mission::MISSION_RUNNING) {
|
2016-11-23 19:42:10 -04:00
|
|
|
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
|
|
|
|
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(RTL, MODE_REASON_MISSION_END);
|
2017-07-09 00:49:39 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
|
2016-04-24 03:08:03 -03:00
|
|
|
return;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
2016-04-24 03:08:03 -03:00
|
|
|
nav_cmd_id = mission.get_current_nav_cmd().id;
|
|
|
|
|
2016-01-01 06:39:36 -04:00
|
|
|
if (quadplane.in_vtol_auto()) {
|
2015-12-26 06:40:40 -04:00
|
|
|
quadplane.control_auto(next_WP_loc);
|
|
|
|
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
|
2016-12-13 23:18:48 -04:00
|
|
|
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
|
2015-05-13 03:09:36 -03:00
|
|
|
takeoff_calc_roll();
|
|
|
|
takeoff_calc_pitch();
|
|
|
|
calc_throttle();
|
2015-05-09 13:36:40 -03:00
|
|
|
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
|
2015-05-13 03:09:36 -03:00
|
|
|
calc_nav_roll();
|
|
|
|
calc_nav_pitch();
|
|
|
|
|
2017-01-11 15:06:32 -04:00
|
|
|
// allow landing to restrict the roll limits
|
|
|
|
nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL);
|
2016-12-12 04:29:33 -04:00
|
|
|
|
2017-02-14 15:13:11 -04:00
|
|
|
if (landing.is_throttle_suppressed()) {
|
2017-01-11 15:06:32 -04:00
|
|
|
// if landing is considered complete throttle is never allowed, regardless of landing type
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
|
2016-12-12 04:29:33 -04:00
|
|
|
} else {
|
|
|
|
calc_throttle();
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2015-05-09 13:36:40 -03:00
|
|
|
} else {
|
2015-05-13 03:09:36 -03:00
|
|
|
// we are doing normal AUTO flight, the special cases
|
|
|
|
// are for takeoff and landing
|
2015-08-24 07:06:24 -03:00
|
|
|
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
|
|
|
|
steer_state.hold_course_cd = -1;
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
calc_nav_roll();
|
|
|
|
calc_nav_pitch();
|
|
|
|
calc_throttle();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
main flight mode dependent update code
|
|
|
|
*/
|
|
|
|
void Plane::update_flight_mode(void)
|
|
|
|
{
|
|
|
|
enum FlightMode effective_mode = control_mode;
|
2016-04-05 18:58:23 -03:00
|
|
|
if (control_mode == AUTO && g.auto_fbw_steer == 42) {
|
2015-05-13 03:09:36 -03:00
|
|
|
effective_mode = FLY_BY_WIRE_A;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (effective_mode != AUTO) {
|
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
switch (effective_mode)
|
|
|
|
{
|
|
|
|
case AUTO:
|
|
|
|
handle_auto_mode();
|
|
|
|
break;
|
|
|
|
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2016-05-05 22:28:26 -03:00
|
|
|
case GUIDED:
|
2016-05-11 02:57:41 -03:00
|
|
|
if (auto_state.vtol_loiter && quadplane.available()) {
|
2016-05-05 22:28:26 -03:00
|
|
|
quadplane.guided_update();
|
|
|
|
break;
|
|
|
|
}
|
2016-12-12 08:36:10 -04:00
|
|
|
// no break
|
2016-05-05 22:28:26 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
case RTL:
|
|
|
|
case LOITER:
|
|
|
|
calc_nav_roll();
|
|
|
|
calc_nav_pitch();
|
|
|
|
calc_throttle();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case TRAINING: {
|
|
|
|
training_manual_roll = false;
|
|
|
|
training_manual_pitch = false;
|
2015-08-14 00:31:59 -03:00
|
|
|
update_load_factor();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// if the roll is past the set roll limit, then
|
|
|
|
// we set target roll to the limit
|
|
|
|
if (ahrs.roll_sensor >= roll_limit_cd) {
|
|
|
|
nav_roll_cd = roll_limit_cd;
|
|
|
|
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
|
|
|
|
nav_roll_cd = -roll_limit_cd;
|
|
|
|
} else {
|
|
|
|
training_manual_roll = true;
|
|
|
|
nav_roll_cd = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// if the pitch is past the set pitch limits, then
|
|
|
|
// we set target pitch to the limit
|
|
|
|
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
|
|
|
|
nav_pitch_cd = aparm.pitch_limit_max_cd;
|
|
|
|
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
|
|
|
|
nav_pitch_cd = pitch_limit_min_cd;
|
|
|
|
} else {
|
|
|
|
training_manual_pitch = true;
|
|
|
|
nav_pitch_cd = 0;
|
|
|
|
}
|
|
|
|
if (fly_inverted()) {
|
|
|
|
nav_pitch_cd = -nav_pitch_cd;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case ACRO: {
|
|
|
|
// handle locked/unlocked control
|
|
|
|
if (acro_state.locked_roll) {
|
|
|
|
nav_roll_cd = acro_state.locked_roll_err;
|
|
|
|
} else {
|
|
|
|
nav_roll_cd = ahrs.roll_sensor;
|
|
|
|
}
|
|
|
|
if (acro_state.locked_pitch) {
|
|
|
|
nav_pitch_cd = acro_state.locked_pitch_cd;
|
|
|
|
} else {
|
|
|
|
nav_pitch_cd = ahrs.pitch_sensor;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case AUTOTUNE:
|
|
|
|
case FLY_BY_WIRE_A: {
|
|
|
|
// set nav_roll and nav_pitch using sticks
|
|
|
|
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
|
|
|
|
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
|
|
|
|
update_load_factor();
|
|
|
|
float pitch_input = channel_pitch->norm_input();
|
|
|
|
if (pitch_input > 0) {
|
|
|
|
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
|
|
|
|
} else {
|
|
|
|
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
|
|
|
|
}
|
|
|
|
adjust_nav_pitch_throttle();
|
|
|
|
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
|
|
|
|
if (fly_inverted()) {
|
|
|
|
nav_pitch_cd = -nav_pitch_cd;
|
|
|
|
}
|
|
|
|
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
|
|
|
|
// FBWA failsafe glide
|
|
|
|
nav_roll_cd = 0;
|
|
|
|
nav_pitch_cd = 0;
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
if (g.fbwa_tdrag_chan > 0) {
|
|
|
|
// check for the user enabling FBWA taildrag takeoff mode
|
|
|
|
bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700);
|
|
|
|
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
|
|
|
|
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
|
|
|
|
auto_state.fbwa_tdrag_takeoff_mode = true;
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case FLY_BY_WIRE_B:
|
|
|
|
// Thanks to Yury MonZon for the altitude limit code!
|
|
|
|
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
|
|
|
|
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
|
|
|
|
update_load_factor();
|
|
|
|
update_fbwb_speed_height();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case CRUISE:
|
|
|
|
/*
|
|
|
|
in CRUISE mode we use the navigation code to control
|
|
|
|
roll when heading is locked. Heading becomes unlocked on
|
|
|
|
any aileron or rudder input
|
|
|
|
*/
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
if ((channel_roll->get_control_in() != 0 ||
|
2015-05-13 03:09:36 -03:00
|
|
|
rudder_input != 0)) {
|
|
|
|
cruise_state.locked_heading = false;
|
|
|
|
cruise_state.lock_timer_ms = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!cruise_state.locked_heading) {
|
|
|
|
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
|
|
|
|
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
|
|
|
|
update_load_factor();
|
|
|
|
} else {
|
|
|
|
calc_nav_roll();
|
|
|
|
}
|
|
|
|
update_fbwb_speed_height();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case STABILIZE:
|
|
|
|
nav_roll_cd = 0;
|
|
|
|
nav_pitch_cd = 0;
|
|
|
|
// throttle is passthrough
|
|
|
|
break;
|
|
|
|
|
|
|
|
case CIRCLE:
|
|
|
|
// we have no GPS installed and have lost radio contact
|
|
|
|
// or we just want to fly around in a gentle circle w/o GPS,
|
|
|
|
// holding altitude at the altitude we set when we
|
|
|
|
// switched into the mode
|
|
|
|
nav_roll_cd = roll_limit_cd / 3;
|
|
|
|
update_load_factor();
|
|
|
|
calc_nav_pitch();
|
|
|
|
calc_throttle();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case MANUAL:
|
2017-01-07 01:36:30 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
|
|
|
|
steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz();
|
2015-05-13 03:09:36 -03:00
|
|
|
break;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QHOVER:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLOITER:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QLAND:
|
|
|
|
case QRTL: {
|
2015-11-24 04:24:04 -04:00
|
|
|
// set nav_roll and nav_pitch using sticks
|
2016-06-16 09:00:04 -03:00
|
|
|
int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
|
2017-02-24 01:47:09 -04:00
|
|
|
nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit;
|
2016-06-16 09:00:04 -03:00
|
|
|
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
|
2015-11-24 04:24:04 -04:00
|
|
|
float pitch_input = channel_pitch->norm_input();
|
2017-05-15 10:30:09 -03:00
|
|
|
// Scale from normalized input [-1,1] to centidegrees
|
2017-05-15 23:42:18 -03:00
|
|
|
if (quadplane.tailsitter_active()) {
|
2017-05-15 10:30:09 -03:00
|
|
|
// For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX]
|
2017-05-10 01:45:18 -03:00
|
|
|
nav_pitch_cd = pitch_input * quadplane.aparm.angle_max;
|
2015-11-24 04:24:04 -04:00
|
|
|
} else {
|
2017-05-15 23:42:18 -03:00
|
|
|
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
|
|
|
|
// tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
|
2017-05-10 01:45:18 -03:00
|
|
|
if (pitch_input > 0) {
|
|
|
|
nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max);
|
|
|
|
} else {
|
|
|
|
nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max);
|
|
|
|
}
|
|
|
|
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
|
2015-11-24 04:24:04 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
case INITIALISING:
|
|
|
|
// handled elsewhere
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::update_navigation()
|
|
|
|
{
|
|
|
|
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
|
|
|
|
// ------------------------------------------------------------------------
|
|
|
|
|
2016-01-19 00:04:30 -04:00
|
|
|
uint16_t radius = 0;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
switch(control_mode) {
|
|
|
|
case AUTO:
|
2016-04-24 03:08:03 -03:00
|
|
|
if (home_is_set != HOME_UNSET) {
|
|
|
|
mission.update();
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case RTL:
|
2016-04-29 03:31:22 -03:00
|
|
|
if (quadplane.available() && quadplane.rtl_mode == 1 &&
|
2017-04-22 08:47:19 -03:00
|
|
|
nav_controller->reached_loiter_target() &&
|
|
|
|
AP_HAL::millis() - last_mode_change_ms > 1000) {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(QRTL, MODE_REASON_UNKNOWN);
|
2016-04-29 03:31:22 -03:00
|
|
|
break;
|
|
|
|
} else if (g.rtl_autoland == 1 &&
|
2016-11-23 07:07:50 -04:00
|
|
|
!auto_state.checked_for_autoland &&
|
2016-05-07 21:40:42 -03:00
|
|
|
reached_loiter_target() &&
|
2015-05-13 03:09:36 -03:00
|
|
|
labs(altitude_error_cm) < 1000) {
|
|
|
|
// we've reached the RTL point, see if we have a landing sequence
|
2016-11-25 20:45:23 -04:00
|
|
|
if (mission.jump_to_landing_sequence()) {
|
2016-11-16 21:19:58 -04:00
|
|
|
// switch from RTL -> AUTO
|
|
|
|
set_mode(AUTO, MODE_REASON_UNKNOWN);
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// prevent running the expensive jump_to_landing_sequence
|
|
|
|
// on every loop
|
2016-11-23 07:07:50 -04:00
|
|
|
auto_state.checked_for_autoland = true;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
else if (g.rtl_autoland == 2 &&
|
2016-11-23 07:07:50 -04:00
|
|
|
!auto_state.checked_for_autoland) {
|
2015-05-13 03:09:36 -03:00
|
|
|
// Go directly to the landing sequence
|
2016-11-25 20:45:23 -04:00
|
|
|
if (mission.jump_to_landing_sequence()) {
|
2016-11-16 21:19:58 -04:00
|
|
|
// switch from RTL -> AUTO
|
|
|
|
set_mode(AUTO, MODE_REASON_UNKNOWN);
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// prevent running the expensive jump_to_landing_sequence
|
|
|
|
// on every loop
|
2016-11-23 07:07:50 -04:00
|
|
|
auto_state.checked_for_autoland = true;
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
2016-01-19 00:04:30 -04:00
|
|
|
radius = abs(g.rtl_radius);
|
2016-02-29 14:11:31 -04:00
|
|
|
if (radius > 0) {
|
|
|
|
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
|
|
|
|
}
|
2016-12-12 08:36:10 -04:00
|
|
|
// no break, fall through to LOITER
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
case LOITER:
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2015-05-13 03:09:36 -03:00
|
|
|
case GUIDED:
|
2016-02-29 14:11:31 -04:00
|
|
|
update_loiter(radius);
|
2015-05-13 03:09:36 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case CRUISE:
|
|
|
|
update_cruise();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case MANUAL:
|
|
|
|
case STABILIZE:
|
|
|
|
case TRAINING:
|
|
|
|
case INITIALISING:
|
|
|
|
case ACRO:
|
|
|
|
case FLY_BY_WIRE_A:
|
|
|
|
case AUTOTUNE:
|
|
|
|
case FLY_BY_WIRE_B:
|
|
|
|
case CIRCLE:
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
|
|
|
case QHOVER:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QLOITER:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2015-05-13 03:09:36 -03:00
|
|
|
// nothing to do
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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",
|
2017-01-11 01:31:11 -04:00
|
|
|
auto_state.takeoff_altitude_rel_cm/100);
|
|
|
|
}
|
2015-05-09 13:36:40 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
flight_stage = fs;
|
2016-04-19 19:41:02 -03:00
|
|
|
|
|
|
|
if (should_log(MASK_LOG_MODE)) {
|
|
|
|
Log_Write_Status();
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::update_alt()
|
|
|
|
{
|
|
|
|
barometer.update();
|
|
|
|
if (should_log(MASK_LOG_IMU)) {
|
|
|
|
Log_Write_Baro();
|
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
geofence_check(true);
|
|
|
|
|
|
|
|
update_flight_stage();
|
2016-01-20 02:21:07 -04:00
|
|
|
|
|
|
|
if (auto_throttle_mode && !throttle_suppressed) {
|
2016-02-23 18:42:59 -04:00
|
|
|
|
2016-04-19 21:11:00 -03:00
|
|
|
float distance_beyond_land_wp = 0;
|
2017-01-11 01:29:03 -04:00
|
|
|
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
|
2016-05-27 20:37:38 -03:00
|
|
|
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
|
2016-02-23 18:42:59 -04:00
|
|
|
}
|
|
|
|
|
2016-01-20 02:21:07 -04:00
|
|
|
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
|
|
|
|
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(),
|
|
|
|
aerodynamic_load_factor);
|
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
recalculate the flight_stage
|
|
|
|
*/
|
|
|
|
void Plane::update_flight_stage(void)
|
|
|
|
{
|
|
|
|
// Update the speed & height controller states
|
|
|
|
if (auto_throttle_mode && !throttle_suppressed) {
|
|
|
|
if (control_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);
|
2017-07-15 04:07:10 -03:00
|
|
|
} else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90 &&
|
|
|
|
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
|
|
|
}
|
|
|
|
} else {
|
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
|
|
|
}
|
|
|
|
|
|
|
|
// tell AHRS the airspeed to true airspeed ratio
|
|
|
|
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if OPTFLOW == ENABLED
|
|
|
|
// called at 50hz
|
|
|
|
void Plane::update_optical_flow(void)
|
|
|
|
{
|
|
|
|
static uint32_t last_of_update = 0;
|
|
|
|
|
|
|
|
// exit immediately if not enabled
|
|
|
|
if (!optflow.enabled()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// read from sensor
|
|
|
|
optflow.update();
|
|
|
|
|
|
|
|
// write to log and send to EKF if new data has arrived
|
|
|
|
if (optflow.last_update() != last_of_update) {
|
|
|
|
last_of_update = optflow.last_update();
|
|
|
|
uint8_t flowQuality = optflow.quality();
|
|
|
|
Vector2f flowRate = optflow.flowRate();
|
|
|
|
Vector2f bodyRate = optflow.bodyRate();
|
2016-10-27 01:47:39 -03:00
|
|
|
const Vector3f &posOffset = optflow.get_pos_offset();
|
2016-10-11 18:25:22 -03:00
|
|
|
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset);
|
2015-05-13 03:09:36 -03:00
|
|
|
Log_Write_Optflow();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
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() &&
|
|
|
|
arming.arming_required() != AP_Arming::NO &&
|
|
|
|
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) {
|
2016-11-16 21:24:51 -04:00
|
|
|
if (disarm_motors()) {
|
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;
|
|
|
|
}
|
|
|
|
|
2015-10-19 15:28:58 -03:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&plane);
|