2015-05-29 23:12:49 -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/>.
|
|
|
|
*/
|
2019-05-30 23:09:25 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
* ArduCopter (also known as APM, APM:Copter or just Copter)
|
|
|
|
* Wiki: copter.ardupilot.org
|
|
|
|
* Creator: Jason Short
|
|
|
|
* Lead Developer: Randy Mackay
|
|
|
|
* Lead Tester: Marco Robustini
|
|
|
|
* Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen,
|
|
|
|
Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
|
|
|
|
Jean-Louis Naudin, Mike Smith, and more
|
2020-04-23 01:14:18 -03:00
|
|
|
* Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
|
2019-05-30 23:09:25 -03:00
|
|
|
*
|
|
|
|
* Special Thanks to contributors (in alphabetical order by first name):
|
|
|
|
*
|
|
|
|
* Adam M Rivera :Auto Compass Declination
|
|
|
|
* Amilcar Lucas :Camera mount library
|
|
|
|
* Andrew Tridgell :General development, Mavlink Support
|
2021-02-11 13:41:25 -04:00
|
|
|
* Andy Piper :Harmonic notch, In-flight FFT, Bi-directional DShot, various drivers
|
2019-05-30 23:09:25 -03:00
|
|
|
* Angel Fernandez :Alpha testing
|
|
|
|
* AndreasAntonopoulous:GeoFence
|
|
|
|
* Arthur Benemann :DroidPlanner GCS
|
|
|
|
* Benjamin Pelletier :Libraries
|
|
|
|
* Bill King :Single Copter
|
|
|
|
* Christof Schmid :Alpha testing
|
|
|
|
* Craig Elder :Release Management, Support
|
|
|
|
* Dani Saez :V Octo Support
|
2020-04-23 01:14:18 -03:00
|
|
|
* Doug Weibel :DCM, Libraries, Control law advice
|
2019-05-30 23:09:25 -03:00
|
|
|
* Emile Castelnuovo :VRBrain port, bug fixes
|
|
|
|
* Gregory Fletcher :Camera mount orientation math
|
|
|
|
* Guntars :Arming safety suggestion
|
|
|
|
* HappyKillmore :Mavlink GCS
|
|
|
|
* Hein Hollander :Octo Support, Heli Testing
|
|
|
|
* Igor van Airde :Control Law optimization
|
|
|
|
* Jack Dunkle :Alpha testing
|
|
|
|
* James Goppert :Mavlink Support
|
|
|
|
* Jani Hiriven :Testing feedback
|
|
|
|
* Jean-Louis Naudin :Auto Landing
|
2020-04-23 01:14:18 -03:00
|
|
|
* John Arne Birkeland :PPM Encoder
|
2019-05-30 23:09:25 -03:00
|
|
|
* Jose Julio :Stabilization Control laws, MPU6k driver
|
|
|
|
* Julien Dubois :PosHold flight mode
|
|
|
|
* Julian Oes :Pixhawk
|
|
|
|
* Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
|
|
|
|
* Kevin Hester :Andropilot GCS
|
|
|
|
* Max Levine :Tri Support, Graphics
|
|
|
|
* Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
|
|
|
|
* Marco Robustini :Lead tester
|
|
|
|
* Michael Oborne :Mission Planner GCS
|
|
|
|
* Mike Smith :Pixhawk driver, coding support
|
|
|
|
* Olivier Adler :PPM Encoder, piezo buzzer
|
|
|
|
* Pat Hickey :Hardware Abstraction Layer (HAL)
|
|
|
|
* Robert Lefebvre :Heli Support, Copter LEDs
|
|
|
|
* Roberto Navoni :Library testing, Porting to VRBrain
|
|
|
|
* Sandro Benigno :Camera support, MinimOSD
|
|
|
|
* Sandro Tognana :PosHold flight mode
|
|
|
|
* Sebastian Quilter :SmartRTL
|
|
|
|
* ..and many more.
|
|
|
|
*
|
|
|
|
* Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors
|
2019-12-08 17:22:11 -04:00
|
|
|
* Wiki: https://copter.ardupilot.org/
|
2019-05-30 23:09:25 -03:00
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "Copter.h"
|
2017-09-12 15:25:20 -03:00
|
|
|
|
|
|
|
#define FORCE_VERSION_H_INCLUDE
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "version.h"
|
2017-09-12 15:25:20 -03:00
|
|
|
#undef FORCE_VERSION_H_INCLUDE
|
2015-05-29 23:12:49 -03:00
|
|
|
|
2015-10-16 17:22:11 -03:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2015-05-29 23:12:49 -03:00
|
|
|
|
2021-11-13 23:18:10 -04:00
|
|
|
#define SCHED_TASK(func, _interval_ticks, _max_time_micros, _prio) SCHED_TASK_CLASS(Copter, &copter, func, _interval_ticks, _max_time_micros, _prio)
|
2022-04-01 16:56:16 -03:00
|
|
|
#define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)
|
2019-05-30 23:09:25 -03:00
|
|
|
|
|
|
|
/*
|
2022-04-01 16:56:16 -03:00
|
|
|
scheduler table - all tasks should be listed here.
|
2021-11-13 23:18:10 -04:00
|
|
|
|
|
|
|
All entries in this table must be ordered by priority.
|
|
|
|
|
|
|
|
This table is interleaved with the table in AP_Vehicle 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)
|
|
|
|
|
2019-05-30 23:09:25 -03:00
|
|
|
*/
|
|
|
|
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
2022-04-01 16:56:16 -03:00
|
|
|
// update INS immediately to get current gyro data populated
|
|
|
|
FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
|
|
|
|
// run low level rate controllers that only require IMU data
|
|
|
|
FAST_TASK(run_rate_controller),
|
2022-07-16 22:49:47 -03:00
|
|
|
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
|
|
|
FAST_TASK(run_custom_controller),
|
|
|
|
#endif
|
2022-04-01 16:56:16 -03:00
|
|
|
// send outputs to the motors library immediately
|
|
|
|
FAST_TASK(motors_output),
|
|
|
|
// run EKF state estimator (expensive)
|
|
|
|
FAST_TASK(read_AHRS),
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
FAST_TASK(update_heli_control_dynamics),
|
|
|
|
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
|
|
|
FAST_TASK(heli_update_autorotation),
|
|
|
|
#endif
|
|
|
|
#endif //HELI_FRAME
|
|
|
|
// Inertial Nav
|
|
|
|
FAST_TASK(read_inertia),
|
|
|
|
// check if ekf has reset target heading or position
|
|
|
|
FAST_TASK(check_ekf_reset),
|
|
|
|
// run the attitude controllers
|
|
|
|
FAST_TASK(update_flight_mode),
|
|
|
|
// update home from EKF if necessary
|
|
|
|
FAST_TASK(update_home_from_EKF),
|
|
|
|
// check if we've landed or crashed
|
|
|
|
FAST_TASK(update_land_and_crash_detectors),
|
|
|
|
#if HAL_MOUNT_ENABLED
|
|
|
|
// camera mount's fast update
|
|
|
|
FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
|
|
|
|
#endif
|
|
|
|
FAST_TASK(Log_Video_Stabilisation),
|
|
|
|
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(rc_loop, 100, 130, 3),
|
|
|
|
SCHED_TASK(throttle_loop, 50, 75, 6),
|
|
|
|
SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9),
|
2021-12-24 01:47:22 -04:00
|
|
|
#if AP_OPTICALFLOW_ENABLED
|
2022-08-14 22:31:14 -03:00
|
|
|
SCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(update_batt_compass, 10, 120, 15),
|
|
|
|
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50, 18),
|
|
|
|
SCHED_TASK(arm_motors_check, 10, 50, 21),
|
2019-05-30 23:09:25 -03:00
|
|
|
#if TOY_MODE_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(auto_disarm_check, 10, 50, 27),
|
|
|
|
SCHED_TASK(auto_trim, 10, 75, 30),
|
2019-05-30 23:09:25 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(read_rangefinder, 20, 100, 33),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-03-25 05:08:44 -03:00
|
|
|
#if HAL_PROXIMITY_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if BEACON_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(update_altitude, 10, 100, 42),
|
|
|
|
SCHED_TASK(run_nav_updates, 50, 100, 45),
|
|
|
|
SCHED_TASK(update_throttle_hover,100, 90, 48),
|
2019-05-30 23:09:25 -03:00
|
|
|
#if MODE_SMARTRTL_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if SPRAYER_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(three_hz_loop, 3, 75, 57),
|
|
|
|
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75, 60),
|
|
|
|
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63),
|
2019-05-30 23:09:25 -03:00
|
|
|
#if PRECISION_LANDING == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(update_precland, 400, 50, 69),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(check_dynamic_flight, 50, 75, 72),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2022-08-06 12:34:55 -03:00
|
|
|
SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90, 78),
|
|
|
|
SCHED_TASK(one_hz_loop, 1, 100, 81),
|
|
|
|
SCHED_TASK(ekf_check, 10, 75, 84),
|
|
|
|
SCHED_TASK(check_vibration, 10, 50, 87),
|
|
|
|
SCHED_TASK(gpsglitch_check, 10, 50, 90),
|
2022-06-28 08:19:10 -03:00
|
|
|
SCHED_TASK(takeoff_check, 50, 50, 91),
|
2021-02-06 23:29:33 -04:00
|
|
|
#if LANDING_GEAR_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(landinggear_update, 10, 75, 93),
|
2021-02-06 23:29:33 -04:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(standby_update, 100, 75, 96),
|
|
|
|
SCHED_TASK(lost_vehicle_check, 10, 50, 99),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),
|
2020-07-24 14:02:40 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if CAMERA == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(ten_hz_logging_loop, 10, 350, 114),
|
|
|
|
SCHED_TASK(twentyfive_hz_logging, 25, 110, 117),
|
|
|
|
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300, 120),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123),
|
2019-07-18 16:19:08 -03:00
|
|
|
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126),
|
2022-07-15 20:59:12 -03:00
|
|
|
#if AP_RPM_ENABLED
|
2022-01-05 07:23:44 -04:00
|
|
|
SCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
|
2020-09-19 05:37:52 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(avoidance_adsb_update, 10, 100, 138),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(afs_fs_check, 10, 100, 141),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-02-12 23:40:10 -04:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(terrain_update, 10, 100, 144),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if GRIPPER_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if WINCH_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#ifdef USERHOOK_FASTLOOP
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(userhook_FastLoop, 100, 75, 153),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#ifdef USERHOOK_50HZLOOP
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(userhook_50Hz, 50, 75, 156),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#ifdef USERHOOK_MEDIUMLOOP
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(userhook_MediumLoop, 10, 75, 159),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#ifdef USERHOOK_SLOWLOOP
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(userhook_SlowLoop, 3.3, 75, 162),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#ifdef USERHOOK_SUPERSLOWLOOP
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 165),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
2021-08-12 00:38:21 -03:00
|
|
|
#if HAL_BUTTON_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
|
2019-10-06 10:59:15 -03:00
|
|
|
#endif
|
2019-05-30 23:09:25 -03:00
|
|
|
#if STATS_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100, 171),
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
};
|
|
|
|
|
2020-01-16 06:31:50 -04:00
|
|
|
void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit)
|
2019-05-30 23:09:25 -03:00
|
|
|
{
|
2020-01-16 06:31:50 -04:00
|
|
|
tasks = &scheduler_tasks[0];
|
|
|
|
task_count = ARRAY_SIZE(scheduler_tasks);
|
|
|
|
log_bit = MASK_LOG_PM;
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
2020-01-16 06:31:50 -04:00
|
|
|
constexpr int8_t Copter::_failsafe_priorities[7];
|
|
|
|
|
2021-11-15 01:08:33 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2020-03-06 22:29:32 -04:00
|
|
|
// start takeoff to given altitude (for use by scripting)
|
|
|
|
bool Copter::start_takeoff(float alt)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
|
|
|
|
copter.set_auto_armed(true);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2020-02-18 00:11:31 -04:00
|
|
|
// set target location (for use by scripting)
|
|
|
|
bool Copter::set_target_location(const Location& target_loc)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return mode_guided.set_destination(target_loc);
|
|
|
|
}
|
|
|
|
|
2021-08-11 08:24:26 -03:00
|
|
|
// set target position (for use by scripting)
|
|
|
|
bool Copter::set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
|
|
|
|
|
|
|
|
return mode_guided.set_destination(pos_neu_cm, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative, terrain_alt);
|
|
|
|
}
|
|
|
|
|
2021-03-21 13:51:58 -03:00
|
|
|
// set target position and velocity (for use by scripting)
|
|
|
|
bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
|
|
|
|
const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
|
|
|
|
|
2021-08-06 06:41:12 -03:00
|
|
|
return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f());
|
2021-03-21 13:51:58 -03:00
|
|
|
}
|
|
|
|
|
2021-08-11 08:24:26 -03:00
|
|
|
// set target position, velocity and acceleration (for use by scripting)
|
|
|
|
bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
|
|
|
|
const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
|
|
|
|
const Vector3f accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f);
|
|
|
|
|
|
|
|
return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, accel_neu_cms, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative);
|
|
|
|
}
|
|
|
|
|
2020-03-06 22:29:32 -04:00
|
|
|
bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// convert vector to neu in cm
|
|
|
|
const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
|
|
|
|
mode_guided.set_velocity(vel_neu_cms);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2021-08-11 08:24:26 -03:00
|
|
|
// set target velocity and acceleration (for use by scripting)
|
|
|
|
bool Copter::set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// convert vector to neu in cm
|
|
|
|
const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
|
|
|
|
const Vector3f accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f);
|
|
|
|
|
|
|
|
mode_guided.set_velaccel(vel_neu_cms, accel_neu_cms, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, relative_yaw);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-05-31 20:29:13 -03:00
|
|
|
bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in Guided mode or Auto-Guided mode
|
|
|
|
if (!flightmode->in_guided_mode()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
Quaternion q;
|
|
|
|
q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
|
|
|
|
|
2022-01-18 14:01:02 -04:00
|
|
|
mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
|
2020-05-31 20:29:13 -03:00
|
|
|
return true;
|
|
|
|
}
|
2021-08-24 08:11:42 -03:00
|
|
|
|
|
|
|
// circle mode controls
|
|
|
|
bool Copter::get_circle_radius(float &radius_m)
|
|
|
|
{
|
|
|
|
radius_m = circle_nav->get_radius() * 0.01f;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool Copter::set_circle_rate(float rate_dps)
|
|
|
|
{
|
|
|
|
circle_nav->set_rate(rate_dps);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2022-06-30 15:43:32 -03:00
|
|
|
// set desired speed (m/s). Used for scripting.
|
|
|
|
bool Copter::set_desired_speed(float speed)
|
|
|
|
{
|
|
|
|
// exit if vehicle is not in auto mode
|
|
|
|
if (!flightmode->is_autopilot()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
wp_nav->set_speed_xy(speed * 100.0f);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2022-02-17 08:59:16 -04:00
|
|
|
// returns true if mode supports NAV_SCRIPT_TIME mission commands
|
|
|
|
bool Copter::nav_scripting_enable(uint8_t mode)
|
|
|
|
{
|
|
|
|
return mode == (uint8_t)mode_auto.mode_number();
|
|
|
|
}
|
|
|
|
|
|
|
|
// lua scripts use this to retrieve the contents of the active command
|
|
|
|
bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
|
|
|
|
{
|
|
|
|
if (flightmode != &mode_auto) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return mode_auto.nav_script_time(id, cmd, arg1, arg2);
|
|
|
|
}
|
|
|
|
|
|
|
|
// lua scripts use this to indicate when they have complete the command
|
|
|
|
void Copter::nav_script_time_done(uint16_t id)
|
|
|
|
{
|
|
|
|
if (flightmode != &mode_auto) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
return mode_auto.nav_script_time_done(id);
|
|
|
|
}
|
|
|
|
|
2022-04-22 23:19:14 -03:00
|
|
|
// returns true if the EKF failsafe has triggered. Only used by Lua scripts
|
|
|
|
bool Copter::has_ekf_failsafed() const
|
|
|
|
{
|
|
|
|
return failsafe.ekf;
|
|
|
|
}
|
|
|
|
|
2021-11-15 01:08:33 -04:00
|
|
|
#endif // AP_SCRIPTING_ENABLED
|
2020-05-31 20:29:13 -03:00
|
|
|
|
|
|
|
|
2019-05-30 23:09:25 -03:00
|
|
|
// rc_loops - reads user input from transmitter/receiver
|
|
|
|
// called at 100hz
|
|
|
|
void Copter::rc_loop()
|
|
|
|
{
|
|
|
|
// Read radio and 3-position switch on radio
|
|
|
|
// -----------------------------------------
|
|
|
|
read_radio();
|
|
|
|
rc().read_mode_switch();
|
|
|
|
}
|
|
|
|
|
|
|
|
// throttle_loop - should be run at 50 hz
|
|
|
|
// ---------------------------
|
|
|
|
void Copter::throttle_loop()
|
|
|
|
{
|
|
|
|
// update throttle_low_comp value (controls priority of throttle vs attitude control)
|
2020-02-21 02:50:56 -04:00
|
|
|
update_throttle_mix();
|
2019-05-30 23:09:25 -03:00
|
|
|
|
|
|
|
// check auto_armed status
|
|
|
|
update_auto_armed();
|
|
|
|
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
// update rotor speed
|
|
|
|
heli_update_rotor_speed_targets();
|
|
|
|
|
|
|
|
// update trad heli swash plate movement
|
|
|
|
heli_update_landing_swash();
|
|
|
|
#endif
|
|
|
|
|
|
|
|
// compensate for ground effect (if enabled)
|
|
|
|
update_ground_effect_detector();
|
2020-06-22 01:00:44 -03:00
|
|
|
update_ekf_terrain_height_stable();
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// update_batt_compass - read battery and compass
|
|
|
|
// should be called at 10hz
|
|
|
|
void Copter::update_batt_compass(void)
|
|
|
|
{
|
|
|
|
// read battery before compass because it may be used for motor interference compensation
|
|
|
|
battery.read();
|
|
|
|
|
2021-07-27 21:27:39 -03:00
|
|
|
if(AP::compass().available()) {
|
2019-05-30 23:09:25 -03:00
|
|
|
// update compass with throttle value - used for compassmot
|
|
|
|
compass.set_throttle(motors->get_throttle());
|
|
|
|
compass.set_voltage(battery.voltage());
|
|
|
|
compass.read();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Full rate logging of attitude, rate and pid loops
|
2022-08-06 12:34:55 -03:00
|
|
|
// should be run at loop rate
|
|
|
|
void Copter::loop_rate_logging()
|
2019-05-30 23:09:25 -03:00
|
|
|
{
|
2019-10-15 02:38:24 -03:00
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
|
2019-05-30 23:09:25 -03:00
|
|
|
Log_Write_Attitude();
|
2022-08-10 15:19:46 -03:00
|
|
|
Log_Write_PIDS(); // only logs if PIDS bitmask is set
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
2022-08-06 16:07:39 -03:00
|
|
|
if (should_log(MASK_LOG_FTN_FAST)) {
|
|
|
|
AP::ins().write_notch_log_messages();
|
|
|
|
}
|
2022-08-10 15:19:46 -03:00
|
|
|
if (should_log(MASK_LOG_IMU_FAST)) {
|
|
|
|
AP::ins().Write_IMU();
|
|
|
|
}
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// ten_hz_logging_loop
|
|
|
|
// should be run at 10hz
|
|
|
|
void Copter::ten_hz_logging_loop()
|
|
|
|
{
|
|
|
|
// log attitude data if we're not already logging at the higher rate
|
2019-10-15 02:38:24 -03:00
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
|
2019-05-30 23:09:25 -03:00
|
|
|
Log_Write_Attitude();
|
2019-07-29 04:35:12 -03:00
|
|
|
}
|
2022-08-10 15:19:46 -03:00
|
|
|
if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
// log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set
|
|
|
|
Log_Write_PIDS();
|
|
|
|
}
|
|
|
|
// log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
|
|
|
|
if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
|
2019-05-30 23:09:25 -03:00
|
|
|
Log_Write_EKF_POS();
|
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_MOTBATT)) {
|
2021-12-21 16:47:04 -04:00
|
|
|
motors->Log_Write();
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_RCIN)) {
|
|
|
|
logger.Write_RCIN();
|
|
|
|
if (rssi.enabled()) {
|
|
|
|
logger.Write_RSSI();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_RCOUT)) {
|
|
|
|
logger.Write_RCOUT();
|
|
|
|
}
|
2021-05-25 01:36:58 -03:00
|
|
|
if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS() || !flightmode->has_manual_throttle())) {
|
2019-05-30 23:09:25 -03:00
|
|
|
pos_control->write_log();
|
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
|
2021-01-22 01:24:11 -04:00
|
|
|
AP::ins().Write_Vibration();
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_CTUN)) {
|
|
|
|
attitude_control->control_monitor_log();
|
2021-03-25 05:08:44 -03:00
|
|
|
#if HAL_PROXIMITY_ENABLED
|
2022-03-03 21:12:18 -04:00
|
|
|
g2.proximity.log(); // Write proximity sensor distances
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
#if BEACON_ENABLED == ENABLED
|
2022-03-03 21:31:26 -04:00
|
|
|
g2.beacon.log();
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
Log_Write_Heli();
|
|
|
|
#endif
|
2020-07-24 05:38:12 -03:00
|
|
|
#if WINCH_ENABLED == ENABLED
|
|
|
|
if (should_log(MASK_LOG_ANY)) {
|
|
|
|
g2.winch.write_log();
|
|
|
|
}
|
|
|
|
#endif
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// twentyfive_hz_logging - should be run at 25hz
|
|
|
|
void Copter::twentyfive_hz_logging()
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
Log_Write_EKF_POS();
|
|
|
|
}
|
|
|
|
|
2022-08-10 15:19:46 -03:00
|
|
|
if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST))) {
|
2021-01-22 01:24:11 -04:00
|
|
|
AP::ins().Write_IMU();
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
2019-11-28 16:21:07 -04:00
|
|
|
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
|
|
|
|
//update autorotation log
|
|
|
|
g2.arot.Log_Write_Autorotation();
|
|
|
|
}
|
|
|
|
#endif
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// three_hz_loop - 3.3hz loop
|
|
|
|
void Copter::three_hz_loop()
|
|
|
|
{
|
|
|
|
// check if we've lost contact with the ground station
|
|
|
|
failsafe_gcs_check();
|
|
|
|
|
|
|
|
// check if we've lost terrain data
|
|
|
|
failsafe_terrain_check();
|
|
|
|
|
2022-05-10 23:17:13 -03:00
|
|
|
// check for deadreckoning failsafe
|
|
|
|
failsafe_deadreckon_check();
|
|
|
|
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2019-05-30 23:09:25 -03:00
|
|
|
// check if we have breached a fence
|
|
|
|
fence_check();
|
2022-03-04 12:36:07 -04:00
|
|
|
#endif // AP_FENCE_ENABLED
|
2019-05-30 23:09:25 -03:00
|
|
|
|
|
|
|
|
|
|
|
// update ch6 in flight tuning
|
|
|
|
tuning();
|
2021-02-09 13:56:15 -04:00
|
|
|
|
|
|
|
// check if avoidance should be enabled based on alt
|
|
|
|
low_alt_avoidance();
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// one_hz_loop - runs at 1Hz
|
|
|
|
void Copter::one_hz_loop()
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_ANY)) {
|
2019-10-25 03:06:05 -03:00
|
|
|
Log_Write_Data(LogDataID::AP_STATE, ap.value);
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!motors->armed()) {
|
|
|
|
update_using_interlock();
|
|
|
|
|
|
|
|
// check the user hasn't updated the frame class or type
|
|
|
|
motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
|
|
|
|
|
|
|
|
#if FRAME_CONFIG != HELI_FRAME
|
|
|
|
// set all throttle channel settings
|
2021-09-04 14:12:23 -03:00
|
|
|
motors->update_throttle_range();
|
2019-05-30 23:09:25 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
// update assigned functions and enable auxiliary servos
|
|
|
|
SRV_Channels::enable_aux_servos();
|
|
|
|
|
|
|
|
// log terrain data
|
|
|
|
terrain_logging();
|
|
|
|
|
2020-09-19 05:37:52 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2019-05-30 23:09:25 -03:00
|
|
|
adsb.set_is_flying(!ap.land_complete);
|
|
|
|
#endif
|
|
|
|
|
|
|
|
AP_Notify::flags.flying = !ap.land_complete;
|
|
|
|
}
|
|
|
|
|
|
|
|
void Copter::init_simple_bearing()
|
|
|
|
{
|
|
|
|
// capture current cos_yaw and sin_yaw values
|
|
|
|
simple_cos_yaw = ahrs.cos_yaw();
|
|
|
|
simple_sin_yaw = ahrs.sin_yaw();
|
|
|
|
|
|
|
|
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
|
|
|
|
super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
|
|
|
|
super_simple_cos_yaw = simple_cos_yaw;
|
|
|
|
super_simple_sin_yaw = simple_sin_yaw;
|
|
|
|
|
|
|
|
// log the simple bearing
|
|
|
|
if (should_log(MASK_LOG_ANY)) {
|
2019-10-25 03:06:05 -03:00
|
|
|
Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// update_simple_mode - rotates pilot input if we are in simple mode
|
|
|
|
void Copter::update_simple_mode(void)
|
|
|
|
{
|
|
|
|
float rollx, pitchx;
|
|
|
|
|
|
|
|
// exit immediately if no new radio frame or not in simple mode
|
2020-06-15 10:05:09 -03:00
|
|
|
if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
|
2019-05-30 23:09:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// mark radio frame as consumed
|
|
|
|
ap.new_radio_frame = false;
|
|
|
|
|
2020-06-15 10:05:09 -03:00
|
|
|
if (simple_mode == SimpleMode::SIMPLE) {
|
2019-05-30 23:09:25 -03:00
|
|
|
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
|
|
|
|
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
|
|
|
|
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
|
|
|
|
}else{
|
|
|
|
// rotate roll, pitch input by -super simple heading (reverse of heading to home)
|
|
|
|
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
|
|
|
|
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
|
|
|
|
}
|
|
|
|
|
|
|
|
// rotate roll, pitch input from north facing to vehicle's perspective
|
|
|
|
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
|
|
|
|
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
|
|
|
|
}
|
|
|
|
|
|
|
|
// update_super_simple_bearing - adjusts simple bearing based on location
|
|
|
|
// should be called after home_bearing has been updated
|
|
|
|
void Copter::update_super_simple_bearing(bool force_update)
|
|
|
|
{
|
|
|
|
if (!force_update) {
|
2020-06-15 10:05:09 -03:00
|
|
|
if (simple_mode != SimpleMode::SUPERSIMPLE) {
|
2019-05-30 23:09:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (home_distance() < SUPER_SIMPLE_RADIUS) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
const int32_t bearing = home_bearing();
|
|
|
|
|
|
|
|
// check the bearing to home has changed by at least 5 degrees
|
|
|
|
if (labs(super_simple_last_bearing - bearing) < 500) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
super_simple_last_bearing = bearing;
|
|
|
|
const float angle_rad = radians((super_simple_last_bearing+18000)/100);
|
|
|
|
super_simple_cos_yaw = cosf(angle_rad);
|
|
|
|
super_simple_sin_yaw = sinf(angle_rad);
|
|
|
|
}
|
|
|
|
|
|
|
|
void Copter::read_AHRS(void)
|
|
|
|
{
|
|
|
|
// we tell AHRS to skip INS update as we have already done it in fast_loop()
|
|
|
|
ahrs.update(true);
|
|
|
|
}
|
|
|
|
|
|
|
|
// read baro and log control tuning
|
|
|
|
void Copter::update_altitude()
|
|
|
|
{
|
|
|
|
// read in baro altitude
|
|
|
|
read_barometer();
|
|
|
|
|
|
|
|
if (should_log(MASK_LOG_CTUN)) {
|
|
|
|
Log_Write_Control_Tuning();
|
2022-08-06 16:07:39 -03:00
|
|
|
if (!should_log(MASK_LOG_FTN_FAST)) {
|
|
|
|
AP::ins().write_notch_log_messages();
|
|
|
|
}
|
2019-07-18 16:19:08 -03:00
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
gyro_fft.write_log_messages();
|
|
|
|
#endif
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-12-05 14:44:07 -04:00
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Copter::get_wp_distance_m(float &distance) const
|
2019-05-30 23:09:25 -03:00
|
|
|
{
|
2020-12-05 14:44:07 -04:00
|
|
|
// see GCS_MAVLINK_Copter::send_nav_controller_output()
|
|
|
|
distance = flightmode->wp_distance() * 0.01;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Copter::get_wp_bearing_deg(float &bearing) const
|
|
|
|
{
|
|
|
|
// see GCS_MAVLINK_Copter::send_nav_controller_output()
|
|
|
|
bearing = flightmode->wp_bearing() * 0.01;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
|
|
|
|
{
|
|
|
|
// see GCS_MAVLINK_Copter::send_nav_controller_output()
|
|
|
|
xtrack_error = flightmode->crosstrack_error() * 0.01;
|
|
|
|
return true;
|
2019-05-30 23:09:25 -03:00
|
|
|
}
|
|
|
|
|
2022-06-10 01:06:02 -03:00
|
|
|
// get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals)
|
|
|
|
bool Copter::get_rate_bf_targets(Vector3f& rate_bf_targets) const
|
|
|
|
{
|
|
|
|
rate_bf_targets = attitude_control->rate_bf_targets();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2016-05-05 19:10:08 -03:00
|
|
|
/*
|
|
|
|
constructor for main Copter class
|
|
|
|
*/
|
2017-08-23 17:51:38 -03:00
|
|
|
Copter::Copter(void)
|
2019-01-18 00:23:42 -04:00
|
|
|
: logger(g.log_bitmask),
|
2015-05-29 23:12:49 -03:00
|
|
|
flight_modes(&g.flight_mode1),
|
|
|
|
simple_cos_yaw(1.0f),
|
|
|
|
super_simple_cos_yaw(1.0),
|
|
|
|
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
|
2016-01-06 17:39:36 -04:00
|
|
|
rc_throttle_control_in_filter(1.0f),
|
2015-05-29 23:12:49 -03:00
|
|
|
inertial_nav(ahrs),
|
2017-11-09 22:45:34 -04:00
|
|
|
param_loader(var_info),
|
2017-12-11 01:43:27 -04:00
|
|
|
flightmode(&mode_stabilize)
|
2015-05-29 23:12:49 -03:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
Copter copter;
|
2019-11-10 16:13:17 -04:00
|
|
|
AP_Vehicle& vehicle = copter;
|
2019-05-30 23:09:25 -03:00
|
|
|
|
|
|
|
AP_HAL_MAIN_CALLBACKS(&copter);
|