ardupilot/AntennaTracker/AntennaTracker.cpp

137 lines
3.8 KiB
C++
Raw Normal View History

/*
Lead developers: Matthew Ridley and Andrew Tridgell
Please contribute your ideas! See http://dev.ardupilot.org for details
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/>.
*/
#include "Tracker.h"
#include "version.h"
#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(Tracker, &tracker, func, _interval_ticks, _max_time_micros)
/*
scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
(in 20ms units) and the maximum time they are expected to take (in
microseconds)
*/
const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
SCHED_TASK(update_ahrs, 50, 1000),
SCHED_TASK(read_radio, 50, 200),
SCHED_TASK(update_tracking, 50, 1000),
SCHED_TASK(update_GPS, 10, 4000),
SCHED_TASK(update_compass, 10, 1500),
SCHED_TASK(update_barometer, 10, 1500),
SCHED_TASK(gcs_update, 50, 1700),
SCHED_TASK(gcs_data_stream_send, 50, 3000),
SCHED_TASK(compass_accumulate, 50, 1500),
SCHED_TASK(barometer_accumulate, 50, 900),
2015-12-27 03:05:14 -04:00
SCHED_TASK(ten_hz_logging_loop, 10, 300),
SCHED_TASK(dataflash_periodic, 50, 300),
SCHED_TASK(update_notify, 50, 100),
SCHED_TASK(check_usb_mux, 10, 300),
SCHED_TASK(gcs_retry_deferred, 50, 1000),
SCHED_TASK(one_second_loop, 1, 3900),
SCHED_TASK(compass_cal_update, 50, 100),
2015-10-21 18:27:35 -03:00
SCHED_TASK(accel_cal_update, 10, 100)
};
/**
setup the sketch - called once on startup
*/
void Tracker::setup()
{
// load the default values of variables listed in var_info[]
AP_Param::setup_sketch_defaults();
init_tracker();
// initialise the main loop scheduler
2015-07-06 12:30:40 -03:00
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
}
/**
loop() is called continuously
*/
void Tracker::loop()
{
// wait for an INS sample
ins.wait_for_sample();
// tell the scheduler one tick has passed
scheduler.tick();
scheduler.run(19900UL);
}
2015-12-27 03:05:14 -04:00
void Tracker::dataflash_periodic(void)
{
DataFlash.periodic_tasks();
}
void Tracker::one_second_loop()
{
// send a heartbeat
gcs().send_message(MSG_HEARTBEAT);
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
// updated armed/disarmed status LEDs
update_armed_disarmed();
one_second_counter++;
if (one_second_counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
one_second_counter = 0;
}
}
2015-12-27 03:05:14 -04:00
void Tracker::ten_hz_logging_loop()
{
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_IMU(ins);
}
if (should_log(MASK_LOG_ATTITUDE)) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
}
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
}
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Tracker::Tracker(void)
: DataFlash{FIRMWARE_STRING, g.log_bitmask}
{
memset(&current_loc, 0, sizeof(current_loc));
memset(&vehicle, 0, sizeof(vehicle));
}
Tracker tracker;
AP_HAL_MAIN_CALLBACKS(&tracker);