Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2011-12-31 20:09:26 -08:00
parent 31765310ac 35fba5d4a3
commit 1cfb7ecf7c
5 changed files with 37 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/ArduCopter.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.1.1r4 alpha"
#define THISFIRMWARE "ArduCopter V2.1.1r5 alpha"
/*
ArduCopter Version 2.0 Beta
Authors: Jason Short
@ -744,7 +744,6 @@ static void medium_loop()
// -------------------
g_gps->new_data = false;
// calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
if(navigate()){
@ -1060,11 +1059,13 @@ static void update_GPS(void)
// ---------------
gps_fix_count++;
// we are not tracking I term on navigation, so this isn't needed
// used to calculate speed in X and Y, iterms
// ------------------------------------------
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
// --------------------------
dTnav = min(dTnav, 1.0);
if(ground_start_count > 1){
@ -1420,6 +1421,12 @@ static void update_navigation()
update_nav_wp();
break;
case STABILIZE:
//wp_control = NO_NAV_MODE;
//update_nav_wp();
break;
}
// are we in SIMPLE mode?
@ -1767,6 +1774,10 @@ static void update_nav_wp()
}else if(wp_control == NO_NAV_MODE){
nav_roll = 0;
nav_pitch = 0;
// calc the Iterms for Loiter based on velicity
//calc_position_hold();
}
}

2
ArduCopter/config.h
View File

@ -582,7 +582,7 @@
// YAW Control
//
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 9.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance

29
ArduCopter/navigation.pde
View File

@ -94,6 +94,17 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
}
static void calc_position_hold()
{
// only if we are not moving
//if (x_actual_speed == 0){
// // what is the error?
// int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
//}
}
#define NAV_ERR_MAX 800
static void calc_loiter(int x_error, int y_error)
{
@ -102,20 +113,20 @@ static void calc_loiter(int x_error, int y_error)
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -1000, 1000);
nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
nav_lon_p = constrain(nav_lon_p, -3500, 3500);
nav_lon_p = constrain(nav_lon_p, -1200, 1200);
nav_lon = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -2500, 2500);
// North/South
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
y_rate_error = y_target_speed - y_actual_speed;
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
nav_lat_p = constrain(nav_lat_p, -3500, 3500);
nav_lat_p = constrain(nav_lat_p, -1200, 1200);
nav_lat = nav_lat_p + y_iterm;
nav_lat = constrain(nav_lat, -2500, 2500);
/*
int8_t ttt = 1.0/dTnav;
@ -232,23 +243,23 @@ static void calc_nav_rate(int max_speed)
//x_actual_speed = -sin(temp) * (float)g_gps->ground_speed;
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
x_rate_error = constrain(x_rate_error, -1400, 1400);
x_rate_error = constrain(x_rate_error, -1000, 1000);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
nav_lon_p = constrain(nav_lon_p, -3500, 3500);
nav_lon = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -3000, 3000);
// heading towards target
//y_actual_speed = cos(temp) * (float)g_gps->ground_speed;
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -1400, 1400); // added a rate error limit to keep pitching down to a minimum
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
nav_lat_p = constrain(nav_lat_p, -3500, 3500);
nav_lat = nav_lat_p + y_iterm;
nav_lat = constrain(nav_lat, -3000, 3000);
/*
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",

2
ArduPlane/ArduPlane.pde
View File

@ -905,7 +905,7 @@ static void update_current_flight_mode(void)
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == 0)) {
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
} else {
if (g.channel_pitch.norm_input()<0)

2
ArduPlane/Parameters.h
View File

@ -85,7 +85,7 @@ public:
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
k_param_FBWB_min_altitude, // -1=disabled, minimum value for altitude in cm (default 30m)
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
//
// 130: Sensor parameters