mirror of https://github.com/ArduPilot/ardupilot
Fixed Timer overflow for throttle PI loop.
Moved look at home to update_nav where it belongs
This commit is contained in:
parent
c2d56de264
commit
7512e686b9
|
@ -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,6 +1093,7 @@ 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;
|
||||
|
@ -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,10 +1419,10 @@ 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();
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -100,10 +100,12 @@ 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;
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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]);
|
||||
|
||||
|
|
Loading…
Reference in New Issue