mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
ecd204c45c
We should never include version.h or ap_version.h headers directly on a header since this will trigger a complete rebuild of the codebase when we commit to the repository. The ap_version.h header is auto-generated containing information from the current commit. If we include it in a header, every other file that ends up including that header (directly or indirectly) will need to be rebuilt. No ccache's cache beats having to do nothing when the header is just not included. version.h contains information that is kept on a struct inside each vehicle. Rather than using the macros from each vehicle, the getter should be preferred, which returns an AP_FWVersion referente.
140 lines
3.9 KiB
C++
140 lines
3.9 KiB
C++
/*
|
|
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"
|
|
|
|
#define FORCE_VERSION_H_INCLUDE
|
|
#include "version.h"
|
|
#undef FORCE_VERSION_H_INCLUDE
|
|
|
|
#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),
|
|
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),
|
|
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
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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{fwver.fw_string, g.log_bitmask}
|
|
{
|
|
memset(¤t_loc, 0, sizeof(current_loc));
|
|
memset(&vehicle, 0, sizeof(vehicle));
|
|
}
|
|
|
|
Tracker tracker;
|
|
|
|
AP_HAL_MAIN_CALLBACKS(&tracker);
|