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:
jasonshort 2011-03-14 06:04:07 +00:00
parent cee3e2d57e
commit 602a4fb423
5 changed files with 87 additions and 16 deletions

View File

@ -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(&current_loc, &temp) + initial_simple_bearing;
nav_bearing = wrap_360(nav_bearing);
wp_distance = get_distance(&current_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){

View File

@ -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

View File

@ -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
@ -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;

View File

@ -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;

View File

@ -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){