zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: use new AP_Scheduler library

mission-4.1.18
Andrew Tridgell 12 years ago
parent
7ddadcf34e
commit
dcb181d2d8
4 changed files with 44 additions and 119 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 157
      ArduCopter/ArduCopter.pde
  2. 2
      ArduCopter/GCS_Mavlink.pde
  3. 3
      ArduCopter/Parameters.h
  4. 1
      ArduCopter/Parameters.pde

157
ArduCopter/ArduCopter.pde
Unescape View File

@ -99,8 +99,9 @@ @@ -99,8 +99,9 @@
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Limits.h>
#include <memcheck.h>
#include <SITL.h>
#include <memcheck.h> // memory limit checker
#include <SITL.h> // software in the loop support
#include <AP_Scheduler.h> // main loop scheduler
// AP_HAL to Arduino compatibility layer
#include "compat.h"
@ -138,6 +139,8 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; @@ -138,6 +139,8 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
//
static Parameters g;
// main loop scheduler
AP_Scheduler scheduler;
////////////////////////////////////////////////////////////////////////////////
// prototypes
@ -817,8 +820,6 @@ AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps); @@ -817,8 +820,6 @@ AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps);
////////////////////////////////////////////////////////////////////////////////
// Performance monitoring
////////////////////////////////////////////////////////////////////////////////
// Used to manage the rate of performance logging messages
static int16_t perf_mon_counter;
// The number of GPS fixes we have had
static int16_t gps_fix_count;
@ -906,6 +907,31 @@ void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t ma @@ -906,6 +907,31 @@ void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t ma
// setup the var_info table
AP_Param param_loader(var_info, WP_START_BYTE);
/*
scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
(in 10ms units) and the maximum time they are expected to take (in
microseconds)
*/
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ update_GPS, 2, 900 },
{ update_navigation, 2, 500 },
{ medium_loop, 2, 700 },
{ update_altitude_est, 2, 1000 },
{ fifty_hz_loop, 2, 950 },
{ run_nav_updates, 2, 500 },
{ slow_loop, 10, 500 },
{ gcs_check_input, 2, 700 },
{ gcs_send_heartbeat, 100, 700 },
{ gcs_data_stream_send, 2, 1500 },
{ gcs_send_deferred, 2, 1200 },
{ compass_accumulate, 2, 700 },
{ barometer_accumulate, 2, 900 },
{ super_slow_loop, 100, 1100 },
{ perf_update, 1000, 500 }
};
void setup() {
// this needs to be the first call, as it fills memory with
// sentinel values
@ -935,6 +961,9 @@ void setup() { @@ -935,6 +961,9 @@ void setup() {
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
}
/*
@ -962,7 +991,7 @@ static void perf_update(void) @@ -962,7 +991,7 @@ static void perf_update(void)
{
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
#if SCHEDULER_DEBUG
#if 1
cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"),
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
@ -972,112 +1001,6 @@ static void perf_update(void) @@ -972,112 +1001,6 @@ static void perf_update(void)
gps_fix_count = 0;
}
/*
return number of micros until the main loop will want to run again
*/
static int16_t main_loop_time_available(void)
{
uint32_t dt = (micros() - fast_loopTimer);
if (dt > 10000) {
return 0;
}
return 10000 - dt;
}
typedef void (*event_fn_t)(void);
struct timer_event_table {
event_fn_t func;
uint16_t time_interval_10ms;
uint16_t min_time_usec;
};
#define NUM_TIMER_EVENTS 15
/*
scheduler table - all regular events apart from the fast_loop()
should be listed here, along with how often they should be called
(in 10ms units) and the maximum time they are expected to take
*/
static const struct timer_event_table PROGMEM timer_events[NUM_TIMER_EVENTS] = {
{ update_GPS, 2, 900 },
{ update_navigation, 2, 500 },
{ medium_loop, 2, 700 },
{ update_altitude_est, 2, 900 },
{ fifty_hz_loop, 2, 950 },
{ run_nav_updates, 2, 500 },
{ slow_loop, 10, 500 },
{ gcs_check_input, 2, 700 },
{ gcs_send_heartbeat, 100, 700 },
{ gcs_data_stream_send, 2, 1100 },
{ gcs_send_deferred, 2, 1000 },
{ compass_accumulate, 2, 700 },
{ barometer_accumulate, 2, 900 },
{ super_slow_loop, 100, 1100 },
{ perf_update, 1000, 500 }
};
static uint16_t timer_counters[NUM_TIMER_EVENTS];
static uint16_t tick_counter;
static uint32_t event_time_started;
static uint16_t event_time_allowed;
/*
return number of micros until the current event reaches its deadline
*/
static uint16_t event_time_available(void)
{
uint32_t dt = micros() - event_time_started;
if (dt > event_time_allowed) {
return 0;
}
return event_time_allowed - dt;
}
/*
run as many scheduler events as we can
*/
static void run_events(uint16_t time_available_usec)
{
for (uint8_t i=0; i<NUM_TIMER_EVENTS; i++) {
// Make sure to use 16-bit arithmetic here for this comparison
// to handle the rollover case. Due to C integer promotion
// rules, we have to force the type of the difference here to
// "uint16_t" or the compiler will do a 32-bit comparison on a
// 32-bit platform.
uint16_t dt = tick_counter - timer_counters[i];
if (dt >= pgm_read_word(&timer_events[i].time_interval_10ms)) {
// this event is due to run. Do we have enough time to run it?
event_time_allowed = pgm_read_word(&timer_events[i].min_time_usec);
if (event_time_allowed <= time_available_usec) {
// run it
event_time_started = micros();
event_fn_t func = (event_fn_t)pgm_read_pointer(&timer_events[i].func);
func();
// record the tick counter when we ran. This drives
// when we next run the event
timer_counters[i] = tick_counter;
// work out how long the event actually took
uint32_t time_taken = micros() - event_time_started;
if (time_taken > event_time_allowed+500) {
// the event overran!
#if SCHEDULER_DEBUG
cliSerial->printf_P(PSTR("overrun in event %u (%u/%u)\n"),
(unsigned)i,
(unsigned)time_taken,
(unsigned)event_time_allowed);
#endif
return;
}
time_available_usec -= time_taken;
}
}
}
}
void loop()
{
uint32_t timer = micros();
@ -1099,16 +1022,14 @@ void loop() @@ -1099,16 +1022,14 @@ void loop()
// ---------------------
fast_loop();
tick_counter++;
// tell the scheduler one tick has passed
scheduler.tick();
} else {
uint16_t time_to_next_loop;
uint16_t dt = timer - fast_loopTimer;
if (dt > 10000) {
time_to_next_loop = 0;
} else {
time_to_next_loop = 10000 - dt;
if (dt < 10000) {
uint16_t time_to_next_loop = 10000 - dt;
scheduler.run(time_to_next_loop);
}
run_events(time_to_next_loop);
}
}

2
ArduCopter/GCS_Mavlink.pde
Unescape View File

@ -544,7 +544,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, @@ -544,7 +544,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (event_time_available() < 500) {
if (scheduler.time_available_usec() < 800) {
gcs_out_of_time = true;
return false;
}

3
ArduCopter/Parameters.h
Unescape View File

@ -61,6 +61,9 @@ public: @@ -61,6 +61,9 @@ public:
// barometer object (needed for SITL)
k_param_barometer,
// scheduler object (for debugging)
k_param_scheduler,
// Misc
//
k_param_log_bitmask = 20,

1
ArduCopter/Parameters.pde
Unescape View File

@ -1003,6 +1003,7 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -1003,6 +1003,7 @@ const AP_Param::Info var_info[] PROGMEM = {
#endif
GOBJECT(barometer, "GND_", AP_Baro),
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
#if AP_LIMITS == ENABLED
//@Group: LIM_

Write Preview
Loading…
Cancel
Save