From 7512e686b9f8f3f6d2eec835284f345e004bfdf1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 21 Sep 2011 22:31:12 -0700 Subject: [PATCH] Fixed Timer overflow for throttle PI loop. Moved look at home to update_nav where it belongs --- ArduCopter/ArduCopter.pde | 39 +++++++++++++++++---------------------- ArduCopter/Attitude.pde | 8 +++++--- ArduCopter/defines.h | 2 +- ArduCopter/system.pde | 6 +----- 4 files changed, 24 insertions(+), 31 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 8059e7f3c0..f390c7be57 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -405,7 +405,6 @@ static long original_target_bearing; // deg * 100, used to check we are not p static long old_target_bearing; // used to track difference in angle static int loiter_total; // deg : how many times to loiter * 360 -//static int loiter_delta; // deg : how far we just turned static int loiter_sum; // deg : how far we have turned around a waypoint static long loiter_time; // millis : when we started LOITER mode static int loiter_time_max; // millis : how long to stay in LOITER mode @@ -494,8 +493,7 @@ static byte gps_watchdog; // -------------- static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static uint16_t throttle_timer; -static float delta_throttle; +static unsigned long throttle_timer; static unsigned long fiftyhz_loopTimer; @@ -1012,12 +1010,7 @@ void update_yaw_mode(void) break; case YAW_LOOK_AT_HOME: - // copter will always point at home - if(home_is_set){ - nav_yaw = point_at_home_yaw(); - } else { - nav_yaw = 0; - } + //nav_yaw updated in update_navigation() break; case YAW_AUTO: @@ -1100,13 +1093,14 @@ void update_roll_pitch_mode(void) void update_throttle_mode(void) { switch(throttle_mode){ + case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ g.rc_3.servo_out = g.rc_3.control_in + boost; }else{ g.rc_3.servo_out = 0; } - break; + break; case THROTTLE_HOLD: // allow interactive changing of atitude @@ -1116,6 +1110,7 @@ void update_throttle_mode(void) case THROTTLE_AUTO: // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ + // how far off are we altitude_error = get_altitude_error(); @@ -1124,6 +1119,7 @@ void update_throttle_mode(void) // clear the new data flag invalid_throttle = false; + Serial.printf("nt %d\n",nav_throttle); } // apply throttle control at 200 hz @@ -1193,6 +1189,15 @@ static void update_navigation() break; } + + if(yaw_mode == YAW_LOOK_AT_HOME){ + if(home_is_set){ + //nav_yaw = point_at_home_yaw(); + nav_yaw = get_bearing(¤t_loc, &home); + } else { + nav_yaw = 0; + } + } } static void read_AHRS(void) @@ -1372,9 +1377,6 @@ static void update_nav_wp() // use error as the desired rate towards the target calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0); - // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); - }else if(wp_control == CIRCLE_MODE){ // check if we have missed the WP @@ -1405,9 +1407,6 @@ static void update_nav_wp() // nav_lon, nav_lat is calculated calc_nav_rate(long_error, lat_error, 200, 0); - // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); - } else { // for long journey's reset the wind resopnse // it assumes we are standing still. @@ -1420,9 +1419,9 @@ static void update_nav_wp() // use error as the desired rate towards the target calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 100); - // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); } + // rotate pitch and roll to the copter frame of reference + calc_nav_pitch_roll(); } static void update_auto_yaw() @@ -1437,9 +1436,5 @@ static void update_auto_yaw() // MAV_ROI_NONE = basic Yaw hold } -static long point_at_home_yaw() -{ - return get_bearing(¤t_loc, &home); -} diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8914cebea7..516d9b2201 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -100,9 +100,11 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - long timer = micros(); - delta_throttle = (float)(timer - throttle_timer)/1000000.0; - throttle_timer = timer; + long timer = millis(); + float delta_throttle = (float)(timer - throttle_timer)/1000.0; + throttle_timer = timer; + + Serial.printf("tt %ld, dt %1.4f ", throttle_timer, delta_throttle); return g.pi_throttle.get_pi(rate_error, delta_throttle); } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 70d6dc3d04..269fcef3a8 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -32,7 +32,7 @@ #define CH7_FLIP 2 #define CH7_SIMPLE_MODE 3 #define CH7_RTL 4 -#deinfe CH7_AUTO_TRIM 5 +#define CH7_AUTO_TRIM 5 // Frame types #define QUAD_FRAME 0 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 4f3c9a0a58..b7777c920f 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -349,11 +349,7 @@ static void set_mode(byte mode) control_mode = constrain(control_mode, 0, NUM_MODES - 1); // used to stop fly_aways - if(g.rc_3.control_in == 0){ // throttle is 0 - // we are on the ground is this is true - // disarm motors for Auto - motor_auto_armed = false; - } + motor_auto_armed = (g.rc_3.control_in > 0); Serial.println(flight_mode_strings[control_mode]);