ardupilot/ArduCopter/Copter.cpp
Andy Piper f84c855dd1 Copter: run copter attitude control with separate rate thread
run motors output at rate thread loop rate
allow rate thread to be enabled/disabled at runtime for in-flight impact testing
setup the right PID notch sample rate when using the rate thread the PID notches
 run at a very different sample rate
call update_dynamic_notch_at_specified_rate() in rate thread
log RTDT messages to track rate loop performance
set dt each cycle of the rate loop thread
run rate controller on samples as soon as they are ready
detect overload conditions in both the rate loop and main loop
decimate the rate thread if the CPU appears overloaded
decimate the gyro window inside the IMU
add in gyro drift to attitude rate thread
add fixed-rate thread option
configure rate loop based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
better rate loop thread decimation management
ensure fix rate attitude is enabled on arming
add rate loop timing debug
update backend filters rather than all the backends
provide more options around attitude rates
only log attitude and IMU from main loop
force trigger_groups() and reduce attitude thread priority
migrate fast rate enablement to FSTRATE_ENABLE
remove rate thread logging configuration and choose sensible logging rates
conditionally compile rate thread pieces
allow fast rate decimation to be user throttled
if target rate changes immediately jump to target rate
recover quickly from rate changes
ensure fixed rate always prints the rate on arming and is always up to date
add support for fixed rate attitude that does not change when disarmed
only push to subsystems at main loop rate
add logging and motor timing debug
correctly round gyro decimation rates
set dshot rate when changing attitude rate
fallback to higher dshot rates at lower loop rates
re-factor rate loop rate updates
log rates in systemid mode
reset target modifiers at loop rate
don't compile in support on tradheli
move rate thread into its own compilation unit
add rate loop config abstraction that allows code to be elided on non-copter builds
dynamically enable/disable rate thread correctly
add design comment for the rate thread

Co-authored-by: Andrew Tridgell <andrew@tridgell.net>
2024-12-04 07:45:05 +11:00

983 lines
33 KiB
C++

/*
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/>.
*/
/*
* 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
* Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
*
* 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
* Andy Piper :Harmonic notch, In-flight FFT, Bi-directional DShot, various drivers
* 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
* Doug Weibel :DCM, Libraries, Control law advice
* 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
* John Arne Birkeland :PPM Encoder
* 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
* Wiki: https://copter.ardupilot.org/
*
*/
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#define FORCE_VERSION_H_INCLUDE
#include "version.h"
#undef FORCE_VERSION_H_INCLUDE
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define SCHED_TASK(func, _interval_ticks, _max_time_micros, _prio) SCHED_TASK_CLASS(Copter, &copter, func, _interval_ticks, _max_time_micros, _prio)
#define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)
/*
scheduler table - all tasks should be listed here.
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)
*/
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
// 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_main),
#if AC_CUSTOMCONTROL_MULTI_ENABLED
FAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAME
FAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME
// send outputs to the motors library immediately
FAST_TASK(motors_output_main),
// run EKF state estimator (expensive)
FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAME
FAST_TASK(update_heli_control_dynamics),
#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),
// surface tracking update
FAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED
// camera mount's fast update
FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endif
#if HAL_LOGGING_ENABLED
FAST_TASK(Log_Video_Stabilisation),
#endif
SCHED_TASK(rc_loop, 250, 130, 3),
SCHED_TASK(throttle_loop, 50, 75, 6),
#if AP_FENCE_ENABLED
SCHED_TASK(fence_check, 25, 100, 7),
#endif
SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9),
#if AP_OPTICALFLOW_ENABLED
SCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12),
#endif
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),
#if TOY_MODE_ENABLED
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
#endif
SCHED_TASK(auto_disarm_check, 10, 50, 27),
SCHED_TASK(auto_trim, 10, 75, 30),
#if AP_RANGEFINDER_ENABLED
SCHED_TASK(read_rangefinder, 20, 100, 33),
#endif
#if HAL_PROXIMITY_ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36),
#endif
#if AP_BEACON_ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39),
#endif
SCHED_TASK(update_altitude, 10, 100, 42),
SCHED_TASK(run_nav_updates, 50, 100, 45),
SCHED_TASK(update_throttle_hover,100, 90, 48),
#if MODE_SMARTRTL_ENABLED
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
#endif
#if HAL_SPRAYER_ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54),
#endif
SCHED_TASK(three_hz_loop, 3, 75, 57),
#if AP_SERVORELAYEVENTS_ENABLED
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75, 60),
#endif
#if AC_PRECLAND_ENABLED
SCHED_TASK(update_precland, 400, 50, 69),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75, 72),
#endif
#if HAL_LOGGING_ENABLED
SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75),
#endif
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),
SCHED_TASK(takeoff_check, 50, 50, 91),
#if AP_LANDINGGEAR_ENABLED
SCHED_TASK(landinggear_update, 10, 75, 93),
#endif
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),
#if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108),
#endif
#if AP_CAMERA_ENABLED
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111),
#endif
#if HAL_LOGGING_ENABLED
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),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123),
#if HAL_LOGGING_ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126),
#endif
#if AP_RPM_ENABLED
SCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129),
#endif
#if AP_TEMPCALIBRATION_ENABLED
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
#endif
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100, 138),
#endif
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
SCHED_TASK(afs_fs_check, 10, 100, 141),
#endif
#if AP_TERRAIN_AVAILABLE
SCHED_TASK(terrain_update, 10, 100, 144),
#endif
#if AP_WINCH_ENABLED
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75, 153),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75, 159),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 165),
#endif
#if HAL_BUTTON_ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
#endif
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215),
#endif
};
void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
uint8_t &task_count,
uint32_t &log_bit)
{
tasks = &scheduler_tasks[0];
task_count = ARRAY_SIZE(scheduler_tasks);
log_bit = MASK_LOG_PM;
}
constexpr int8_t Copter::_failsafe_priorities[7];
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if MODE_GUIDED_ENABLED
// set target location (for use by external control and 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);
}
// start takeoff to given altitude (for use by scripting)
bool Copter::start_takeoff(const 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;
}
#endif //MODE_GUIDED_ENABLED
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
#if AP_SCRIPTING_ENABLED
#if MODE_GUIDED_ENABLED
// 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);
}
// 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);
return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f());
}
// 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);
}
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;
}
// 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;
}
// set target roll pitch and yaw angles with throttle (for use by scripting)
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));
mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
return true;
}
// set target roll pitch and yaw rates with throttle (for use by scripting)
bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
// Zero quaternion indicates rate control
Quaternion q;
q.zero();
// Convert from degrees per second to radians per second
Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
ang_vel_body *= DEG_TO_RAD;
// Pass to guided mode
mode_guided.set_angle(q, ang_vel_body, throttle, true);
return true;
}
// Register a custom mode with given number and names
AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
{
const Mode::Number number = (Mode::Number)num;
// See if this mode has been registered already, if it has return the state for it
// This allows scripting restarts
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
if (mode_guided_custom[i] == nullptr) {
break;
}
if ((mode_guided_custom[i]->mode_number() == number) &&
(strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
(strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
return &mode_guided_custom[i]->state;
}
}
// Number already registered to existing mode
if (mode_from_mode_num(number) != nullptr) {
return nullptr;
}
// Find free slot
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
if (mode_guided_custom[i] == nullptr) {
// Duplicate strings so were not pointing to unknown memory
const char* full_name_copy = strdup(full_name);
const char* short_name_copy = strndup(short_name, 4);
if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
}
if (mode_guided_custom[i] == nullptr) {
// Allocation failure
return nullptr;
}
// Registration sucsessful, notify the GCS that it should re-request the avalable modes
gcs().available_modes_changed();
return &mode_guided_custom[i]->state;
}
}
// No free slots
return nullptr;
}
#endif // MODE_GUIDED_ENABLED
#if MODE_CIRCLE_ENABLED
// 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;
}
#endif
// set desired speed (m/s). Used for scripting.
bool Copter::set_desired_speed(float speed)
{
return flightmode->set_speed_xy(speed * 100.0f);
}
#if MODE_AUTO_ENABLED
// 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, int16_t &arg3, int16_t &arg4)
{
if (flightmode != &mode_auto) {
return false;
}
return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4);
}
// 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);
}
#endif
// returns true if the EKF failsafe has triggered. Only used by Lua scripts
bool Copter::has_ekf_failsafed() const
{
return failsafe.ekf;
}
// get target location (for use by scripting)
bool Copter::get_target_location(Location& target_loc)
{
return flightmode->get_wp(target_loc);
}
/*
update_target_location() acts as a wrapper for set_target_location
*/
bool Copter::update_target_location(const Location &old_loc, const Location &new_loc)
{
/*
by checking the caller has provided the correct old target
location we prevent a race condition where the user changes mode
or commands a different target in the controlling lua script
*/
Location next_WP_loc;
flightmode->get_wp(next_WP_loc);
if (!old_loc.same_loc_as(next_WP_loc) ||
old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
return false;
}
return set_target_location(new_loc);
}
#endif // AP_SCRIPTING_ENABLED
// returns true if vehicle is landing.
bool Copter::is_landing() const
{
return flightmode->is_landing();
}
// returns true if vehicle is taking off.
bool Copter::is_taking_off() const
{
return flightmode->is_taking_off();
}
bool Copter::current_mode_requires_mission() const
{
#if MODE_AUTO_ENABLED
return flightmode == &mode_auto;
#else
return false;
#endif
}
// 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)
update_throttle_mix();
// 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();
update_ekf_terrain_height_stable();
}
// 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();
if(AP::compass().available()) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors->get_throttle());
compass.set_voltage(battery.voltage());
compass.read();
}
}
#if HAL_LOGGING_ENABLED
// Full rate logging of attitude, rate and pid loops
// should be run at loop rate
void Copter::loop_rate_logging()
{
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
if (!using_rate_thread) {
Log_Write_Rate();
Log_Write_PIDS(); // only logs if PIDS bitmask is set
}
}
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
AP::ins().write_notch_log_messages();
}
#endif
if (should_log(MASK_LOG_IMU_FAST)) {
AP::ins().Write_IMU();
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
{
// always write AHRS attitude at 10Hz
ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
// log attitude controller data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
if (!using_rate_thread) {
Log_Write_Rate();
}
}
if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
// 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
if (!using_rate_thread) {
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)) {
Log_Write_EKF_POS();
}
if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) {
// always write motors log if we are a heli
motors->Log_Write();
}
if (should_log(MASK_LOG_RCIN)) {
logger.Write_RCIN();
#if AP_RSSI_ENABLED
if (rssi.enabled()) {
logger.Write_RSSI();
}
#endif
}
if (should_log(MASK_LOG_RCOUT)) {
logger.Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS() || !flightmode->has_manual_throttle())) {
pos_control->write_log();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
AP::ins().Write_Vibration();
}
if (should_log(MASK_LOG_CTUN)) {
attitude_control->control_monitor_log();
#if HAL_PROXIMITY_ENABLED
g2.proximity.log(); // Write proximity sensor distances
#endif
#if AP_BEACON_ENABLED
g2.beacon.log();
#endif
}
#if AP_WINCH_ENABLED
if (should_log(MASK_LOG_ANY)) {
g2.winch.write_log();
}
#endif
#if HAL_MOUNT_ENABLED
if (should_log(MASK_LOG_CAMERA)) {
camera_mount.write_log();
}
#endif
}
// twentyfive_hz_logging - should be run at 25hz
void Copter::twentyfive_hz_logging()
{
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_EKF_POS();
}
if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST))) {
AP::ins().Write_IMU();
}
#if MODE_AUTOROTATE_ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
#if HAL_GYROFFT_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
gyro_fft.write_log_messages();
}
#endif
}
#endif // HAL_LOGGING_ENABLED
// three_hz_loop - 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();
// check for deadreckoning failsafe
failsafe_deadreckon_check();
//update transmitter based in flight tuning
tuning();
// check if avoidance should be enabled based on alt
low_alt_avoidance();
}
// ap_value calculates a 32-bit bitmask representing various pieces of
// state about the Copter. It replaces a global variable which was
// used to track this state.
uint32_t Copter::ap_value() const
{
uint32_t ret = 0;
const bool *b = (const bool *)&ap;
for (uint8_t i=0; i<sizeof(ap); i++) {
if (b[i]) {
ret |= 1U<<i;
}
}
return ret;
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
#if HAL_LOGGING_ENABLED
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::AP_STATE, ap_value());
}
#endif
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
motors->update_throttle_range();
#endif
}
// update assigned functions and enable auxiliary servos
AP::srv().enable_aux_servos();
#if HAL_LOGGING_ENABLED
// log terrain data
terrain_logging();
#endif
#if HAL_ADSB_ENABLED
adsb.set_is_flying(!ap.land_complete);
#endif
AP_Notify::flags.flying = !ap.land_complete;
// slowly update the PID notches with the average loop rate
if (!using_rate_thread) {
attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
}
pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
#if AC_CUSTOMCONTROL_MULTI_ENABLED
custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
#endif
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// see if we should have a separate rate thread
if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
"rate",
1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
started_rate_thread = true;
}
}
#endif
}
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;
#if HAL_LOGGING_ENABLED
// log the simple bearing
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
}
#endif
}
// 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
if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
return;
}
// mark radio frame as consumed
ap.new_radio_frame = false;
if (simple_mode == SimpleMode::SIMPLE) {
// 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) {
if (simple_mode != SimpleMode::SUPERSIMPLE) {
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_TASK.
ahrs.update(true);
}
// read baro and log control tuning
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
#if HAL_LOGGING_ENABLED
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
if (!should_log(MASK_LOG_FTN_FAST)) {
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
AP::ins().write_notch_log_messages();
#endif
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#endif
}
}
#endif
}
// vehicle specific waypoint info helpers
bool Copter::get_wp_distance_m(float &distance) const
{
// 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;
}
// get the target earth-frame angular velocities in rad/s (Z-axis component used by some gimbals)
bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const
{
// always returns zero vector if landed or disarmed
if (copter.ap.land_complete) {
rate_ef_targets.zero();
} else {
rate_ef_targets = attitude_control->get_rate_ef_targets();
}
return true;
}
/*
constructor for main Copter class
*/
Copter::Copter(void)
:
flight_modes(&g.flight_mode1),
pos_variance_filt(FS_EKF_FILT_DEFAULT),
vel_variance_filt(FS_EKF_FILT_DEFAULT),
hgt_variance_filt(FS_EKF_FILT_DEFAULT),
flightmode(&mode_stabilize),
simple_cos_yaw(1.0f),
super_simple_cos_yaw(1.0),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
rc_throttle_control_in_filter(1.0f),
inertial_nav(ahrs),
param_loader(var_info)
{
}
Copter copter;
AP_Vehicle& vehicle = copter;
AP_HAL_MAIN_CALLBACKS(&copter);