ArduCopter/Copter.cpp

   1 /*
   2    This program is free software: you can redistribute it and/or modify
   3    it under the terms of the GNU General Public License as published by
   4    the Free Software Foundation, either version 3 of the License, or
   5    (at your option) any later version.
   6
   7    This program is distributed in the hope that it will be useful,
   8    but WITHOUT ANY WARRANTY; without even the implied warranty of
   9    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10    GNU General Public License for more details.
  11
  12    You should have received a copy of the GNU General Public License
  13    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  14  */
  15
  16 /*
  17  *  ArduCopter (also known as APM, APM:Copter or just Copter)
  18  *  Wiki:           copter.ardupilot.org
  19  *  Creator:        Jason Short
  20  *  Lead Developer: Randy Mackay
  21  *  Lead Tester:    Marco Robustini
  22  *  Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen,
  23                                                       Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
  24                                                       Jean-Louis Naudin, Mike Smith, and more
  25  *  Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
  26  *
  27  *  Special Thanks to contributors (in alphabetical order by first name):
  28  *
  29  *  Adam M Rivera       :Auto Compass Declination
  30  *  Amilcar Lucas       :Camera mount library
  31  *  Andrew Tridgell     :General development, Mavlink Support
  32  *  Andy Piper          :Harmonic notch, In-flight FFT, Bi-directional DShot, various drivers
  33  *  Angel Fernandez     :Alpha testing
  34  *  AndreasAntonopoulous:GeoFence
  35  *  Arthur Benemann     :DroidPlanner GCS
  36  *  Benjamin Pelletier  :Libraries
  37  *  Bill King           :Single Copter
  38  *  Christof Schmid     :Alpha testing
  39  *  Craig Elder         :Release Management, Support
  40  *  Dani Saez           :V Octo Support
  41  *  Doug Weibel         :DCM, Libraries, Control law advice
  42  *  Emile Castelnuovo   :VRBrain port, bug fixes
  43  *  Gregory Fletcher    :Camera mount orientation math
  44  *  Guntars             :Arming safety suggestion
  45  *  HappyKillmore       :Mavlink GCS
  46  *  Hein Hollander      :Octo Support, Heli Testing
  47  *  Igor van Airde      :Control Law optimization
  48  *  Jack Dunkle         :Alpha testing
  49  *  James Goppert       :Mavlink Support
  50  *  Jani Hiriven        :Testing feedback
  51  *  Jean-Louis Naudin   :Auto Landing
  52  *  John Arne Birkeland :PPM Encoder
  53  *  Jose Julio          :Stabilization Control laws, MPU6k driver
  54  *  Julien Dubois       :PosHold flight mode
  55  *  Julian Oes          :Pixhawk
  56  *  Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
  57  *  Kevin Hester        :Andropilot GCS
  58  *  Max Levine          :Tri Support, Graphics
  59  *  Leonard Hall        :Flight Dynamics, Throttle, Loiter and Navigation Controllers
  60  *  Marco Robustini     :Lead tester
  61  *  Michael Oborne      :Mission Planner GCS
  62  *  Mike Smith          :Pixhawk driver, coding support
  63  *  Olivier Adler       :PPM Encoder, piezo buzzer
  64  *  Pat Hickey          :Hardware Abstraction Layer (HAL)
  65  *  Robert Lefebvre     :Heli Support, Copter LEDs
  66  *  Roberto Navoni      :Library testing, Porting to VRBrain
  67  *  Sandro Benigno      :Camera support, MinimOSD
  68  *  Sandro Tognana      :PosHold flight mode
  69  *  Sebastian Quilter   :SmartRTL
  70  *  ..and many more.
  71  *
  72  *  Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors
  73  *  Wiki: https://copter.ardupilot.org/
  74  *
  75  */
  76
  77 #include "Copter.h"
  78 #include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
  79
  80 #define FORCE_VERSION_H_INCLUDE
  81 #include "version.h"
  82 #undef FORCE_VERSION_H_INCLUDE
  83
  84 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
  85
  86 #define SCHED_TASK(func, _interval_ticks, _max_time_micros, _prio) SCHED_TASK_CLASS(Copter, &copter, func, _interval_ticks, _max_time_micros, _prio)
  87 #define FAST_TASK(func) FAST_TASK_CLASS(Copter, &copter, func)
  88
  89 /*
  90   scheduler table - all tasks should be listed here.
  91
  92   All entries in this table must be ordered by priority.
  93
  94   This table is interleaved with the table in AP_Vehicle to determine
  95   the order in which tasks are run.  Convenience methods SCHED_TASK
  96   and SCHED_TASK_CLASS are provided to build entries in this structure:
  97
  98 SCHED_TASK arguments:
  99  - name of static function to call
 100  - rate (in Hertz) at which the function should be called
 101  - expected time (in MicroSeconds) that the function should take to run
 102  - priority (0 through 255, lower number meaning higher priority)
 103
 104 SCHED_TASK_CLASS arguments:
 105  - class name of method to be called
 106  - instance on which to call the method
 107  - method to call on that instance
 108  - rate (in Hertz) at which the method should be called
 109  - expected time (in MicroSeconds) that the method should take to run
 110  - priority (0 through 255, lower number meaning higher priority)
 111
 112  */
 113 const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 114     // update INS immediately to get current gyro data populated
 115     FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
 116     // run low level rate controllers that only require IMU data
 117     FAST_TASK(run_rate_controller_main),
 118 #if AC_CUSTOMCONTROL_MULTI_ENABLED
 119     FAST_TASK(run_custom_controller),
 120 #endif
 121 #if FRAME_CONFIG == HELI_FRAME
 122     FAST_TASK(heli_update_autorotation),
 123 #endif //HELI_FRAME
 124     // send outputs to the motors library immediately
 125     FAST_TASK(motors_output_main),
 126      // run EKF state estimator (expensive)
 127     FAST_TASK(read_AHRS),
 128 #if FRAME_CONFIG == HELI_FRAME
 129     FAST_TASK(update_heli_control_dynamics),
 130 #endif //HELI_FRAME
 131     // Inertial Nav
 132     FAST_TASK(read_inertia),
 133     // check if ekf has reset target heading or position
 134     FAST_TASK(check_ekf_reset),
 135     // run the attitude controllers
 136     FAST_TASK(update_flight_mode),
 137     // update home from EKF if necessary
 138     FAST_TASK(update_home_from_EKF),
 139     // check if we've landed or crashed
 140     FAST_TASK(update_land_and_crash_detectors),
 141     // surface tracking update
 142     FAST_TASK(update_rangefinder_terrain_offset),
 143 #if HAL_MOUNT_ENABLED
 144     // camera mount's fast update
 145     FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
 146 #endif
 147 #if HAL_LOGGING_ENABLED
 148     FAST_TASK(Log_Video_Stabilisation),
 149 #endif
 150
 151     SCHED_TASK(rc_loop,              250,    130,  3),
 152     SCHED_TASK(throttle_loop,         50,     75,  6),
 153 #if AP_FENCE_ENABLED
 154     SCHED_TASK(fence_check,           25,    100,  7),
 155 #endif
 156     SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
 157 #if AP_OPTICALFLOW_ENABLED
 158     SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
 159 #endif
 160     SCHED_TASK(update_batt_compass,   10,    120, 15),
 161     SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
 162     SCHED_TASK(arm_motors_check,      10,     50, 21),
 163 #if TOY_MODE_ENABLED
 164     SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
 165 #endif
 166     SCHED_TASK(auto_disarm_check,     10,     50,  27),
 167     SCHED_TASK(auto_trim,             10,     75,  30),
 168 #if AP_RANGEFINDER_ENABLED
 169     SCHED_TASK(read_rangefinder,      20,    100,  33),
 170 #endif
 171 #if HAL_PROXIMITY_ENABLED
 172     SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
 173 #endif
 174 #if AP_BEACON_ENABLED
 175     SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),
 176 #endif
 177     SCHED_TASK(update_altitude,       10,    100,  42),
 178     SCHED_TASK(run_nav_updates,       50,    100,  45),
 179     SCHED_TASK(update_throttle_hover,100,     90,  48),
 180 #if MODE_SMARTRTL_ENABLED
 181     SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
 182 #endif
 183 #if HAL_SPRAYER_ENABLED
 184     SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),
 185 #endif
 186     SCHED_TASK(three_hz_loop,          3,     75, 57),
 187 #if AP_SERVORELAYEVENTS_ENABLED
 188     SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
 189 #endif
 190 #if AC_PRECLAND_ENABLED
 191     SCHED_TASK(update_precland,      400,     50,  69),
 192 #endif
 193 #if FRAME_CONFIG == HELI_FRAME
 194     SCHED_TASK(check_dynamic_flight,  50,     75,  72),
 195 #endif
 196 #if HAL_LOGGING_ENABLED
 197     SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
 198 #endif
 199     SCHED_TASK(one_hz_loop,            1,    100,  81),
 200     SCHED_TASK(ekf_check,             10,     75,  84),
 201     SCHED_TASK(check_vibration,       10,     50,  87),
 202     SCHED_TASK(gpsglitch_check,       10,     50,  90),
 203     SCHED_TASK(takeoff_check,         50,     50,  91),
 204 #if AP_LANDINGGEAR_ENABLED
 205     SCHED_TASK(landinggear_update,    10,     75,  93),
 206 #endif
 207     SCHED_TASK(standby_update,        100,    75,  96),
 208     SCHED_TASK(lost_vehicle_check,    10,     50,  99),
 209     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
 210     SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550, 105),
 211 #if HAL_MOUNT_ENABLED
 212     SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),
 213 #endif
 214 #if AP_CAMERA_ENABLED
 215     SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),
 216 #endif
 217 #if HAL_LOGGING_ENABLED
 218     SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
 219     SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
 220     SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),
 221 #endif
 222     SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50, 123),
 223
 224 #if HAL_LOGGING_ENABLED
 225     SCHED_TASK_CLASS(AP_Scheduler,         &copter.scheduler,           update_logging, 0.1,  75, 126),
 226 #endif
 227 #if AP_RPM_ENABLED
 228     SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),
 229 #endif
 230 #if AP_TEMPCALIBRATION_ENABLED
 231     SCHED_TASK_CLASS(AP_TempCalibration,   &copter.g2.temp_calibration, update,          10, 100, 135),
 232 #endif
 233 #if HAL_ADSB_ENABLED
 234     SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
 235 #endif
 236 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
 237     SCHED_TASK(afs_fs_check,          10,    100, 141),
 238 #endif
 239 #if AP_TERRAIN_AVAILABLE
 240     SCHED_TASK(terrain_update,        10,    100, 144),
 241 #endif
 242 #if AP_WINCH_ENABLED
 243     SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
 244 #endif
 245 #ifdef USERHOOK_FASTLOOP
 246     SCHED_TASK(userhook_FastLoop,    100,     75, 153),
 247 #endif
 248 #ifdef USERHOOK_50HZLOOP
 249     SCHED_TASK(userhook_50Hz,         50,     75, 156),
 250 #endif
 251 #ifdef USERHOOK_MEDIUMLOOP
 252     SCHED_TASK(userhook_MediumLoop,   10,     75, 159),
 253 #endif
 254 #ifdef USERHOOK_SLOWLOOP
 255     SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),
 256 #endif
 257 #ifdef USERHOOK_SUPERSLOWLOOP
 258     SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),
 259 #endif
 260 #if HAL_BUTTON_ENABLED
 261     SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
 262 #endif
 263 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 264     // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case
 265     SCHED_TASK(update_dynamic_notch_at_specified_rate_main,                       LOOP_RATE, 200, 215),
 266 #endif
 267 };
 268
 269 void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
 270                                  uint8_t &task_count,
 271                                  uint32_t &log_bit)
 272 {
 273     tasks = &scheduler_tasks[0];
 274     task_count = ARRAY_SIZE(scheduler_tasks);
 275     log_bit = MASK_LOG_PM;
 276 }
 277
 278 constexpr int8_t Copter::_failsafe_priorities[7];
 279
 280
 281 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 282 #if MODE_GUIDED_ENABLED
 283 // set target location (for use by external control and scripting)
 284 bool Copter::set_target_location(const Location& target_loc)
 285 {
 286     // exit if vehicle is not in Guided mode or Auto-Guided mode
 287     if (!flightmode->in_guided_mode()) {
 288         return false;
 289     }
 290
 291     return mode_guided.set_destination(target_loc);
 292 }
 293
 294 // start takeoff to given altitude (for use by scripting)
 295 bool Copter::start_takeoff(const float alt)
 296 {
 297     // exit if vehicle is not in Guided mode or Auto-Guided mode
 298     if (!flightmode->in_guided_mode()) {
 299         return false;
 300     }
 301
 302     if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
 303         copter.set_auto_armed(true);
 304         return true;
 305     }
 306     return false;
 307 }
 308 #endif //MODE_GUIDED_ENABLED
 309 #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 310
 311 #if AP_SCRIPTING_ENABLED
 312 #if MODE_GUIDED_ENABLED
 313 // set target position (for use by scripting)
 314 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)
 315 {
 316     // exit if vehicle is not in Guided mode or Auto-Guided mode
 317     if (!flightmode->in_guided_mode()) {
 318         return false;
 319     }
 320
 321     const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
 322
 323     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);
 324 }
 325
 326 // set target position and velocity (for use by scripting)
 327 bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel)
 328 {
 329     // exit if vehicle is not in Guided mode or Auto-Guided mode
 330     if (!flightmode->in_guided_mode()) {
 331         return false;
 332     }
 333
 334     const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
 335     const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
 336
 337     return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f());
 338 }
 339
 340 // set target position, velocity and acceleration (for use by scripting)
 341 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)
 342 {
 343     // exit if vehicle is not in Guided mode or Auto-Guided mode
 344     if (!flightmode->in_guided_mode()) {
 345         return false;
 346     }
 347
 348     const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
 349     const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
 350     const Vector3f accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f);
 351
 352     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);
 353 }
 354
 355 bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
 356 {
 357     // exit if vehicle is not in Guided mode or Auto-Guided mode
 358     if (!flightmode->in_guided_mode()) {
 359         return false;
 360     }
 361
 362     // convert vector to neu in cm
 363     const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
 364     mode_guided.set_velocity(vel_neu_cms);
 365     return true;
 366 }
 367
 368 // set target velocity and acceleration (for use by scripting)
 369 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)
 370 {
 371     // exit if vehicle is not in Guided mode or Auto-Guided mode
 372     if (!flightmode->in_guided_mode()) {
 373         return false;
 374     }
 375
 376     // convert vector to neu in cm
 377     const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
 378     const Vector3f accel_neu_cms(target_accel.x * 100.0f, target_accel.y * 100.0f, -target_accel.z * 100.0f);
 379
 380     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);
 381     return true;
 382 }
 383
 384 // set target roll pitch and yaw angles with throttle (for use by scripting)
 385 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)
 386 {
 387     // exit if vehicle is not in Guided mode or Auto-Guided mode
 388     if (!flightmode->in_guided_mode()) {
 389         return false;
 390     }
 391
 392     Quaternion q;
 393     q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
 394
 395     mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
 396     return true;
 397 }
 398
 399 // set target roll pitch and yaw rates with throttle (for use by scripting)
 400 bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
 401 {
 402     // exit if vehicle is not in Guided mode or Auto-Guided mode
 403     if (!flightmode->in_guided_mode()) {
 404         return false;
 405     }
 406
 407     // Zero quaternion indicates rate control
 408     Quaternion q;
 409     q.zero();
 410
 411     // Convert from degrees per second to radians per second
 412     Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
 413     ang_vel_body *= DEG_TO_RAD;
 414
 415     // Pass to guided mode
 416     mode_guided.set_angle(q, ang_vel_body, throttle, true);
 417     return true;
 418 }
 419
 420 // Register a custom mode with given number and names
 421 AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
 422 {
 423     const Mode::Number number = (Mode::Number)num;
 424
 425     // See if this mode has been registered already, if it has return the state for it
 426     // This allows scripting restarts
 427     for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
 428         if (mode_guided_custom[i] == nullptr) {
 429             break;
 430         }
 431         if ((mode_guided_custom[i]->mode_number() == number) &&
 432             (strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
 433             (strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
 434             return &mode_guided_custom[i]->state;
 435         }
 436     }
 437
 438     // Number already registered to existing mode
 439     if (mode_from_mode_num(number) != nullptr) {
 440         return nullptr;
 441     }
 442
 443     // Find free slot
 444     for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
 445         if (mode_guided_custom[i] == nullptr) {
 446             // Duplicate strings so were not pointing to unknown memory
 447             const char* full_name_copy = strdup(full_name);
 448             const char* short_name_copy = strndup(short_name, 4);
 449             if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
 450                 mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
 451             }
 452             if (mode_guided_custom[i] == nullptr) {
 453                 // Allocation failure
 454                 free((void*)full_name_copy);
 455                 free((void*)short_name_copy);
 456                 return nullptr;
 457             }
 458
 459             // Registration sucsessful, notify the GCS that it should re-request the avalable modes
 460             gcs().available_modes_changed();
 461
 462             return &mode_guided_custom[i]->state;
 463         }
 464     }
 465
 466     // No free slots
 467     return nullptr;
 468 }
 469 #endif // MODE_GUIDED_ENABLED
 470
 471 #if MODE_CIRCLE_ENABLED
 472 // circle mode controls
 473 bool Copter::get_circle_radius(float &radius_m)
 474 {
 475     radius_m = circle_nav->get_radius() * 0.01f;
 476     return true;
 477 }
 478
 479 bool Copter::set_circle_rate(float rate_dps)
 480 {
 481     circle_nav->set_rate(rate_dps);
 482     return true;
 483 }
 484 #endif
 485
 486 // set desired speed (m/s). Used for scripting.
 487 bool Copter::set_desired_speed(float speed)
 488 {
 489     return flightmode->set_speed_xy(speed * 100.0f);
 490 }
 491
 492 #if MODE_AUTO_ENABLED
 493 // returns true if mode supports NAV_SCRIPT_TIME mission commands
 494 bool Copter::nav_scripting_enable(uint8_t mode)
 495 {
 496     return mode == (uint8_t)mode_auto.mode_number();
 497 }
 498
 499 // lua scripts use this to retrieve the contents of the active command
 500 bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
 501 {
 502     if (flightmode != &mode_auto) {
 503         return false;
 504     }
 505
 506     return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4);
 507 }
 508
 509 // lua scripts use this to indicate when they have complete the command
 510 void Copter::nav_script_time_done(uint16_t id)
 511 {
 512     if (flightmode != &mode_auto) {
 513         return;
 514     }
 515
 516     return mode_auto.nav_script_time_done(id);
 517 }
 518 #endif
 519
 520 // returns true if the EKF failsafe has triggered.  Only used by Lua scripts
 521 bool Copter::has_ekf_failsafed() const
 522 {
 523     return failsafe.ekf;
 524 }
 525
 526 // get target location (for use by scripting)
 527 bool Copter::get_target_location(Location& target_loc)
 528 {
 529     return flightmode->get_wp(target_loc);
 530 }
 531
 532 /*
 533   update_target_location() acts as a wrapper for set_target_location
 534  */
 535 bool Copter::update_target_location(const Location &old_loc, const Location &new_loc)
 536 {
 537     /*
 538       by checking the caller has provided the correct old target
 539       location we prevent a race condition where the user changes mode
 540       or commands a different target in the controlling lua script
 541     */
 542     Location next_WP_loc;
 543     flightmode->get_wp(next_WP_loc);
 544     if (!old_loc.same_loc_as(next_WP_loc) ||
 545          old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
 546         return false;
 547     }
 548
 549     return set_target_location(new_loc);
 550 }
 551
 552 #endif // AP_SCRIPTING_ENABLED
 553
 554 // returns true if vehicle is landing.
 555 bool Copter::is_landing() const
 556 {
 557     return flightmode->is_landing();
 558 }
 559
 560 // returns true if vehicle is taking off.
 561 bool Copter::is_taking_off() const
 562 {
 563     return flightmode->is_taking_off();
 564 }
 565
 566 bool Copter::current_mode_requires_mission() const
 567 {
 568 #if MODE_AUTO_ENABLED
 569         return flightmode == &mode_auto;
 570 #else
 571         return false;
 572 #endif
 573 }
 574
 575 // rc_loops - reads user input from transmitter/receiver
 576 // called at 100hz
 577 void Copter::rc_loop()
 578 {
 579     // Read radio and 3-position switch on radio
 580     // -----------------------------------------
 581     read_radio();
 582     rc().read_mode_switch();
 583 }
 584
 585 // throttle_loop - should be run at 50 hz
 586 // ---------------------------
 587 void Copter::throttle_loop()
 588 {
 589     // update throttle_low_comp value (controls priority of throttle vs attitude control)
 590     update_throttle_mix();
 591
 592     // check auto_armed status
 593     update_auto_armed();
 594
 595 #if FRAME_CONFIG == HELI_FRAME
 596     // update rotor speed
 597     heli_update_rotor_speed_targets();
 598
 599     // update trad heli swash plate movement
 600     heli_update_landing_swash();
 601 #endif
 602
 603     // compensate for ground effect (if enabled)
 604     update_ground_effect_detector();
 605     update_ekf_terrain_height_stable();
 606 }
 607
 608 // update_batt_compass - read battery and compass
 609 // should be called at 10hz
 610 void Copter::update_batt_compass(void)
 611 {
 612     // read battery before compass because it may be used for motor interference compensation
 613     battery.read();
 614
 615     if(AP::compass().available()) {
 616         // update compass with throttle value - used for compassmot
 617         compass.set_throttle(motors->get_throttle());
 618         compass.set_voltage(battery.voltage());
 619         compass.read();
 620     }
 621 }
 622
 623 #if HAL_LOGGING_ENABLED
 624 // Full rate logging of attitude, rate and pid loops
 625 // should be run at loop rate
 626 void Copter::loop_rate_logging()
 627 {
 628    if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
 629         Log_Write_Attitude();
 630         if (!using_rate_thread) {
 631             Log_Write_Rate();
 632             Log_Write_PIDS(); // only logs if PIDS bitmask is set
 633         }
 634     }
 635 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 636     if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
 637         AP::ins().write_notch_log_messages();
 638     }
 639 #endif
 640     if (should_log(MASK_LOG_IMU_FAST)) {
 641         AP::ins().Write_IMU();
 642     }
 643 }
 644
 645 // ten_hz_logging_loop
 646 // should be run at 10hz
 647 void Copter::ten_hz_logging_loop()
 648 {
 649     // always write AHRS attitude at 10Hz
 650     ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
 651     // log attitude controller data if we're not already logging at the higher rate
 652     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
 653         Log_Write_Attitude();
 654         if (!using_rate_thread) {
 655             Log_Write_Rate();
 656         }
 657     }
 658     if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
 659     // 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
 660         if (!using_rate_thread) {
 661             Log_Write_PIDS();
 662         }
 663     }
 664     // log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
 665     if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
 666         Log_Write_EKF_POS();
 667     }
 668     if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) {
 669         // always write motors log if we are a heli
 670         motors->Log_Write();
 671     }
 672     if (should_log(MASK_LOG_RCIN)) {
 673         logger.Write_RCIN();
 674 #if AP_RSSI_ENABLED
 675         if (rssi.enabled()) {
 676             logger.Write_RSSI();
 677         }
 678 #endif
 679     }
 680     if (should_log(MASK_LOG_RCOUT)) {
 681         logger.Write_RCOUT();
 682     }
 683     if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS() || !flightmode->has_manual_throttle())) {
 684         pos_control->write_log();
 685     }
 686     if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
 687         AP::ins().Write_Vibration();
 688     }
 689     if (should_log(MASK_LOG_CTUN)) {
 690         attitude_control->control_monitor_log();
 691 #if HAL_PROXIMITY_ENABLED
 692         g2.proximity.log();  // Write proximity sensor distances
 693 #endif
 694 #if AP_BEACON_ENABLED
 695         g2.beacon.log();
 696 #endif
 697     }
 698 #if AP_WINCH_ENABLED
 699     if (should_log(MASK_LOG_ANY)) {
 700         g2.winch.write_log();
 701     }
 702 #endif
 703 #if HAL_MOUNT_ENABLED
 704     if (should_log(MASK_LOG_CAMERA)) {
 705         camera_mount.write_log();
 706     }
 707 #endif
 708 }
 709
 710 // twentyfive_hz_logging - should be run at 25hz
 711 void Copter::twentyfive_hz_logging()
 712 {
 713     if (should_log(MASK_LOG_ATTITUDE_FAST)) {
 714         Log_Write_EKF_POS();
 715     }
 716
 717     if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST))) {
 718         AP::ins().Write_IMU();
 719     }
 720
 721 #if HAL_GYROFFT_ENABLED
 722     if (should_log(MASK_LOG_FTN_FAST)) {
 723         gyro_fft.write_log_messages();
 724     }
 725 #endif
 726 }
 727 #endif  // HAL_LOGGING_ENABLED
 728
 729 // three_hz_loop - 3hz loop
 730 void Copter::three_hz_loop()
 731 {
 732     // check if we've lost contact with the ground station
 733     failsafe_gcs_check();
 734
 735     // check if we've lost terrain data
 736     failsafe_terrain_check();
 737
 738     // check for deadreckoning failsafe
 739     failsafe_deadreckon_check();
 740
 741     //update transmitter based in flight tuning
 742     tuning();
 743
 744     // check if avoidance should be enabled based on alt
 745     low_alt_avoidance();
 746 }
 747
 748 // ap_value calculates a 32-bit bitmask representing various pieces of
 749 // state about the Copter.  It replaces a global variable which was
 750 // used to track this state.
 751 uint32_t Copter::ap_value() const
 752 {
 753     uint32_t ret = 0;
 754
 755     const bool *b = (const bool *)&ap;
 756     for (uint8_t i=0; i<sizeof(ap); i++) {
 757         if (b[i]) {
 758             ret |= 1U<<i;
 759         }
 760     }
 761
 762     return ret;
 763 }
 764
 765 // one_hz_loop - runs at 1Hz
 766 void Copter::one_hz_loop()
 767 {
 768 #if HAL_LOGGING_ENABLED
 769     if (should_log(MASK_LOG_ANY)) {
 770         Log_Write_Data(LogDataID::AP_STATE, ap_value());
 771     }
 772 #endif
 773
 774     if (!motors->armed()) {
 775         update_using_interlock();
 776
 777         // check the user hasn't updated the frame class or type
 778         motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
 779
 780 #if FRAME_CONFIG != HELI_FRAME
 781         // set all throttle channel settings
 782         motors->update_throttle_range();
 783 #endif
 784     }
 785
 786     // update assigned functions and enable auxiliary servos
 787     AP::srv().enable_aux_servos();
 788
 789 #if HAL_LOGGING_ENABLED
 790     // log terrain data
 791     terrain_logging();
 792 #endif
 793
 794 #if HAL_ADSB_ENABLED
 795     adsb.set_is_flying(!ap.land_complete);
 796 #endif
 797
 798     AP_Notify::flags.flying = !ap.land_complete;
 799
 800     // slowly update the PID notches with the average loop rate
 801     if (!using_rate_thread) {
 802         attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 803     }
 804     pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 805 #if AC_CUSTOMCONTROL_MULTI_ENABLED
 806     custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 807 #endif
 808
 809 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 810     // see if we should have a separate rate thread
 811     if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
 812         if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
 813                                          "rate",
 814                                          1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
 815             started_rate_thread = true;
 816         } else {
 817             AP_BoardConfig::allocation_error("rate thread");
 818         }
 819     }
 820 #endif
 821 }
 822
 823 void Copter::init_simple_bearing()
 824 {
 825     // capture current cos_yaw and sin_yaw values
 826     simple_cos_yaw = ahrs.cos_yaw();
 827     simple_sin_yaw = ahrs.sin_yaw();
 828
 829     // initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
 830     super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
 831     super_simple_cos_yaw = simple_cos_yaw;
 832     super_simple_sin_yaw = simple_sin_yaw;
 833
 834 #if HAL_LOGGING_ENABLED
 835     // log the simple bearing
 836     if (should_log(MASK_LOG_ANY)) {
 837         Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
 838     }
 839 #endif
 840 }
 841
 842 // update_simple_mode - rotates pilot input if we are in simple mode
 843 void Copter::update_simple_mode(void)
 844 {
 845     float rollx, pitchx;
 846
 847     // exit immediately if no new radio frame or not in simple mode
 848     if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
 849         return;
 850     }
 851
 852     // mark radio frame as consumed
 853     ap.new_radio_frame = false;
 854
 855     if (simple_mode == SimpleMode::SIMPLE) {
 856         // rotate roll, pitch input by -initial simple heading (i.e. north facing)
 857         rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
 858         pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
 859     }else{
 860         // rotate roll, pitch input by -super simple heading (reverse of heading to home)
 861         rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
 862         pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
 863     }
 864
 865     // rotate roll, pitch input from north facing to vehicle's perspective
 866     channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
 867     channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
 868 }
 869
 870 // update_super_simple_bearing - adjusts simple bearing based on location
 871 // should be called after home_bearing has been updated
 872 void Copter::update_super_simple_bearing(bool force_update)
 873 {
 874     if (!force_update) {
 875         if (simple_mode != SimpleMode::SUPERSIMPLE) {
 876             return;
 877         }
 878         if (home_distance() < SUPER_SIMPLE_RADIUS) {
 879             return;
 880         }
 881     }
 882
 883     const int32_t bearing = home_bearing();
 884
 885     // check the bearing to home has changed by at least 5 degrees
 886     if (labs(super_simple_last_bearing - bearing) < 500) {
 887         return;
 888     }
 889
 890     super_simple_last_bearing = bearing;
 891     const float angle_rad = radians((super_simple_last_bearing+18000)/100);
 892     super_simple_cos_yaw = cosf(angle_rad);
 893     super_simple_sin_yaw = sinf(angle_rad);
 894 }
 895
 896 void Copter::read_AHRS(void)
 897 {
 898     // we tell AHRS to skip INS update as we have already done it in FAST_TASK.
 899     ahrs.update(true);
 900 }
 901
 902 // read baro and log control tuning
 903 void Copter::update_altitude()
 904 {
 905     // read in baro altitude
 906     read_barometer();
 907
 908 #if HAL_LOGGING_ENABLED
 909     if (should_log(MASK_LOG_CTUN)) {
 910         Log_Write_Control_Tuning();
 911         if (!should_log(MASK_LOG_FTN_FAST)) {
 912 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 913             AP::ins().write_notch_log_messages();
 914 #endif
 915 #if HAL_GYROFFT_ENABLED
 916             gyro_fft.write_log_messages();
 917 #endif
 918         }
 919     }
 920 #endif
 921 }
 922
 923 // vehicle specific waypoint info helpers
 924 bool Copter::get_wp_distance_m(float &distance) const
 925 {
 926     // see GCS_MAVLINK_Copter::send_nav_controller_output()
 927     distance = flightmode->wp_distance() * 0.01;
 928     return true;
 929 }
 930
 931 // vehicle specific waypoint info helpers
 932 bool Copter::get_wp_bearing_deg(float &bearing) const
 933 {
 934     // see GCS_MAVLINK_Copter::send_nav_controller_output()
 935     bearing = flightmode->wp_bearing() * 0.01;
 936     return true;
 937 }
 938
 939 // vehicle specific waypoint info helpers
 940 bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
 941 {
 942     // see GCS_MAVLINK_Copter::send_nav_controller_output()
 943     xtrack_error = flightmode->crosstrack_error() * 0.01;
 944     return true;
 945 }
 946
 947 // get the target earth-frame angular velocities in rad/s (Z-axis component used by some gimbals)
 948 bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const
 949 {
 950     // always returns zero vector if landed or disarmed
 951     if (copter.ap.land_complete) {
 952         rate_ef_targets.zero();
 953     } else {
 954         rate_ef_targets = attitude_control->get_rate_ef_targets();
 955     }
 956     return true;
 957 }
 958
 959 /*
 960   constructor for main Copter class
 961  */
 962 Copter::Copter(void)
 963     :
 964     flight_modes(&g.flight_mode1),
 965     pos_variance_filt(FS_EKF_FILT_DEFAULT),
 966     vel_variance_filt(FS_EKF_FILT_DEFAULT),
 967     flightmode(&mode_stabilize),
 968     simple_cos_yaw(1.0f),
 969     super_simple_cos_yaw(1.0),
 970     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
 971     rc_throttle_control_in_filter(1.0f),
 972     inertial_nav(ahrs),
 973     param_loader(var_info)
 974 {
 975 }
 976
 977 Copter copter;
 978 AP_Vehicle& vehicle = copter;
 979
 980 AP_HAL_MAIN_CALLBACKS(&copter);