Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: use G_Dt for acro angle integration

Browse Source 
also cleanup some other uses of performance timing
This commit is contained in:
Andrew Tridgell 2013-10-12 13:30:27 +11:00
parent 05115684c1
commit 035ac3800a
7 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduPlane/ArduPlane.pde
View File

@ -649,26 +649,26 @@ static int32_t offset_altitude_cm;
////////////////////////////////////////////////////////////////////////////////
// The main loop execution time. Seconds
//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
static float G_Dt = 0.02;
static float G_Dt = 0.02f;
////////////////////////////////////////////////////////////////////////////////
// Performance monitoring
////////////////////////////////////////////////////////////////////////////////
// Timer used to accrue data and trigger recording of the performanc monitoring log message
static int32_t perf_mon_timer;
static uint32_t perf_mon_timer;
// The maximum main loop execution time recorded in the current performance monitoring interval
static int16_t G_Dt_max = 0;
static uint32_t G_Dt_max = 0;
// The number of gps fixes recorded in the current performance monitoring interval
static uint8_t gps_fix_count = 0;
////////////////////////////////////////////////////////////////////////////////
// System Timers
////////////////////////////////////////////////////////////////////////////////
// Time in miliseconds of start of main control loop. Milliseconds
static uint32_t fast_loopTimer_ms;
// Time in microseconds of start of main control loop
static uint32_t fast_loopTimer_us;
// Number of milliseconds used in last main loop cycle
static uint8_t delta_ms_fast_loop;
static uint32_t delta_us_fast_loop;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
static uint16_t mainLoop_count;
@ -765,11 +765,11 @@ void loop()
if (!ins.wait_for_sample(1000)) {
return;
}
uint32_t timer = millis();
uint32_t timer = hal.scheduler->micros();
delta_ms_fast_loop = timer - fast_loopTimer_ms;
G_Dt = delta_ms_fast_loop * 0.001f;
fast_loopTimer_ms = timer;
delta_us_fast_loop = timer - fast_loopTimer_us;
G_Dt = delta_us_fast_loop * 1.0e-6f;
fast_loopTimer_us = timer;
mainLoop_count++;
@ -785,7 +785,11 @@ void loop()
// 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
// call until scheduler.tick() is called again
scheduler.run(19500U);
uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
if (remaining > 19500) {
remaining = 19500;
}
scheduler.run(remaining);
}
// Main loop 50Hz
@ -793,8 +797,8 @@ static void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
if (delta_ms_fast_loop > G_Dt_max)
G_Dt_max = delta_ms_fast_loop;
if (delta_us_fast_loop > G_Dt_max)
G_Dt_max = delta_us_fast_loop;
// Read radio
// ----------

6
ArduPlane/Attitude.pde
View File

@ -260,7 +260,7 @@ static void stabilize_acro(float speed_scaler)
acro_state.locked_roll = true;
acro_state.locked_roll_err = 0;
} else {
acro_state.locked_roll_err += ahrs.get_gyro().x * 0.02f;
acro_state.locked_roll_err += ahrs.get_gyro().x * G_Dt;
}
int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
@ -428,7 +428,7 @@ static void calc_nav_yaw_ground(void)
channel_rudder->servo_out = steerController.get_steering_out_rate(steer_rate);
} else {
// use a error controller on the summed error
steer_state.locked_course_err += ahrs.get_gyro().z * 0.02f;
steer_state.locked_course_err += ahrs.get_gyro().z * G_Dt;
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
channel_rudder->servo_out = steerController.get_steering_out_angle_error(yaw_error_cd);
}
@ -460,7 +460,7 @@ static void throttle_slew_limit(int16_t last_throttle)
// if slew limit rate is set to zero then do not slew limit
if (aparm.throttle_slewrate) {
// limit throttle change by the given percentage per second
float temp = aparm.throttle_slewrate * G_Dt * 0.01 * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
float temp = aparm.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle
if (temp < 1) {
temp = 1;

6
ArduPlane/Log.pde
View File

@ -183,7 +183,7 @@ struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint32_t loop_time;
uint16_t main_loop_count;
int16_t g_dt_max;
uint32_t g_dt_max;
uint8_t renorm_count;
uint8_t renorm_blowup;
uint8_t gps_fix_count;
@ -198,7 +198,7 @@ static void Log_Write_Performance()
{
struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
loop_time : millis()- perf_mon_timer,
loop_time : millis() - perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
renorm_count : ahrs.renorm_range_count,
@ -440,7 +440,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,I2CErr" },
"PM", "IHIBBBhhhhB", "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),

2
ArduPlane/navigation.pde
View File

@ -209,7 +209,7 @@ static void update_fbwb_speed_height(void)
elevator_input = -elevator_input;
}
target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_ms_fast_loop * 0.1f;
target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f;
if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
// the user has just released the elevator, lock in

2
ArduPlane/radio.pde
View File

@ -100,7 +100,7 @@ static void read_radio()
channel_throttle->servo_out = channel_throttle->control_in;
if (g.throttle_nudge && channel_throttle->servo_out > 50) {
float nudge = (channel_throttle->servo_out - 50) * 0.02;
float nudge = (channel_throttle->servo_out - 50) * 0.02f;
if (airspeed.use()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {

4
ArduPlane/system.pde
View File

@ -463,9 +463,9 @@ static void update_notify()
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
G_Dt_max = 0;
ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0;
ahrs.renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
}

12
ArduPlane/test.pde
View File

@ -440,10 +440,8 @@ test_ins(uint8_t argc, const Menu::arg *argv)
while(1) {
delay(20);
if (millis() - fast_loopTimer_ms > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer_ms = millis();
if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = hal.scheduler->micros();
// INS
// ---
@ -506,10 +504,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
while(1) {
delay(20);
if (millis() - fast_loopTimer_ms > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer_ms = millis();
if (hal.scheduler->micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = hal.scheduler->micros();
// INS
// ---