2013-10-13 04:14:13 -03:00
|
|
|
/*
|
|
|
|
Lead developers: Matthew Ridley and Andrew Tridgell
|
2021-05-26 23:33:17 -03:00
|
|
|
|
|
|
|
Please contribute your ideas! See https://ardupilot.org/dev for details
|
2013-10-13 04:14:13 -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/>.
|
|
|
|
*/
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
#include "Tracker.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
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2021-11-13 23:18:10 -04:00
|
|
|
#define SCHED_TASK(func, _interval_ticks, _max_time_micros, _prio) SCHED_TASK_CLASS(Tracker, &tracker, func, _interval_ticks, _max_time_micros, _prio)
|
2014-10-06 02:55:45 -03:00
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
/*
|
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)
|
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
*/
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(update_ahrs, 50, 1000, 5),
|
|
|
|
SCHED_TASK(read_radio, 50, 200, 10),
|
|
|
|
SCHED_TASK(update_tracking, 50, 1000, 15),
|
|
|
|
SCHED_TASK(update_GPS, 10, 4000, 20),
|
|
|
|
SCHED_TASK(update_compass, 10, 1500, 25),
|
|
|
|
SCHED_TASK(compass_save, 0.02, 200, 30),
|
|
|
|
SCHED_TASK_CLASS(AP_BattMonitor, &tracker.battery, read, 10, 1500, 35),
|
|
|
|
SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, update, 10, 1500, 40),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_receive, 50, 1700, 45),
|
|
|
|
SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_send, 50, 3000, 50),
|
|
|
|
SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, accumulate, 50, 900, 55),
|
|
|
|
SCHED_TASK(ten_hz_logging_loop, 10, 300, 60),
|
2018-04-20 20:18:04 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300, 65),
|
2018-04-20 20:18:04 -03:00
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50, 70),
|
|
|
|
SCHED_TASK_CLASS(AP_Notify, &tracker.notify, update, 50, 100, 75),
|
|
|
|
SCHED_TASK(one_second_loop, 1, 3900, 80),
|
|
|
|
SCHED_TASK_CLASS(Compass, &tracker.compass, cal_update, 50, 100, 85),
|
|
|
|
SCHED_TASK(stats_update, 1, 200, 90),
|
2013-10-13 04:14:13 -03:00
|
|
|
};
|
|
|
|
|
2020-01-16 06:25:40 -04:00
|
|
|
void Tracker::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit)
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
2020-01-16 06:25:40 -04:00
|
|
|
tasks = &scheduler_tasks[0];
|
|
|
|
task_count = ARRAY_SIZE(scheduler_tasks);
|
|
|
|
log_bit = (uint32_t)-1;
|
2013-10-13 04:14:13 -03:00
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::one_second_loop()
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
|
|
|
// make it possible to change orientation at runtime
|
2019-02-17 00:53:49 -04:00
|
|
|
ahrs.update_orientation();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
|
|
|
// sync MAVLink system ID
|
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
2018-09-05 05:47:35 -03:00
|
|
|
// update assigned functions and enable auxiliary servos
|
|
|
|
SRV_Channels::enable_aux_servos();
|
|
|
|
|
2014-09-29 07:40:25 -03:00
|
|
|
// updated armed/disarmed status LEDs
|
|
|
|
update_armed_disarmed();
|
|
|
|
|
2018-05-16 07:23:29 -03:00
|
|
|
if (!ahrs.home_is_set()) {
|
|
|
|
// set home to current location
|
|
|
|
Location temp_loc;
|
2021-08-27 00:04:59 -03:00
|
|
|
if (ahrs.get_position(temp_loc)) {
|
2019-02-20 21:43:55 -04:00
|
|
|
if (!set_home(temp_loc)){
|
|
|
|
// fail silently
|
|
|
|
}
|
2018-05-16 07:23:29 -03:00
|
|
|
}
|
|
|
|
}
|
2018-10-19 04:31:34 -03:00
|
|
|
|
|
|
|
// need to set "likely flying" when armed to allow for compass
|
|
|
|
// learning to run
|
2020-01-06 19:43:22 -04:00
|
|
|
set_likely_flying(hal.util->get_soft_armed());
|
2019-03-01 02:34:33 -04:00
|
|
|
|
|
|
|
AP_Notify::flags.flying = hal.util->get_soft_armed();
|
2013-10-13 04:14:13 -03:00
|
|
|
}
|
|
|
|
|
2015-12-27 03:05:14 -04:00
|
|
|
void Tracker::ten_hz_logging_loop()
|
|
|
|
{
|
|
|
|
if (should_log(MASK_LOG_IMU)) {
|
2021-01-22 01:23:36 -04:00
|
|
|
AP::ins().Write_IMU();
|
2015-12-27 03:05:14 -04:00
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_ATTITUDE)) {
|
|
|
|
Log_Write_Attitude();
|
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_RCIN)) {
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_RCIN();
|
2015-12-27 03:05:14 -04:00
|
|
|
}
|
|
|
|
if (should_log(MASK_LOG_RCOUT)) {
|
2019-01-18 00:24:08 -04:00
|
|
|
logger.Write_RCOUT();
|
2015-12-27 03:05:14 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-09-13 12:41:55 -03:00
|
|
|
Mode *Tracker::mode_from_mode_num(const Mode::Number num)
|
|
|
|
{
|
|
|
|
Mode *ret = nullptr;
|
|
|
|
switch (num) {
|
|
|
|
case Mode::Number::MANUAL:
|
|
|
|
ret = &mode_manual;
|
|
|
|
break;
|
|
|
|
case Mode::Number::STOP:
|
|
|
|
ret = &mode_stop;
|
|
|
|
break;
|
|
|
|
case Mode::Number::SCAN:
|
|
|
|
ret = &mode_scan;
|
|
|
|
break;
|
2019-09-25 06:55:59 -03:00
|
|
|
case Mode::Number::GUIDED:
|
|
|
|
ret = &mode_guided;
|
|
|
|
break;
|
2019-09-13 12:41:55 -03:00
|
|
|
case Mode::Number::SERVOTEST:
|
|
|
|
ret = &mode_servotest;
|
|
|
|
break;
|
|
|
|
case Mode::Number::AUTO:
|
|
|
|
ret = &mode_auto;
|
|
|
|
break;
|
|
|
|
case Mode::Number::INITIALISING:
|
|
|
|
ret = &mode_initialising;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2019-08-06 04:09:08 -03:00
|
|
|
/*
|
|
|
|
update AP_Stats
|
|
|
|
*/
|
|
|
|
void Tracker::stats_update(void)
|
|
|
|
{
|
|
|
|
stats.set_flying(hal.util->get_soft_armed());
|
|
|
|
stats.update();
|
|
|
|
}
|
|
|
|
|
2015-10-16 17:22:11 -03:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2015-06-01 02:04:25 -03:00
|
|
|
|
|
|
|
Tracker::Tracker(void)
|
2019-01-18 00:23:42 -04:00
|
|
|
: logger(g.log_bitmask)
|
2015-06-08 01:14:10 -03:00
|
|
|
{
|
|
|
|
}
|
2015-06-01 02:04:25 -03:00
|
|
|
|
|
|
|
Tracker tracker;
|
2019-12-19 18:59:06 -04:00
|
|
|
AP_Vehicle& vehicle = tracker;
|
2015-06-01 02:04:25 -03:00
|
|
|
|
2015-10-19 15:32:37 -03:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&tracker);
|