mirror of https://github.com/ArduPilot/ardupilot
Rover: convert to use AP_Scheduler
this gives us better timing information and scheduling, while also making the code easier to read
This commit is contained in:
Andrew Tridgell
parent
ba959fd594
commit
2d795ac85a
|
|
@ -75,6 +75,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||
#include <DataFlash.h>
|
||||
#include <AP_RCMapper.h> // RC input mapping library
|
||||
#include <SITL.h>
|
||||
#include <AP_Scheduler.h> // main loop scheduler
|
||||
#include <stdarg.h>
|
||||
|
||||
#include <AP_HAL_AVR.h>
|
||||
|
|
@ -116,6 +117,9 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
|
|||
//
|
||||
static Parameters g;
|
||||
|
||||
// main loop scheduler
|
||||
static AP_Scheduler scheduler;
|
||||
|
||||
// mapping between input channels
|
||||
static RCMapper rcmap;
|
||||
|
||||
|
|
@ -532,20 +536,6 @@ static uint8_t delta_ms_fast_loop;
|
|||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
static uint16_t mainLoop_count;
|
||||
|
||||
// Time in miliseconds of start of medium control loop. Milliseconds
|
||||
static uint32_t medium_loopTimer;
|
||||
// Counters for branching from main control loop to slower loops
|
||||
static uint8_t medium_loopCounter;
|
||||
// Number of milliseconds used in last medium loop cycle
|
||||
static uint8_t delta_ms_medium_loop;
|
||||
|
||||
// Counters for branching from medium control loop to slower loops
|
||||
static uint8_t slow_loopCounter;
|
||||
// Counter to trigger execution of very low rate processes
|
||||
static uint8_t superslow_loopCounter;
|
||||
// Counter to trigger execution of 1 Hz processes
|
||||
static uint8_t counter_one_herz;
|
||||
|
||||
// % MCU cycles used
|
||||
static float load;
|
||||
|
||||
|
|
@ -553,6 +543,33 @@ static float load;
|
|||
// Top-level logic
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*
|
||||
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)
|
||||
*/
|
||||
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||
{ update_GPS, 5, 900 },
|
||||
{ navigate, 5, 1000 },
|
||||
{ update_compass, 5, 1000 },
|
||||
{ update_commands, 5, 1000 },
|
||||
{ update_logging, 5, 1000 },
|
||||
{ read_battery, 5, 1000 },
|
||||
{ read_receiver_rssi, 5, 1000 },
|
||||
{ read_trim_switch, 5, 1000 },
|
||||
{ read_control_switch, 15, 1000 },
|
||||
{ update_events, 15, 1000 },
|
||||
{ check_usb_mux, 15, 1000 },
|
||||
{ mount_update, 1, 500 },
|
||||
{ failsafe_check, 5, 500 },
|
||||
{ one_second_loop, 50, 1000 }
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
setup is called when the sketch starts
|
||||
*/
|
||||
void setup() {
|
||||
memcheck_init();
|
||||
cliSerial = hal.console;
|
||||
|
|
@ -566,10 +583,18 @@ void setup() {
|
|||
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
|
||||
|
||||
init_ardupilot();
|
||||
|
||||
// initialise the main loop scheduler
|
||||
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
|
||||
}
|
||||
|
||||
/*
|
||||
loop() is called rapidly while the sketch is running
|
||||
*/
|
||||
void loop()
|
||||
{
|
||||
uint32_t timer = millis();
|
||||
|
||||
// We want this to execute at 50Hz, but synchronised with the gyro/accel
|
||||
uint16_t num_samples = ins.num_samples_available();
|
||||
if (num_samples >= 1) {
|
||||
|
|
@ -584,35 +609,14 @@ void loop()
|
|||
// ---------------------
|
||||
fast_loop();
|
||||
|
||||
// Execute the medium loop
|
||||
// -----------------------
|
||||
medium_loop();
|
||||
|
||||
counter_one_herz++;
|
||||
if(counter_one_herz == 50){
|
||||
one_second_loop();
|
||||
counter_one_herz = 0;
|
||||
}
|
||||
|
||||
if (millis() - perf_mon_timer > 20000) {
|
||||
if (mainLoop_count != 0) {
|
||||
if (g.log_bitmask & MASK_LOG_PM)
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
Log_Write_Performance();
|
||||
#endif
|
||||
resetPerfData();
|
||||
}
|
||||
}
|
||||
|
||||
// tell the scheduler one tick has passed
|
||||
scheduler.tick();
|
||||
fast_loopTimeStamp = millis();
|
||||
} else if (millis() - fast_loopTimeStamp < 19) {
|
||||
// less than 19ms has passed. We have at least one millisecond
|
||||
// of free time. The most useful thing to do with that time is
|
||||
// to accumulate some sensor readings, specifically the
|
||||
// compass, which is often very noisy but is not interrupt
|
||||
// driven, so it can't accumulate readings by itself
|
||||
if (g.compass_enabled) {
|
||||
compass.accumulate();
|
||||
} else {
|
||||
uint16_t dt = timer - fast_loopTimer;
|
||||
if (dt < 20) {
|
||||
uint16_t time_to_next_loop = 20 - dt;
|
||||
scheduler.run(time_to_next_loop * 1000U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -665,135 +669,60 @@ static void fast_loop()
|
|||
gcs_data_stream_send();
|
||||
}
|
||||
|
||||
static void medium_loop()
|
||||
/*
|
||||
update camera mount - 50Hz
|
||||
*/
|
||||
static void mount_update(void)
|
||||
{
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_position();
|
||||
#endif
|
||||
|
||||
// This is the start of the medium (10 Hz) loop pieces
|
||||
// -----------------------------------------
|
||||
switch(medium_loopCounter) {
|
||||
|
||||
// This case deals with the GPS
|
||||
//-------------------------------
|
||||
case 0:
|
||||
failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
|
||||
medium_loopCounter++;
|
||||
update_GPS();
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
// Calculate heading
|
||||
compass.null_offsets();
|
||||
if (g.log_bitmask & MASK_LOG_COMPASS) {
|
||||
Log_Write_Compass();
|
||||
}
|
||||
} else {
|
||||
ahrs.set_compass(NULL);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
// This case performs some navigation computations
|
||||
//------------------------------------------------
|
||||
case 1:
|
||||
medium_loopCounter++;
|
||||
navigate();
|
||||
break;
|
||||
|
||||
// command processing
|
||||
//------------------------------
|
||||
case 2:
|
||||
medium_loopCounter++;
|
||||
|
||||
read_receiver_rssi();
|
||||
|
||||
// perform next command
|
||||
// --------------------
|
||||
update_commands();
|
||||
break;
|
||||
|
||||
// This case deals with sending high rate telemetry
|
||||
//-------------------------------------------------
|
||||
case 3:
|
||||
medium_loopCounter++;
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
|
||||
Log_Write_Attitude();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||
Log_Write_Control_Tuning();
|
||||
#endif
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_NTUN)
|
||||
Log_Write_Nav_Tuning();
|
||||
break;
|
||||
|
||||
// This case controls the slow loop
|
||||
//---------------------------------
|
||||
case 4:
|
||||
medium_loopCounter = 0;
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
|
||||
if (g.battery_monitoring != 0){
|
||||
read_battery();
|
||||
}
|
||||
|
||||
read_trim_switch();
|
||||
|
||||
slow_loop();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void slow_loop()
|
||||
/*
|
||||
check for GCS failsafe - 10Hz
|
||||
*/
|
||||
static void failsafe_check(void)
|
||||
{
|
||||
// This is the slow (3 1/3 Hz) loop pieces
|
||||
//----------------------------------------
|
||||
switch (slow_loopCounter){
|
||||
case 0:
|
||||
slow_loopCounter++;
|
||||
superslow_loopCounter++;
|
||||
if(superslow_loopCounter >=200) { // 200 = Execute every minute
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if(g.compass_enabled) {
|
||||
compass.save_offsets();
|
||||
}
|
||||
#endif
|
||||
superslow_loopCounter = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
slow_loopCounter++;
|
||||
|
||||
// Read 3-position switch on radio
|
||||
// -------------------------------
|
||||
read_control_switch();
|
||||
|
||||
update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_type();
|
||||
#endif
|
||||
break;
|
||||
|
||||
case 2:
|
||||
slow_loopCounter = 0;
|
||||
|
||||
update_events();
|
||||
|
||||
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
|
||||
|
||||
check_usb_mux();
|
||||
break;
|
||||
}
|
||||
failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
|
||||
}
|
||||
|
||||
static void one_second_loop()
|
||||
/*
|
||||
check for new compass data - 10Hz
|
||||
*/
|
||||
static void update_compass(void)
|
||||
{
|
||||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
// update offsets
|
||||
compass.null_offsets();
|
||||
if (g.log_bitmask & MASK_LOG_COMPASS) {
|
||||
Log_Write_Compass();
|
||||
}
|
||||
} else {
|
||||
ahrs.set_compass(NULL);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
log some key data - 10Hz
|
||||
*/
|
||||
static void update_logging(void)
|
||||
{
|
||||
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
|
||||
Log_Write_Attitude();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||
Log_Write_Control_Tuning();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_NTUN)
|
||||
Log_Write_Nav_Tuning();
|
||||
}
|
||||
|
||||
/*
|
||||
once a second events
|
||||
*/
|
||||
static void one_second_loop(void)
|
||||
{
|
||||
if (g.log_bitmask & MASK_LOG_CURRENT)
|
||||
Log_Write_Current();
|
||||
|
|
@ -804,6 +733,35 @@ static void one_second_loop()
|
|||
ahrs.set_orientation();
|
||||
|
||||
set_control_channels();
|
||||
|
||||
// cope with changes to aux functions
|
||||
update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_type();
|
||||
#endif
|
||||
|
||||
// cope with changes to mavlink system ID
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
static uint8_t counter;
|
||||
|
||||
counter++;
|
||||
|
||||
// write perf data every 20s
|
||||
if (counter == 20) {
|
||||
if (g.log_bitmask & MASK_LOG_PM)
|
||||
Log_Write_Performance();
|
||||
resetPerfData();
|
||||
}
|
||||
|
||||
// save compass offsets once a minute
|
||||
if (counter >= 60) {
|
||||
if (g.compass_enabled) {
|
||||
compass.save_offsets();
|
||||
}
|
||||
counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void update_GPS(void)
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ public:
|
|||
k_param_num_resets,
|
||||
k_param_reset_switch_chan,
|
||||
k_param_initial_mode,
|
||||
k_param_scheduler,
|
||||
|
||||
// IO pins
|
||||
k_param_rssi_pin = 20,
|
||||
|
|
|
|||
|
|
@ -416,6 +416,10 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @Path: ../libraries/AP_Compass/Compass.cpp
|
||||
GOBJECT(compass, "COMPASS_", Compass),
|
||||
|
||||
// @Group: SCHED_
|
||||
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
|
||||
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
|
||||
|
||||
// @Group: RCMAP_
|
||||
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
|
||||
GOBJECT(rcmap, "RCMAP_", RCMapper),
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@ void ReadSCP1000(void) {}
|
|||
|
||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
/*
|
||||
read and update the battery
|
||||
*/
|
||||
static void read_battery(void)
|
||||
{
|
||||
if(g.battery_monitoring == 0) {
|
||||
|
|
@ -30,11 +33,19 @@ static void read_battery(void)
|
|||
batt_volt_pin->set_pin(g.battery_volt_pin);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin);
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
// this copes with changing the pin at runtime
|
||||
batt_curr_pin->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_pin);
|
||||
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
|
||||
|
||||
if (g.battery_monitoring == 4) {
|
||||
static uint32_t last_time_ms;
|
||||
uint32_t tnow = hal.scheduler->millis();
|
||||
float dt = tnow - last_time_ms;
|
||||
if (last_time_ms != 0 && dt < 2000) {
|
||||
// this copes with changing the pin at runtime
|
||||
batt_curr_pin->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_pin);
|
||||
// .0002778 is 1/3600 (conversion to hours)
|
||||
current_total1 += current_amps1 * dt * 0.0002778;
|
||||
}
|
||||
last_time_ms = tnow;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -294,6 +294,10 @@ static void startup_ground(void)
|
|||
// -----------------------
|
||||
demo_servos(3);
|
||||
|
||||
hal.uartA->set_blocking_writes(false);
|
||||
hal.uartB->set_blocking_writes(false);
|
||||
hal.uartC->set_blocking_writes(false);
|
||||
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -233,7 +233,6 @@ test_battery(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
print_hit_enter();
|
||||
delta_ms_medium_loop = 100;
|
||||
|
||||
while(1){
|
||||
delay(100);
|
||||
|
|
@ -423,6 +422,8 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
uint8_t medium_loopCounter = 0;
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
|
|
@ -436,7 +437,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
if(g.compass_enabled) {
|
||||
medium_loopCounter++;
|
||||
if(medium_loopCounter == 5){
|
||||
if(medium_loopCounter >= 5){
|
||||
compass.read();
|
||||
medium_loopCounter = 0;
|
||||
}
|
||||
|
|
@ -489,6 +490,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
print_hit_enter();
|
||||
|
||||
uint8_t medium_loopCounter = 0;
|
||||
|
||||
while(1) {
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
|
|
@ -501,7 +504,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
ahrs.update();
|
||||
|
||||
medium_loopCounter++;
|
||||
if(medium_loopCounter == 5){
|
||||
if(medium_loopCounter >= 5){
|
||||
if (compass.read()) {
|
||||
// Calculate heading
|
||||
Matrix3f m = ahrs.get_dcm_matrix();
|
||||
|
|
|
|||
Loading…
Reference in New Issue