mirror of https://github.com/ArduPilot/ardupilot
2.0.46 -Not flight tested!
restored 42 alt hold code removed throttle timer, replaced with safer constant increased alt control range reformatted nav_rate calls removed unused simple mode defines set alt hold home to 10m tuned down rateP to .13 from .14 for broader application.
This commit is contained in:
Jason Short
parent
911d840ddd
commit
c209d6e6dd
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.0.45 Beta"
|
||||
#define THISFIRMWARE "ArduCopter V2.0.46 Beta"
|
||||
/*
|
||||
ArduCopter Version 2.0 Beta
|
||||
Authors: Jason Short
|
||||
|
|
@ -129,6 +129,9 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
|||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
|
|
@ -200,10 +203,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
|||
#endif
|
||||
// normal dcm
|
||||
AP_DCM dcm(&imu, g_gps);
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -483,7 +482,6 @@ 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 unsigned long throttle_timer;
|
||||
|
||||
static unsigned long fiftyhz_loopTimer;
|
||||
|
||||
|
|
@ -585,6 +583,9 @@ static void fast_loop()
|
|||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
set_servos_4();
|
||||
|
||||
//if(motor_armed)
|
||||
// Log_Write_Attitude();
|
||||
}
|
||||
|
||||
static void medium_loop()
|
||||
|
|
@ -950,19 +951,10 @@ static void update_GPS(void)
|
|||
// so that the altitude is more accurate
|
||||
// -------------------------------------
|
||||
if (current_loc.lat == 0) {
|
||||
//SendDebugln("!! bad loc");
|
||||
ground_start_count = 5;
|
||||
|
||||
}else{
|
||||
//Serial.printf("init Home!");
|
||||
|
||||
// reset our nav loop timer
|
||||
//nav_loopTimer = millis();
|
||||
init_home();
|
||||
|
||||
// init altitude
|
||||
// commented out because we aren't using absolute altitude
|
||||
// current_loc.alt = home.alt;
|
||||
ground_start_count = 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -1048,10 +1040,7 @@ void update_roll_pitch_mode(void)
|
|||
|
||||
switch(roll_pitch_mode){
|
||||
case ROLL_PITCH_ACRO:
|
||||
// Roll control
|
||||
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
|
||||
|
||||
// Pitch control
|
||||
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
|
||||
break;
|
||||
|
||||
|
|
@ -1070,7 +1059,6 @@ void update_roll_pitch_mode(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
// 50 hz update rate, not 250
|
||||
void update_throttle_mode(void)
|
||||
{
|
||||
|
|
@ -1084,8 +1072,6 @@ void update_throttle_mode(void)
|
|||
g.pi_rate_pitch.reset_I();
|
||||
g.rc_3.servo_out = 0;
|
||||
}
|
||||
// reset the timer to throttle so that we never get fast I term run-ups
|
||||
throttle_timer = 0;
|
||||
break;
|
||||
|
||||
case THROTTLE_HOLD:
|
||||
|
|
@ -1276,11 +1262,11 @@ adjust_altitude()
|
|||
{
|
||||
if(g.rc_3.control_in <= 200){
|
||||
next_WP.alt -= 1; // 1 meter per second
|
||||
next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location
|
||||
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
|
||||
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
|
||||
}else if (g.rc_3.control_in > 700){
|
||||
next_WP.alt += 1; // 1 meter per second
|
||||
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
|
||||
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1360,10 +1346,10 @@ static void update_nav_wp()
|
|||
calc_location_error(&next_WP);
|
||||
|
||||
// use error as the desired rate towards the target
|
||||
calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0);
|
||||
calc_loiter(long_error, lat_error);
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
calc_loiter_pitch_roll();
|
||||
|
||||
}else if(wp_control == CIRCLE_MODE){
|
||||
|
||||
|
|
@ -1393,18 +1379,18 @@ static void update_nav_wp()
|
|||
|
||||
// use error as the desired rate towards the target
|
||||
// nav_lon, nav_lat is calculated
|
||||
calc_nav_rate(long_error, lat_error, 200, 0);
|
||||
calc_loiter(long_error, lat_error);
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
calc_loiter_pitch_roll();
|
||||
|
||||
} else {
|
||||
// for long journey's reset the wind resopnse
|
||||
// it assumes we are standing still.
|
||||
// use error as the desired rate towards the target
|
||||
calc_nav_rate2(g.waypoint_speed_max);
|
||||
calc_nav_rate(g.waypoint_speed_max);
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll2();
|
||||
calc_nav_pitch_roll();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle)
|
|||
return (int)constrain(rate, -2500, 2500);
|
||||
}
|
||||
|
||||
#define ALT_ERROR_MAX 350
|
||||
#define ALT_ERROR_MAX 300
|
||||
static int
|
||||
get_nav_throttle(long z_error, int target_speed)
|
||||
{
|
||||
|
|
@ -95,9 +95,8 @@ get_nav_throttle(long z_error, int target_speed)
|
|||
// limit error to prevent I term run up
|
||||
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
||||
target_speed = z_error * scaler;
|
||||
|
||||
rate_error = target_speed - altitude_rate;
|
||||
rate_error = constrain(rate_error, -120, 140);
|
||||
rate_error = constrain(rate_error, -110, 110);
|
||||
|
||||
return (int)g.pi_throttle.get_pi(rate_error, .1);
|
||||
}
|
||||
|
|
@ -106,10 +105,9 @@ static int
|
|||
get_rate_roll(long target_rate)
|
||||
{
|
||||
long error;
|
||||
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, G_Dt);
|
||||
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, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
|
|
@ -119,10 +117,9 @@ static int
|
|||
get_rate_pitch(long target_rate)
|
||||
{
|
||||
long error;
|
||||
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, G_Dt);
|
||||
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, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
|
|
@ -132,7 +129,6 @@ static int
|
|||
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, G_Dt);
|
||||
|
||||
|
|
|
|||
|
|
@ -221,8 +221,6 @@
|
|||
//#define OPTFLOW_ENABLED
|
||||
#endif
|
||||
|
||||
//#define OPTFLOW_ENABLED
|
||||
|
||||
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
|
||||
# define OPTFLOW DISABLED
|
||||
#endif
|
||||
|
|
@ -317,19 +315,6 @@
|
|||
// Attitude Control
|
||||
//
|
||||
|
||||
// SIMPLE Mode
|
||||
#ifndef SIMPLE_YAW
|
||||
# define SIMPLE_YAW YAW_HOLD
|
||||
#endif
|
||||
|
||||
#ifndef SIMPLE_RP
|
||||
# define SIMPLE_RP ROLL_PITCH_STABLE
|
||||
#endif
|
||||
|
||||
#ifndef SIMPLE_THR
|
||||
# define SIMPLE_THR THROTTLE_MANUAL
|
||||
#endif
|
||||
|
||||
// Alt Hold Mode
|
||||
#ifndef ALT_HOLD_YAW
|
||||
# define ALT_HOLD_YAW YAW_HOLD
|
||||
|
|
@ -403,40 +388,40 @@
|
|||
// Attitude Control
|
||||
//
|
||||
#ifndef STABILIZE_ROLL_P
|
||||
# define STABILIZE_ROLL_P 4.2
|
||||
# define STABILIZE_ROLL_P 4.0
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_I
|
||||
# define STABILIZE_ROLL_I 0.001
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_IMAX
|
||||
# define STABILIZE_ROLL_IMAX 0 // degrees
|
||||
# define STABILIZE_ROLL_IMAX 1.5 // degrees
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZE_PITCH_P
|
||||
# define STABILIZE_PITCH_P 4.2
|
||||
# define STABILIZE_PITCH_P 4.0
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_I
|
||||
# define STABILIZE_PITCH_I 0.001
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_IMAX
|
||||
# define STABILIZE_PITCH_IMAX 0 // degrees
|
||||
# define STABILIZE_PITCH_IMAX 1.5 // degrees
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Rate Control
|
||||
//
|
||||
#ifndef RATE_ROLL_P
|
||||
# define RATE_ROLL_P 0.14
|
||||
# define RATE_ROLL_P .13
|
||||
#endif
|
||||
#ifndef RATE_ROLL_I
|
||||
# define RATE_ROLL_I 0 //0.18
|
||||
# define RATE_ROLL_I 0.0
|
||||
#endif
|
||||
#ifndef RATE_ROLL_IMAX
|
||||
# define RATE_ROLL_IMAX 15 // degrees
|
||||
#endif
|
||||
|
||||
#ifndef RATE_PITCH_P
|
||||
# define RATE_PITCH_P 0.14
|
||||
# define RATE_PITCH_P 0.13
|
||||
#endif
|
||||
#ifndef RATE_PITCH_I
|
||||
# define RATE_PITCH_I 0 //0.18
|
||||
|
|
@ -519,7 +504,7 @@
|
|||
# define THROTTLE_P 0.6 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.02 // with 4m error, 8 PWM gain/s
|
||||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||
#endif
|
||||
#ifndef THROTTLE_IMAX
|
||||
# define THROTTLE_IMAX 300
|
||||
|
|
@ -671,7 +656,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef ALT_HOLD_HOME
|
||||
# define ALT_HOLD_HOME -1
|
||||
# define ALT_HOLD_HOME 10
|
||||
#endif
|
||||
|
||||
#ifndef USE_CURRENT_ALT
|
||||
|
|
|
|||
|
|
@ -18,10 +18,7 @@ static void read_control_switch()
|
|||
// setup Simple mode
|
||||
// do we enable simple mode?
|
||||
do_simple = (g.simple_modes & (1 << switchPosition));
|
||||
//Serial.printf("do simple: %d \n", (int)do_simple);
|
||||
#endif
|
||||
|
||||
|
||||
}else{
|
||||
switch_debouncer = true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -59,35 +59,16 @@ static void calc_location_error(struct Location *next_loc)
|
|||
// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0);
|
||||
|
||||
#define NAV_ERR_MAX 400
|
||||
static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
{
|
||||
// moved to globals for logging
|
||||
//int x_actual_speed, y_actual_speed;
|
||||
//int x_rate_error, y_rate_error;
|
||||
|
||||
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
|
||||
float scaler = (float)max_speed/(float)NAV_ERR_MAX;
|
||||
g.pi_loiter_lat.kP(scaler);
|
||||
g.pi_loiter_lon.kP(scaler);
|
||||
|
||||
int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
|
||||
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
|
||||
|
||||
//Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed);
|
||||
|
||||
if(x_target_speed > 0){
|
||||
x_target_speed = max(x_target_speed, min_speed);
|
||||
}else{
|
||||
x_target_speed = min(x_target_speed, -min_speed);
|
||||
}
|
||||
|
||||
if(y_target_speed > 0){
|
||||
y_target_speed = max(y_target_speed, min_speed);
|
||||
}else{
|
||||
y_target_speed = min(y_target_speed, -min_speed);
|
||||
}
|
||||
|
||||
// find the rates:
|
||||
float temp = radians((float)g_gps->ground_course/100.0);
|
||||
|
||||
|
|
@ -116,7 +97,7 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
|
|||
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
|
||||
}
|
||||
|
||||
static void calc_nav_rate2(int max_speed)
|
||||
static void calc_nav_rate(int max_speed)
|
||||
{
|
||||
/*
|
||||
0 1 2 3 4 5 6 7 8
|
||||
|
|
@ -151,7 +132,7 @@ static void calc_nav_rate2(int max_speed)
|
|||
}
|
||||
|
||||
// nav_roll, nav_pitch
|
||||
static void calc_nav_pitch_roll2()
|
||||
static void calc_nav_pitch_roll()
|
||||
{
|
||||
float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0);
|
||||
float _cos_yaw_x = cos(temp);
|
||||
|
|
@ -173,7 +154,7 @@ static void calc_nav_pitch_roll2()
|
|||
|
||||
|
||||
// nav_roll, nav_pitch
|
||||
static void calc_nav_pitch_roll()
|
||||
static void calc_loiter_pitch_roll()
|
||||
{
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
|
||||
|
|
|
|||
Loading…
Reference in New Issue