AntennaTracker/Tracker.cpp

   1 /*
   2    Lead developers: Matthew Ridley and Andrew Tridgell
   3
   4    Please contribute your ideas! See https://ardupilot.org/dev for details
   5
   6    This program is free software: you can redistribute it and/or modify
   7    it under the terms of the GNU General Public License as published by
   8    the Free Software Foundation, either version 3 of the License, or
   9    (at your option) any later version.
  10
  11    This program is distributed in the hope that it will be useful,
  12    but WITHOUT ANY WARRANTY; without even the implied warranty of
  13    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14    GNU General Public License for more details.
  15
  16    You should have received a copy of the GNU General Public License
  17    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include "Tracker.h"
  21
  22 #define FORCE_VERSION_H_INCLUDE
  23 #include "version.h"
  24 #undef FORCE_VERSION_H_INCLUDE
  25
  26 #define SCHED_TASK(func, _interval_ticks, _max_time_micros, _prio) SCHED_TASK_CLASS(Tracker, &tracker, func, _interval_ticks, _max_time_micros, _prio)
  27
  28 /*
  29   All entries in this table must be ordered by priority.
  30
  31   This table is interleaved with the table in AP_Vehicle to determine
  32   the order in which tasks are run.  Convenience methods SCHED_TASK
  33   and SCHED_TASK_CLASS are provided to build entries in this structure:
  34
  35 SCHED_TASK arguments:
  36  - name of static function to call
  37  - rate (in Hertz) at which the function should be called
  38  - expected time (in MicroSeconds) that the function should take to run
  39  - priority (0 through 255, lower number meaning higher priority)
  40
  41 SCHED_TASK_CLASS arguments:
  42  - class name of method to be called
  43  - instance on which to call the method
  44  - method to call on that instance
  45  - rate (in Hertz) at which the method should be called
  46  - expected time (in MicroSeconds) that the method should take to run
  47  - priority (0 through 255, lower number meaning higher priority)
  48
  49  */
  50 const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
  51     SCHED_TASK(update_ahrs,            50,    1000,  5),
  52     SCHED_TASK(read_radio,             50,     200, 10),
  53     SCHED_TASK(update_tracking,        50,    1000, 15),
  54     SCHED_TASK(update_GPS,             10,    4000, 20),
  55     SCHED_TASK(update_compass,         10,    1500, 25),
  56     SCHED_TASK(compass_save,            0.02,  200, 30),
  57     SCHED_TASK_CLASS(AP_BattMonitor,    &tracker.battery,   read,           10, 1500, 35),
  58     SCHED_TASK_CLASS(AP_Baro,          &tracker.barometer,  update,         10, 1500, 40),
  59     SCHED_TASK_CLASS(GCS,              (GCS*)&tracker._gcs, update_receive, 50, 1700, 45),
  60     SCHED_TASK_CLASS(GCS,              (GCS*)&tracker._gcs, update_send,    50, 3000, 50),
  61 #if HAL_LOGGING_ENABLED
  62     SCHED_TASK(ten_hz_logging_loop,    10,    300, 60),
  63     SCHED_TASK_CLASS(AP_Logger,   &tracker.logger, periodic_tasks, 50,  300, 65),
  64 #endif
  65     SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins,       periodic,       50,   50, 70),
  66     SCHED_TASK(one_second_loop,         1,   3900, 80),
  67     SCHED_TASK(stats_update,            1,    200, 90),
  68 };
  69
  70 void Tracker::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
  71                                 uint8_t &task_count,
  72                                 uint32_t &log_bit)
  73 {
  74     tasks = &scheduler_tasks[0];
  75     task_count = ARRAY_SIZE(scheduler_tasks);
  76     log_bit = (uint32_t)-1;
  77 }
  78
  79 void Tracker::one_second_loop()
  80 {
  81     // sync MAVLink system ID
  82     mavlink_system.sysid = g.sysid_this_mav;
  83
  84     // update assigned functions and enable auxiliary servos
  85     SRV_Channels::enable_aux_servos();
  86
  87     // updated armed/disarmed status LEDs
  88     update_armed_disarmed();
  89
  90     if (!ahrs.home_is_set()) {
  91         // set home to current location
  92         Location temp_loc;
  93         if (ahrs.get_location(temp_loc)) {
  94             if (!set_home(temp_loc, false)) {
  95                 // fail silently
  96             }
  97         }
  98     }
  99
 100     // need to set "likely flying" when armed to allow for compass
 101     // learning to run
 102     set_likely_flying(hal.util->get_soft_armed());
 103
 104     AP_Notify::flags.flying = hal.util->get_soft_armed();
 105
 106     g.pidYaw2Srv.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 107 }
 108
 109 #if HAL_LOGGING_ENABLED
 110 void Tracker::ten_hz_logging_loop()
 111 {
 112     if (should_log(MASK_LOG_IMU)) {
 113         AP::ins().Write_IMU();
 114     }
 115     if (should_log(MASK_LOG_ATTITUDE)) {
 116         Log_Write_Attitude();
 117     }
 118     if (should_log(MASK_LOG_RCIN)) {
 119         logger.Write_RCIN();
 120     }
 121     if (should_log(MASK_LOG_RCOUT)) {
 122         logger.Write_RCOUT();
 123     }
 124 }
 125 #endif
 126
 127 Mode *Tracker::mode_from_mode_num(const Mode::Number num)
 128 {
 129     Mode *ret = nullptr;
 130     switch (num) {
 131     case Mode::Number::MANUAL:
 132         ret = &mode_manual;
 133         break;
 134     case Mode::Number::STOP:
 135         ret = &mode_stop;
 136         break;
 137     case Mode::Number::SCAN:
 138         ret = &mode_scan;
 139         break;
 140     case Mode::Number::GUIDED:
 141         ret = &mode_guided;
 142         break;
 143     case Mode::Number::SERVOTEST:
 144         ret = &mode_servotest;
 145         break;
 146     case Mode::Number::AUTO:
 147         ret = &mode_auto;
 148         break;
 149     case Mode::Number::INITIALISING:
 150         ret = &mode_initialising;
 151         break;
 152     }
 153     return ret;
 154 }
 155
 156 /*
 157   update AP_Stats
 158 */
 159 void Tracker::stats_update(void)
 160 {
 161     AP::stats()->set_flying(hal.util->get_soft_armed());
 162 }
 163
 164 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 165
 166 Tracker tracker;
 167 AP_Vehicle& vehicle = tracker;
 168
 169 AP_HAL_MAIN_CALLBACKS(&tracker);