aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: switch to new AP_Scheduler

Browse Source 
this gives us more accurate task scheduling in ArduPlane, plus better
monitoring of task timing (via SCHED_DEBUG)
This commit is contained in:
Andrew Tridgell 2013-06-04 13:34:58 +10:00
parent 07587222a3
commit 93cd0f9a31
10 changed files with 153 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

295
ArduPlane/ArduPlane.pde
View File

@ -53,6 +53,7 @@
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <DataFlash.h>
#include <SITL.h>
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_Navigation.h>
#include <AP_L1_Control.h>
@ -99,6 +100,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;
@ -593,10 +597,6 @@ static int32_t perf_mon_timer;
static int16_t G_Dt_max = 0;
// The number of gps fixes recorded in the current performance monitoring interval
static uint8_t gps_fix_count = 0;
// A variable used by developers to track performanc metrics.
// Currently used to record the number of GCS heartbeat messages received
static int16_t pmTest1 = 0;
////////////////////////////////////////////////////////////////////////////////
// System Timers
@ -613,21 +613,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_ms;
// 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;
@ -654,6 +639,32 @@ AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
// 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, 2500 },
{ navigate, 5, 4800 },
{ update_compass, 5, 1500 },
{ read_airspeed, 5, 1500 },
{ read_control_switch, 15, 1000 },
{ update_alt, 5, 1000 },
{ calc_altitude_error, 5, 1000 },
{ update_commands, 5, 1000 },
{ update_mount, 2, 1500 },
{ obc_fs_check, 5, 1000 },
{ update_events, 15, 1500 },
{ check_usb_mux, 5, 1000 },
{ read_battery, 5, 1000 },
{ one_second_loop, 50, 3000 },
{ update_logging, 5, 1000 },
{ read_receiver_rssi, 5, 1000 },
{ check_long_failsafe, 15, 1000 },
};
// setup the var_info table
AP_Param param_loader(var_info, WP_START_BYTE);
@ -675,17 +686,21 @@ 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]));
}
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) {
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
delta_ms_fast_loop = timer - fast_loopTimer_ms;
load = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f;
fast_loopTimer_ms = millis();
G_Dt = delta_ms_fast_loop * 0.001f;
fast_loopTimer_ms = timer;
mainLoop_count++;
@ -693,33 +708,15 @@ 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)
Log_Write_Performance();
resetPerfData();
}
}
// tell the scheduler one tick has passed
scheduler.tick();
fast_loopTimeStamp_ms = millis();
} else if (millis() - fast_loopTimeStamp_ms < 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_loopTimeStamp_ms;
if (dt < 20) {
uint16_t time_to_next_loop = 20 - dt;
scheduler.run(time_to_next_loop * 1000U);
}
}
}
@ -782,7 +779,10 @@ static void fast_loop()
gcs_data_stream_send();
}
static void medium_loop()
/*
update camera mount
*/
static void update_mount(void)
{
#if MOUNT == ENABLED
camera_mount.update_mount_position();
@ -795,133 +795,58 @@ static void medium_loop()
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
#endif
}
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
// This case deals with the GPS
//-------------------------------
case 0:
medium_loopCounter++;
update_GPS();
calc_gndspeed_undershoot();
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
compass.null_offsets();
if (g.log_bitmask & MASK_LOG_COMPASS) {
Log_Write_Compass();
}
} else {
ahrs.set_compass(NULL);
/*
read and update compass
*/
static void update_compass(void)
{
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
compass.null_offsets();
if (g.log_bitmask & MASK_LOG_COMPASS) {
Log_Write_Compass();
}
break;
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
// Read 6-position switch on radio
// -------------------------------
read_control_switch();
// calculate the plane's desired bearing
// -------------------------------------
navigate();
break;
// command processing
//------------------------------
case 2:
medium_loopCounter++;
// Read Airspeed
// -------------
if (airspeed.enabled()) {
read_airspeed();
}
read_receiver_rssi();
// Read altitude from sensors
// ------------------
update_alt();
// altitude smoothing
// ------------------
if (control_mode != FLY_BY_WIRE_B)
calc_altitude_error();
// perform next command
// --------------------
update_commands();
break;
// This case deals with sending high rate telemetry
//-------------------------------------------------
case 3:
medium_loopCounter++;
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();
break;
// This case controls the slow loop
//---------------------------------
case 4:
medium_loopCounter = 0;
delta_ms_medium_loop = millis() - medium_loopTimer_ms;
medium_loopTimer_ms = millis();
if (g.battery_monitoring != 0) {
read_battery();
}
slow_loop();
#if OBC_FAILSAFE == ENABLED
// perform OBC failsafe checks
obc.check(OBC_MODE(control_mode),
last_heartbeat_ms,
g_gps ? g_gps->last_fix_time : 0);
#endif
break;
} else {
ahrs.set_compass(NULL);
}
}
static void slow_loop()
/*
do 10Hz logging
*/
static void update_logging(void)
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter) {
case 0:
slow_loopCounter++;
check_long_failsafe();
superslow_loopCounter++;
if(superslow_loopCounter >=200) {
// 200 = Execute every minute
if(g.compass_enabled) {
compass.save_offsets();
}
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();
}
superslow_loopCounter = 0;
}
break;
/*
check for OBC failsafe check
*/
static void obc_fs_check(void)
{
#if OBC_FAILSAFE == ENABLED
// perform OBC failsafe checks
obc.check(OBC_MODE(control_mode),
last_heartbeat_ms,
g_gps ? g_gps->last_fix_time : 0);
#endif
}
case 1:
slow_loopCounter++;
/*
update aux servo mappings
*/
static void update_aux(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
@ -937,18 +862,6 @@ static void slow_loop()
#if MOUNT2 == ENABLED
camera_mount2.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;
}
}
static void one_second_loop()
@ -964,8 +877,32 @@ static void one_second_loop()
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
static uint8_t counter;
counter++;
if (counter == 20) {
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
if (counter >= 60) {
if(g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
}
/*
read the GPS and update position
*/
static void update_GPS(void)
{
static uint32_t last_gps_reading;
@ -1023,6 +960,8 @@ static void update_GPS(void)
// see if we've breached the geo-fence
geofence_check(false);
}
calc_gndspeed_undershoot();
}
static void update_current_flight_mode(void)

1
ArduPlane/GCS_Mavlink.pde
View File

@ -1881,7 +1881,6 @@ mission_failed:
// our GCS for failsafe purposes
if (msg->sysid != g.sysid_my_gcs) break;
last_heartbeat_ms = millis();
pmTest1++;
break;
}

4
ArduPlane/Log.pde
View File

@ -186,7 +186,6 @@ struct PACKED log_Performance {
int16_t gyro_drift_x;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
int16_t pm_test;
uint8_t i2c_lockup_count;
};
@ -204,7 +203,6 @@ static void Log_Write_Performance()
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000),
gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000),
pm_test : pmTest1,
i2c_lockup_count: hal.i2c->lockup_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -432,7 +430,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),
"ATT", "ccC", "Roll,Pitch,Yaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
"PM", "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,I2CErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),

1
ArduPlane/Parameters.h
View File

@ -88,6 +88,7 @@ public:
k_param_elevon_output,
k_param_att_controller,
k_param_mixing_gain,
k_param_scheduler,
// 110: Telemetry control
//

4
ArduPlane/Parameters.pde
View File

@ -778,6 +778,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),

2
ArduPlane/commands_process.pde
View File

@ -37,7 +37,7 @@ static void update_commands(void)
if(home_is_set == true && g.command_total > 1) {
process_next_command();
}
} // Other (eg GCS_Auto) modes may be implemented here
}
}
static void verify_commands(void)

3
ArduPlane/navigation.pde
View File

@ -129,6 +129,9 @@ static void calc_gndspeed_undershoot()
static void calc_altitude_error()
{
if (control_mode == FLY_BY_WIRE_B) {
return;
}
if (control_mode == AUTO && offset_altitude_cm != 0) {
// limit climb rates
target_altitude_cm = next_WP.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));

25
ArduPlane/sensors.pde
View File

@ -26,8 +26,10 @@ static int32_t read_barometer(void)
// in M/S * 100
static void read_airspeed(void)
{
airspeed.read();
calc_airspeed_errors();
if (airspeed.enabled()) {
airspeed.read();
calc_airspeed_errors();
}
}
static void zero_airspeed(void)
@ -48,12 +50,19 @@ static void read_battery(void)
batt_volt_pin->set_pin(g.battery_volt_pin);
battery.voltage = 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);
battery.current_amps = CURRENT_AMPS(batt_curr_pin);
// .0002778 is 1/3600 (conversion to hours)
battery.current_total_mah += battery.current_amps * (float)delta_ms_medium_loop * 0.0002778;
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);
battery.current_amps = CURRENT_AMPS(batt_curr_pin);
// .0002778 is 1/3600 (conversion to hours)
battery.current_total_mah += battery.current_amps * dt * 0.0002778f;
}
last_time_ms = tnow;
}
if (battery.voltage != 0 &&

6
ArduPlane/system.pde
View File

@ -311,10 +311,9 @@ static void startup_ground(void)
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
hal.uartA->set_blocking_writes(false);
hal.uartB->set_blocking_writes(false);
hal.uartC->set_blocking_writes(false);
if (gcs3.initialised) {
hal.uartC->set_blocking_writes(false);
}
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
@ -523,7 +522,6 @@ static void resetPerfData(void) {
ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0;
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();
}

15
ArduPlane/test.pde
View File

@ -254,7 +254,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);
@ -480,6 +479,8 @@ test_ins(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
uint8_t counter = 0;
while(1) {
delay(20);
@ -493,10 +494,10 @@ test_ins(uint8_t argc, const Menu::arg *argv)
ahrs.update();
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5) {
counter++;
if(counter == 5) {
compass.read();
medium_loopCounter = 0;
counter = 0;
}
}
@ -543,7 +544,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
flash_leds);
ahrs.reset();
int16_t counter = 0;
uint16_t counter = 0;
float heading = 0;
print_hit_enter();
@ -559,15 +560,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// ---
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter == 5) {
if(counter % 5 == 0) {
if (compass.read()) {
// Calculate heading
const Matrix3f &m = ahrs.get_dcm_matrix();
heading = compass.calculate_heading(m);
compass.null_offsets();
}
medium_loopCounter = 0;
}
counter++;