mirror of https://github.com/ArduPilot/ardupilot
changed PI Library to take Float for DeltaTime
Changed timing to be more accurate in main loop. Still need to switch to micros(). Left that for Tridge. This gets rid of six floating point calcs *250. Nice!
This commit is contained in:
Jason Short
parent
a502d04e6e
commit
3b4743ef03
|
|
@ -490,9 +490,9 @@ static byte gps_watchdog;
|
|||
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
||||
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
||||
|
||||
static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
|
||||
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
||||
static uint8_t delta_ms_medium_loop;
|
||||
static uint16_t throttle_timer;
|
||||
static float delta_throttle;
|
||||
|
||||
static unsigned long fiftyhz_loopTimer;
|
||||
static uint8_t delta_ms_fiftyhz;
|
||||
|
|
@ -502,7 +502,7 @@ static int superslow_loopCounter;
|
|||
static byte simple_timer; // for limiting the execution of flight mode thingys
|
||||
|
||||
|
||||
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
static float dTnav; // Delta Time in milliseconds for navigation computations
|
||||
static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
//static float load; // % MCU cycles used
|
||||
|
||||
|
|
@ -526,9 +526,6 @@ void loop()
|
|||
// ----------------------------
|
||||
if (millis() - fast_loopTimer >= 4) {
|
||||
//PORTK |= B00010000;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
|
|
@ -573,18 +570,6 @@ void loop()
|
|||
// Main loop
|
||||
static void fast_loop()
|
||||
{
|
||||
// IMU DCM Algorithm
|
||||
read_AHRS();
|
||||
|
||||
// This is the fast loop - we want it to execute at >= 100Hz
|
||||
// ---------------------------------------------------------
|
||||
if (delta_ms_fast_loop > G_Dt_max)
|
||||
G_Dt_max = delta_ms_fast_loop;
|
||||
|
||||
// Read radio
|
||||
// ----------
|
||||
read_radio();
|
||||
|
||||
// try to send any deferred messages if the serial port now has
|
||||
// some space available
|
||||
gcs.send_message(MSG_RETRY_DEFERRED);
|
||||
|
|
@ -592,6 +577,22 @@ static void fast_loop()
|
|||
hil.send_message(MSG_RETRY_DEFERRED);
|
||||
#endif
|
||||
|
||||
// Read radio
|
||||
// ----------
|
||||
read_radio();
|
||||
|
||||
// IMU DCM Algorithm
|
||||
read_AHRS();
|
||||
|
||||
fast_loopTimer = millis();
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by PI Loops
|
||||
|
||||
// Look for slow loop times
|
||||
// ------------------------
|
||||
if (delta_ms_fast_loop > G_Dt_max)
|
||||
G_Dt_max = delta_ms_fast_loop;
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
update_yaw_mode();
|
||||
|
|
@ -663,7 +664,7 @@ static void medium_loop()
|
|||
g_gps->new_data = false;
|
||||
|
||||
// we are not tracking I term on navigation, so this isn't needed
|
||||
dTnav = millis() - nav_loopTimer;
|
||||
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
|
||||
nav_loopTimer = millis();
|
||||
|
||||
// calculate the copter's desired bearing and WP distance
|
||||
|
|
@ -745,9 +746,6 @@ static void medium_loop()
|
|||
loop_step = 5;
|
||||
medium_loopCounter = 0;
|
||||
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
|
||||
if (g.battery_monitoring != 0){
|
||||
read_battery();
|
||||
}
|
||||
|
|
@ -1190,7 +1188,6 @@ static void update_navigation()
|
|||
// calculates desired Yaw
|
||||
update_auto_yaw();
|
||||
{
|
||||
//circle_angle += dTnav; //1000 * (dTnav/1000);
|
||||
circle_angle = wrap_360(target_bearing + 3000 + 18000);
|
||||
|
||||
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));
|
||||
|
|
|
|||
|
|
@ -13,13 +13,13 @@ get_stabilize_roll(long target_angle)
|
|||
error = constrain(error, -2500, 2500);
|
||||
|
||||
// desired Rate:
|
||||
rate = g.pi_stabilize_roll.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.x) * 100.0);
|
||||
rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
#endif
|
||||
|
||||
|
|
@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle)
|
|||
error = constrain(error, -2500, 2500);
|
||||
|
||||
// desired Rate:
|
||||
rate = g.pi_stabilize_pitch.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.y) * 100.0);
|
||||
rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
#endif
|
||||
|
||||
|
|
@ -66,19 +66,19 @@ get_stabilize_yaw(long target_angle)
|
|||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
|
||||
rate = g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop);
|
||||
rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt);
|
||||
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
||||
if( ! g.heli_ext_gyro_enabled ) {
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.z) * 100.0);
|
||||
rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
}
|
||||
#else
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.z) * 100.0);
|
||||
rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
#endif
|
||||
|
||||
|
|
@ -100,7 +100,10 @@ get_nav_throttle(long z_error, int target_speed)
|
|||
rate_error = target_speed - altitude_rate;
|
||||
rate_error = constrain(rate_error, -110, 110);
|
||||
|
||||
return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||
delta_throttle = (float)(millis() - throttle_timer)/1000.0;
|
||||
throttle_timer = millis();
|
||||
|
||||
return g.pi_throttle.get_pi(rate_error, delta_throttle);
|
||||
}
|
||||
|
||||
static int
|
||||
|
|
@ -110,7 +113,7 @@ get_rate_roll(long target_rate)
|
|||
target_rate = constrain(target_rate, -2500, 2500);
|
||||
|
||||
error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
|
||||
target_rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
|
||||
target_rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
|
|
@ -123,7 +126,7 @@ get_rate_pitch(long target_rate)
|
|||
target_rate = constrain(target_rate, -2500, 2500);
|
||||
|
||||
error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
|
||||
target_rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
|
||||
target_rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
|
|
@ -135,7 +138,7 @@ get_rate_yaw(long target_rate)
|
|||
long error;
|
||||
|
||||
error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
|
||||
target_rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
|
||||
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ static void read_battery(void)
|
|||
|
||||
if(g.battery_monitoring == 4) {
|
||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
||||
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
|
||||
current_total += current_amps * 0.0278; // called at 100ms on average
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == 1
|
||||
|
|
|
|||
|
|
@ -649,7 +649,7 @@ static int8_t
|
|||
test_current(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delta_ms_medium_loop = 100;
|
||||
//delta_ms_medium_loop = 100;
|
||||
|
||||
while(1){
|
||||
delay(100);
|
||||
|
|
|
|||
|
|
@ -8,16 +8,16 @@
|
|||
#include "APM_PI.h"
|
||||
|
||||
long
|
||||
APM_PI::get_pi(int32_t error, uint16_t dt)
|
||||
APM_PI::get_pi(int32_t error, float dt)
|
||||
{
|
||||
float output = 0;
|
||||
float delta_time = (float)dt / 1000.0;
|
||||
//float delta_time = (float)dt / 1000.0;
|
||||
|
||||
// Compute proportional component
|
||||
output += error * _kp;
|
||||
|
||||
// Compute integral component if time has elapsed
|
||||
_integrator += (error * _ki) * delta_time;
|
||||
_integrator += (error * _ki) * dt;
|
||||
|
||||
if (_integrator < -_imax) {
|
||||
_integrator = -_imax;
|
||||
|
|
@ -26,7 +26,6 @@ APM_PI::get_pi(int32_t error, uint16_t dt)
|
|||
}
|
||||
|
||||
output += _integrator;
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -77,7 +77,7 @@ public:
|
|||
///
|
||||
/// @returns The updated control output.
|
||||
///
|
||||
long get_pi(int32_t error, uint16_t dt);
|
||||
long get_pi(int32_t error, float dt);
|
||||
|
||||
/// Reset the PI integrator
|
||||
///
|
||||
|
|
|
|||
Loading…
Reference in New Issue