mirror of https://github.com/ArduPilot/ardupilot
Added new Simple mode - Better version of FBW that is offset by initial yaw angle and not north.
Added new navigation control based on bearing_error so we can leverage Crosstrack correction. I've done limited testing on this. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1773 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
cee3e2d57e
commit
602a4fb423
|
@ -185,6 +185,7 @@ const char* flight_mode_strings[] = {
|
|||
"STABILIZE",
|
||||
"ACRO",
|
||||
"ALT_HOLD",
|
||||
"SIMPLE",
|
||||
"FBW",
|
||||
"AUTO",
|
||||
"GCS_AUTO",
|
||||
|
@ -252,6 +253,8 @@ float cos_roll_x = 1;
|
|||
float cos_pitch_x = 1;
|
||||
float cos_yaw_x = 1;
|
||||
float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
||||
float sin_nav_y, cos_nav_x; // used in calc_waypoint_nav
|
||||
long initial_simple_bearing; // used for Simple mode
|
||||
|
||||
// Airspeed
|
||||
// --------
|
||||
|
@ -526,6 +529,10 @@ void medium_loop()
|
|||
case 1:
|
||||
medium_loopCounter++;
|
||||
|
||||
// hack to stop navigation in Simple mode
|
||||
if (control_mode == SIMPLE)
|
||||
break;
|
||||
|
||||
if(g_gps->new_data){
|
||||
g_gps->new_data = false;
|
||||
dTnav = millis() - nav_loopTimer;
|
||||
|
@ -540,7 +547,13 @@ void medium_loop()
|
|||
// -----------------------------
|
||||
dTnav2 = millis() - nav2_loopTimer;
|
||||
nav2_loopTimer = millis();
|
||||
calc_nav();
|
||||
|
||||
if(wp_distance < 800){ // 8 meters
|
||||
calc_loiter_nav();
|
||||
}else{
|
||||
calc_waypoint_nav();
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
|
@ -849,12 +862,47 @@ void update_current_flight_mode(void)
|
|||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case SIMPLE:
|
||||
fbw_timer++;
|
||||
// 25 hz
|
||||
if(fbw_timer > 4){
|
||||
Location temp = current_loc;
|
||||
temp.lng += g.rc_1.control_in / 2;
|
||||
temp.lat -= g.rc_2.control_in / 2;
|
||||
|
||||
// calc a new bearing
|
||||
nav_bearing = get_bearing(¤t_loc, &temp) + initial_simple_bearing;
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
wp_distance = get_distance(¤t_loc, &temp);
|
||||
|
||||
calc_bearing_error();
|
||||
|
||||
// get nav_pitch and nav_roll
|
||||
calc_waypoint_nav();
|
||||
}
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
// Yaw control
|
||||
output_manual_yaw();
|
||||
|
||||
// apply throttle control
|
||||
output_manual_throttle();
|
||||
|
||||
// apply nav_pitch and nav_roll to output
|
||||
fbw_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case FBW:
|
||||
// we are currently using manual throttle during alpha testing.
|
||||
fbw_timer++;
|
||||
|
||||
//call at 5 hz
|
||||
if(fbw_timer > 20){
|
||||
// 10 hz
|
||||
if(fbw_timer > 10){
|
||||
fbw_timer = 0;
|
||||
|
||||
if(home_is_set == false){
|
||||
|
|
|
@ -86,12 +86,13 @@
|
|||
#define STABILIZE 0 // hold level position
|
||||
#define ACRO 1 // rate control
|
||||
#define ALT_HOLD 2 // AUTO control
|
||||
#define FBW 3 // AUTO control
|
||||
#define AUTO 4 // AUTO control
|
||||
#define GCS_AUTO 5 // AUTO control
|
||||
#define LOITER 6 // Hold a single location
|
||||
#define RTL 7 // AUTO control
|
||||
#define NUM_MODES 8
|
||||
#define SIMPLE 3 //
|
||||
#define FBW 4 // AUTO control
|
||||
#define AUTO 5 // AUTO control
|
||||
#define GCS_AUTO 6 // AUTO control
|
||||
#define LOITER 7 // Hold a single location
|
||||
#define RTL 8 // AUTO control
|
||||
#define NUM_MODES 9
|
||||
|
||||
|
||||
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
|
||||
|
|
|
@ -52,11 +52,8 @@ void navigate()
|
|||
}
|
||||
|
||||
#define DIST_ERROR_MAX 1800
|
||||
void calc_nav()
|
||||
void calc_loiter_nav()
|
||||
{
|
||||
Vector2f yawvector;
|
||||
Matrix3f temp;
|
||||
|
||||
/*
|
||||
Becuase we are using lat and lon to do our distance errors here's a quick chart:
|
||||
100 = 1m
|
||||
|
@ -83,7 +80,7 @@ void calc_nav()
|
|||
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
|
||||
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);
|
||||
|
||||
long pmax = g.pitch_max.get();
|
||||
|
@ -92,6 +89,27 @@ void calc_nav()
|
|||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
}
|
||||
|
||||
void calc_waypoint_nav()
|
||||
{
|
||||
nav_lat = constrain(wp_distance, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
|
||||
|
||||
// get the sin and cos of the bearing error - rotated 90°
|
||||
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
|
||||
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lat * cos_nav_x;
|
||||
nav_pitch = (float)nav_lat * sin_nav_y;
|
||||
|
||||
// Scale response by kP
|
||||
nav_roll *= g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
|
||||
nav_pitch *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||
|
||||
long pmax = g.pitch_max.get();
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
}
|
||||
|
||||
void calc_bearing_error()
|
||||
{
|
||||
bearing_error = nav_bearing - dcm.yaw_sensor;
|
||||
|
|
|
@ -292,6 +292,10 @@ void set_mode(byte mode)
|
|||
update_auto();
|
||||
break;
|
||||
|
||||
case SIMPLE:
|
||||
initial_simple_bearing = dcm.yaw_sensor;
|
||||
break;
|
||||
|
||||
case LOITER:
|
||||
do_loiter_at_location();
|
||||
break;
|
||||
|
|
|
@ -385,7 +385,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
|||
ts_num = 0;
|
||||
g_gps->longitude = 0;
|
||||
g_gps->latitude = 0;
|
||||
calc_nav();
|
||||
calc_loiter_nav();
|
||||
|
||||
Serial.printf_P(PSTR("ys:%ld, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "),
|
||||
dcm.yaw_sensor,
|
||||
|
@ -504,7 +504,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||
sin_roll_y,
|
||||
cos_yaw_x, // x
|
||||
sin_yaw_y); // y
|
||||
*/
|
||||
//*/
|
||||
}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
|
|
Loading…
Reference in New Issue