mirror of https://github.com/ArduPilot/ardupilot
Copter: run main loop at 400hz for pixhawk
This commit is contained in:
parent
b7565affcd
commit
2253cf9769
|
@ -212,7 +212,11 @@ static DataFlash_Empty DataFlash;
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// the rate we run the main loop at
|
// the rate we run the main loop at
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
#if MAIN_LOOP_RATE == 400
|
||||||
|
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_400HZ;
|
||||||
|
#else
|
||||||
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ;
|
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ;
|
||||||
|
#endif
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Sensors
|
// Sensors
|
||||||
|
@ -835,6 +839,72 @@ static void pre_arm_checks(bool display_failure);
|
||||||
// setup the var_info table
|
// setup the var_info table
|
||||||
AP_Param param_loader(var_info, WP_START_BYTE);
|
AP_Param param_loader(var_info, WP_START_BYTE);
|
||||||
|
|
||||||
|
#if MAIN_LOOP_RATE == 400
|
||||||
|
/*
|
||||||
|
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
|
||||||
|
should be listed here, along with how often they should be called
|
||||||
|
(in 2.5ms units) and the maximum time they are expected to take (in
|
||||||
|
microseconds)
|
||||||
|
1 = 400hz
|
||||||
|
2 = 200hz
|
||||||
|
4 = 100hz
|
||||||
|
8 = 50hz
|
||||||
|
20 = 20hz
|
||||||
|
40 = 10hz
|
||||||
|
133 = 3hz
|
||||||
|
400 = 1hz
|
||||||
|
4000 = 0.1hz
|
||||||
|
|
||||||
|
*/
|
||||||
|
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||||
|
{ rc_loop, 4, 10 },
|
||||||
|
{ throttle_loop, 8, 45 },
|
||||||
|
{ update_GPS, 8, 90 },
|
||||||
|
{ update_nav_mode, 4, 40 },
|
||||||
|
{ update_batt_compass, 40, 72 },
|
||||||
|
{ read_aux_switches, 40, 5 },
|
||||||
|
{ arm_motors_check, 40, 1 },
|
||||||
|
{ auto_trim, 40, 14 },
|
||||||
|
{ update_altitude, 40, 100 },
|
||||||
|
{ run_nav_updates, 40, 80 },
|
||||||
|
{ three_hz_loop, 133, 9 },
|
||||||
|
{ compass_accumulate, 8, 42 },
|
||||||
|
{ barometer_accumulate, 8, 25 },
|
||||||
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
|
{ check_dynamic_flight, 8, 10 },
|
||||||
|
#endif
|
||||||
|
{ update_notify, 8, 10 },
|
||||||
|
{ one_hz_loop, 400, 42 },
|
||||||
|
{ crash_check, 40, 2 },
|
||||||
|
{ gcs_check_input, 8, 550 },
|
||||||
|
{ gcs_send_heartbeat, 400, 150 },
|
||||||
|
{ gcs_send_deferred, 8, 720 },
|
||||||
|
{ gcs_data_stream_send, 8, 950 },
|
||||||
|
#if COPTER_LEDS == ENABLED
|
||||||
|
{ update_copter_leds, 40, 5 },
|
||||||
|
#endif
|
||||||
|
{ update_mount, 8, 45 },
|
||||||
|
{ ten_hz_logging_loop, 40, 30 },
|
||||||
|
{ fifty_hz_logging_loop, 2, 22 },
|
||||||
|
{ perf_update, 4000, 20 },
|
||||||
|
{ read_receiver_rssi, 40, 5 },
|
||||||
|
#ifdef USERHOOK_FASTLOOP
|
||||||
|
{ userhook_FastLoop, 4, 10 },
|
||||||
|
#endif
|
||||||
|
#ifdef USERHOOK_50HZLOOP
|
||||||
|
{ userhook_50Hz, 8, 10 },
|
||||||
|
#endif
|
||||||
|
#ifdef USERHOOK_MEDIUMLOOP
|
||||||
|
{ userhook_MediumLoop, 40, 10 },
|
||||||
|
#endif
|
||||||
|
#ifdef USERHOOK_SLOWLOOP
|
||||||
|
{ userhook_SlowLoop, 120, 10 },
|
||||||
|
#endif
|
||||||
|
#ifdef USERHOOK_SUPERSLOWLOOP
|
||||||
|
{ userhook_SuperSlowLoop,400, 10 },
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
#else
|
||||||
/*
|
/*
|
||||||
scheduler table - all regular tasks apart from the fast_loop()
|
scheduler table - all regular tasks apart from the fast_loop()
|
||||||
should be listed here, along with how often they should be called
|
should be listed here, along with how often they should be called
|
||||||
|
@ -842,6 +912,7 @@ AP_Param param_loader(var_info, WP_START_BYTE);
|
||||||
microseconds)
|
microseconds)
|
||||||
*/
|
*/
|
||||||
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||||
|
{ rc_loop, 1, 100 },
|
||||||
{ throttle_loop, 2, 450 },
|
{ throttle_loop, 2, 450 },
|
||||||
{ update_GPS, 2, 900 },
|
{ update_GPS, 2, 900 },
|
||||||
{ update_nav_mode, 1, 400 },
|
{ update_nav_mode, 1, 400 },
|
||||||
|
@ -885,6 +956,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||||
{ userhook_SuperSlowLoop,100, 100 },
|
{ userhook_SuperSlowLoop,100, 100 },
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
|
@ -963,8 +1035,8 @@ void loop()
|
||||||
// in multiples of the main loop tick. So if they don't run on
|
// in multiples of the main loop tick. So if they don't run on
|
||||||
// the first call to the scheduler they won't run on a later
|
// the first call to the scheduler they won't run on a later
|
||||||
// call until scheduler.tick() is called again
|
// call until scheduler.tick() is called again
|
||||||
uint32_t time_available = (timer + 10000) - micros();
|
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
|
||||||
scheduler.run(time_available - 300);
|
scheduler.run(time_available);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -1007,6 +1079,12 @@ static void fast_loop()
|
||||||
}
|
}
|
||||||
#endif // OPTFLOW == ENABLED
|
#endif // OPTFLOW == ENABLED
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// rc_loops - reads user input from transmitter/receiver
|
||||||
|
// called at 100hz
|
||||||
|
static void rc_loop()
|
||||||
|
{
|
||||||
// Read radio and 3-position switch on radio
|
// Read radio and 3-position switch on radio
|
||||||
// -----------------------------------------
|
// -----------------------------------------
|
||||||
read_radio();
|
read_radio();
|
||||||
|
|
|
@ -59,6 +59,7 @@
|
||||||
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
|
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
|
# define MAIN_LOOP_RATE 100
|
||||||
# ifdef APM2_BETA_HARDWARE
|
# ifdef APM2_BETA_HARDWARE
|
||||||
# define CONFIG_BARO AP_BARO_BMP085
|
# define CONFIG_BARO AP_BARO_BMP085
|
||||||
# else // APM2 Production Hardware (default)
|
# else // APM2 Production Hardware (default)
|
||||||
|
@ -70,12 +71,14 @@
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
# define OPTFLOW DISABLED
|
# define OPTFLOW DISABLED
|
||||||
|
# define MAIN_LOOP_RATE 100
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||||
# define CONFIG_IMU_TYPE CONFIG_IMU_PX4
|
# define CONFIG_IMU_TYPE CONFIG_IMU_PX4
|
||||||
# define CONFIG_BARO AP_BARO_PX4
|
# define CONFIG_BARO AP_BARO_PX4
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
# define OPTFLOW DISABLED
|
# define OPTFLOW DISABLED
|
||||||
|
# define MAIN_LOOP_RATE 400
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||||
# define CONFIG_IMU_TYPE CONFIG_IMU_FLYMAPLE
|
# define CONFIG_IMU_TYPE CONFIG_IMU_FLYMAPLE
|
||||||
# define CONFIG_BARO AP_BARO_BMP085
|
# define CONFIG_BARO AP_BARO_BMP085
|
||||||
|
@ -84,6 +87,7 @@
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define OPTFLOW DISABLED
|
# define OPTFLOW DISABLED
|
||||||
|
# define MAIN_LOOP_RATE 400
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||||
# define CONFIG_IMU_TYPE CONFIG_IMU_L3G4200D
|
# define CONFIG_IMU_TYPE CONFIG_IMU_L3G4200D
|
||||||
# define CONFIG_BARO AP_BARO_BMP085
|
# define CONFIG_BARO AP_BARO_BMP085
|
||||||
|
@ -92,8 +96,16 @@
|
||||||
# define MAGNETOMETER ENABLED
|
# define MAGNETOMETER ENABLED
|
||||||
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
|
||||||
# define OPTFLOW DISABLED
|
# define OPTFLOW DISABLED
|
||||||
|
# define MAIN_LOOP_RATE 400
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if MAIN_LOOP_RATE == 400
|
||||||
|
# define MAIN_LOOP_SECONDS 0.0025
|
||||||
|
# define MAIN_LOOP_MICROS 2500
|
||||||
|
#else
|
||||||
|
# define MAIN_LOOP_SECONDS 0.01
|
||||||
|
# define MAIN_LOOP_MICROS 10000
|
||||||
|
#endif
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// FRAME_CONFIG
|
// FRAME_CONFIG
|
||||||
//
|
//
|
||||||
|
|
|
@ -232,6 +232,9 @@ static void init_ardupilot()
|
||||||
if(g.compass_enabled)
|
if(g.compass_enabled)
|
||||||
init_compass();
|
init_compass();
|
||||||
|
|
||||||
|
// initialise attitude controller
|
||||||
|
attitude_control.set_dt(MAIN_LOOP_SECONDS);
|
||||||
|
|
||||||
// init the optical flow sensor
|
// init the optical flow sensor
|
||||||
if(g.optflow_enabled) {
|
if(g.optflow_enabled) {
|
||||||
init_optflow();
|
init_optflow();
|
||||||
|
|
Loading…
Reference in New Issue