mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
Chris Anderson
commit
31765310ac
|
|
@ -69,6 +69,3 @@
|
|||
// enable this for the new 'APM2' hardware
|
||||
// #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// #define APM2_BETA_HARDWARE // for developers who received an early beta board with the older BMP085
|
||||
|
||||
#define LOITER_METHOD 0
|
||||
// set to 1 to try an alternative Loiter control method
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r3 alpha"
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r4 alpha"
|
||||
/*
|
||||
ArduCopter Version 2.0 Beta
|
||||
Authors: Jason Short
|
||||
|
|
@ -366,6 +366,7 @@ static bool nav_ok;
|
|||
static const float radius_of_earth = 6378100; // meters
|
||||
static const float gravity = 9.81; // meters/ sec^2
|
||||
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
static int32_t nav_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
static int32_t home_bearing; // used to track difference in angle
|
||||
|
||||
static byte wp_control; // used to control - navgation or loiter
|
||||
|
|
@ -1390,8 +1391,6 @@ static void update_navigation()
|
|||
// reset LOITER to current position
|
||||
next_WP = current_loc;
|
||||
|
||||
// clear Loiter Iterms
|
||||
reset_nav();
|
||||
}else{
|
||||
// this is also set by GPS in update_nav
|
||||
wp_control = LOITER_MODE;
|
||||
|
|
@ -1762,7 +1761,8 @@ static void update_nav_wp()
|
|||
// use error as the desired rate towards the target
|
||||
calc_nav_rate(g.waypoint_speed_max);
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
//calc_nav_pitch_roll();
|
||||
calc_loiter_pitch_roll();
|
||||
|
||||
}else if(wp_control == NO_NAV_MODE){
|
||||
nav_roll = 0;
|
||||
|
|
@ -1772,11 +1772,16 @@ static void update_nav_wp()
|
|||
|
||||
static void update_auto_yaw()
|
||||
{
|
||||
// If we Loiter, don't start Yawing, allow Yaw control
|
||||
if(wp_control == LOITER_MODE)
|
||||
return;
|
||||
|
||||
// this tracks a location so the copter is always pointing towards it.
|
||||
if(yaw_tracking == MAV_ROI_LOCATION){
|
||||
auto_yaw = get_bearing(¤t_loc, &target_WP);
|
||||
|
||||
}else if(yaw_tracking == MAV_ROI_WPNEXT){
|
||||
// Point towards next WP
|
||||
auto_yaw = target_bearing;
|
||||
}
|
||||
// MAV_ROI_NONE = basic Yaw hold
|
||||
|
|
|
|||
|
|
@ -243,8 +243,8 @@ static void reset_nav(void)
|
|||
g.pi_nav_lon.reset_I();
|
||||
|
||||
// considering not reseting wind control
|
||||
//g.pi_loiter_lat.reset_I();
|
||||
//g.pi_loiter_lon.reset_I();
|
||||
g.pi_loiter_lat.reset_I();
|
||||
g.pi_loiter_lon.reset_I();
|
||||
|
||||
circle_angle = 0;
|
||||
crosstrack_error = 0;
|
||||
|
|
|
|||
|
|
@ -73,11 +73,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
}
|
||||
|
||||
uint8_t status = MAV_STATE_ACTIVE;
|
||||
|
||||
|
||||
if (!motor_armed) {
|
||||
status = MAV_STATE_STANDBY;
|
||||
}
|
||||
|
||||
|
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
|
||||
mavlink_msg_sys_status_send(
|
||||
|
|
@ -116,12 +116,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
|||
chan,
|
||||
nav_roll / 1.0e2,
|
||||
nav_pitch / 1.0e2,
|
||||
nav_bearing / 1.0e2,
|
||||
target_bearing / 1.0e2,
|
||||
dcm.yaw_sensor / 1.0e2, // was target_bearing
|
||||
wp_distance,
|
||||
altitude_error / 1.0e2,
|
||||
nav_lon, // was 0
|
||||
nav_lat); // was 0
|
||||
0,
|
||||
crosstrack_error); // was 0
|
||||
}
|
||||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
|
|
|
|||
|
|
@ -329,7 +329,7 @@ public:
|
|||
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
|
||||
loiter_radius (LOITER_RADIUS, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
|
||||
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
|
||||
crosstrack_gain (CROSSTRACK_GAIN, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
|
||||
crosstrack_gain (CROSSTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
|
||||
|
||||
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
|
||||
throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")),
|
||||
|
|
|
|||
|
|
@ -8,6 +8,9 @@ static void init_commands()
|
|||
prev_nav_index = NO_COMMAND;
|
||||
command_cond_queue.id = NO_COMMAND;
|
||||
command_nav_queue.id = NO_COMMAND;
|
||||
|
||||
// default Yaw tracking
|
||||
yaw_tracking = MAV_ROI_WPNEXT;
|
||||
}
|
||||
|
||||
// Getters
|
||||
|
|
@ -176,6 +179,7 @@ static void set_next_WP(struct Location *wp)
|
|||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing(&prev_WP, &next_WP);
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ---------------------------------
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ static void execute_nav_command(void)
|
|||
process_nav_command();
|
||||
|
||||
// clear navigation prameters
|
||||
reset_nav();
|
||||
//reset_nav();
|
||||
|
||||
// clear May indexes to force loading of more commands
|
||||
// existing May commands are tossed.
|
||||
|
|
|
|||
|
|
@ -508,7 +508,7 @@
|
|||
|
||||
|
||||
#ifndef STABILIZE_D
|
||||
# define STABILIZE_D .25
|
||||
# define STABILIZE_D .2
|
||||
#endif
|
||||
|
||||
// Jasons default values that are good for smaller payload motors.
|
||||
|
|
@ -606,17 +606,17 @@
|
|||
// Navigation control gains
|
||||
//
|
||||
#ifndef LOITER_P
|
||||
# define LOITER_P .2 // .3 was too aggressive
|
||||
# define LOITER_P .25 //
|
||||
#endif
|
||||
#ifndef LOITER_I
|
||||
# define LOITER_I 0.05 // Wind control
|
||||
# define LOITER_I 0.1 // Wind control
|
||||
#endif
|
||||
#ifndef LOITER_IMAX
|
||||
# define LOITER_IMAX 30 // degrees°
|
||||
#endif
|
||||
|
||||
#ifndef NAV_P
|
||||
# define NAV_P 1.5 // 3 was causing rapid oscillations in Loiter
|
||||
# define NAV_P 2.2 // 3 was causing rapid oscillations in Loiter
|
||||
#endif
|
||||
#ifndef NAV_I
|
||||
# define NAV_I 0.15 // used in traverals
|
||||
|
|
@ -649,7 +649,7 @@
|
|||
|
||||
// RATE control
|
||||
#ifndef THROTTLE_P
|
||||
# define THROTTLE_P 0.4 //
|
||||
# define THROTTLE_P 0.5 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.0 //
|
||||
|
|
@ -663,7 +663,7 @@
|
|||
// Crosstrack compensation
|
||||
//
|
||||
#ifndef CROSSTRACK_GAIN
|
||||
# define CROSSTRACK_GAIN 40
|
||||
# define CROSSTRACK_GAIN 1
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -22,6 +22,10 @@ static byte navigate()
|
|||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
home_to_copter_bearing = get_bearing(&home, ¤t_loc);
|
||||
|
||||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -55,13 +59,16 @@ static void calc_XY_velocity(){
|
|||
x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
|
||||
y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
|
||||
|
||||
if(g_gps->ground_speed > 120){
|
||||
// Above simply works better than GPS groundspeed
|
||||
// which is proving to be problematic
|
||||
|
||||
/*if(g_gps->ground_speed > 120){
|
||||
// Derive X/Y speed from GPS
|
||||
// this is far more accurate when traveling about 1.5m/s
|
||||
float temp = g_gps->ground_course * RADX100;
|
||||
x_GPS_speed = sin(temp) * (float)g_gps->ground_speed;
|
||||
y_GPS_speed = cos(temp) * (float)g_gps->ground_speed;
|
||||
}
|
||||
}*/
|
||||
|
||||
last_longitude = g_gps->longitude;
|
||||
last_latutude = g_gps->latitude;
|
||||
|
|
@ -110,7 +117,7 @@ static void calc_loiter(int x_error, int y_error)
|
|||
nav_lat_p = constrain(nav_lat_p, -3500, 3500);
|
||||
nav_lat = nav_lat_p + y_iterm;
|
||||
|
||||
///*
|
||||
/*
|
||||
int8_t ttt = 1.0/dTnav;
|
||||
int16_t t2 = g.pi_nav_lat.get_integrator();
|
||||
|
||||
|
|
@ -119,6 +126,7 @@ static void calc_loiter(int x_error, int y_error)
|
|||
wp_distance, //1
|
||||
y_error, //2
|
||||
y_GPS_speed, //3
|
||||
|
||||
y_actual_speed, //4 ;
|
||||
y_target_speed, //5
|
||||
y_rate_error, //6
|
||||
|
|
@ -152,16 +160,24 @@ static void calc_loiter(int x_error, int y_error)
|
|||
static void estimate_velocity()
|
||||
{
|
||||
// we need to extimate velocity when below GPS threshold of 1.5m/s
|
||||
if(g_gps->ground_speed < 150){
|
||||
//if(g_gps->ground_speed < 120){
|
||||
// some smoothing to prevent bumpy rides
|
||||
x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16;
|
||||
y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16;
|
||||
|
||||
}else{
|
||||
// integration of nav_p angle
|
||||
//x_actual_speed += (nav_lon_p >>2);
|
||||
//y_actual_speed += (nav_lat_p >>2);
|
||||
|
||||
// this is just what worked best in SIM
|
||||
//x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4;
|
||||
//y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4;
|
||||
|
||||
//}else{
|
||||
// less smoothing needed since the GPS already filters
|
||||
x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
|
||||
y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
|
||||
}
|
||||
// x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
|
||||
// y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
|
||||
//}
|
||||
}
|
||||
|
||||
// this calculation rotates our World frame of reference to the copter's frame of reference
|
||||
|
|
@ -206,32 +222,47 @@ static void calc_nav_rate(int max_speed)
|
|||
max_speed = min(max_speed, waypoint_speed_gov);
|
||||
}
|
||||
|
||||
float temp = (target_bearing - g_gps->ground_course) * RADX100;
|
||||
|
||||
// push us towards the original track
|
||||
update_crosstrack();
|
||||
|
||||
// nav_bearing includes crosstrack
|
||||
float temp = (9000 - nav_bearing) * RADX100;
|
||||
|
||||
// heading laterally, we want a zero speed here
|
||||
x_actual_speed = -sin(temp) * (float)g_gps->ground_speed;
|
||||
x_rate_error = crosstrack_error -x_actual_speed;
|
||||
x_rate_error = constrain(x_rate_error, -1400, 1400);
|
||||
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
|
||||
/*Serial.printf("max_sp %d,\tx_actual_sp %d,\tx_rate_err: %d, Xtrack %d, \tnav_lon: %d,\ty_actual_sp %d,\ty_rate_err: %d,\tnav_lat: %d,\n",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
x_rate_error,
|
||||
crosstrack_error,
|
||||
nav_lon,
|
||||
y_actual_speed,
|
||||
y_rate_error,
|
||||
nav_lat);
|
||||
//*/
|
||||
//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);
|
||||
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;
|
||||
|
||||
// heading towards target
|
||||
y_actual_speed = cos(temp) * (float)g_gps->ground_speed;
|
||||
y_rate_error = 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
|
||||
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
|
||||
//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
|
||||
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;
|
||||
|
||||
/*
|
||||
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",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat,
|
||||
x_iterm,
|
||||
y_iterm,
|
||||
crosstrack_error);
|
||||
//*/
|
||||
|
||||
|
||||
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
|
||||
|
|
@ -246,53 +277,21 @@ static void calc_nav_rate(int max_speed)
|
|||
nav_lat);*/
|
||||
}
|
||||
|
||||
|
||||
static void update_crosstrack(void)
|
||||
{
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
if (cross_track_test() < 4000) { // If we are too far off or too close we don't do track following
|
||||
if (abs(wrap_180(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
|
||||
float temp = (target_bearing - original_target_bearing) * RADX100;
|
||||
crosstrack_error = sin(temp) * (wp_distance * g.crosstrack_gain); // Meters we are off track line
|
||||
crosstrack_error = constrain(crosstrack_error, -1200, 1200);
|
||||
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
}else{
|
||||
nav_bearing = target_bearing;
|
||||
}
|
||||
}
|
||||
|
||||
// used to generate the offset angle for testing crosstrack viability
|
||||
static int32_t cross_track_test()
|
||||
{
|
||||
int32_t temp;
|
||||
temp = target_bearing - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
return abs(temp);
|
||||
}
|
||||
|
||||
|
||||
// this calculation is different than loiter above because we are in a different Frame of Reference.
|
||||
// nav_lat is pointed towards the target, where as in Loiter, nav_lat is pointed north!
|
||||
static void calc_nav_pitch_roll()
|
||||
{
|
||||
int32_t angle = wrap_360(dcm.yaw_sensor - target_bearing);
|
||||
float temp = (9000l - angle) * RADX100;
|
||||
//t: 1.5465, t1: -10.9451, t2: 1.5359, t3: 1.5465
|
||||
float _cos_yaw_x = cos(temp);
|
||||
float _sin_yaw_y = sin(temp);
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
|
||||
nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
|
||||
|
||||
// flip pitch because forward is negative
|
||||
nav_pitch = -nav_pitch;
|
||||
|
||||
/*Serial.printf("Yaw %d, Tbear:%d, \tangle: %d, \t_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%d, nav_pitch:%d\n",
|
||||
dcm.yaw_sensor,
|
||||
target_bearing,
|
||||
angle,
|
||||
_cos_yaw_x,
|
||||
_sin_yaw_y,
|
||||
nav_roll,
|
||||
nav_pitch);*/
|
||||
}
|
||||
|
||||
static int32_t get_altitude_error()
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue