diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index e3ce6d8e66..5a827ed45f 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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){ diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index de4fb29c71..2187a77bc1 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 396da343f0..e01d80d2b8 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -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; diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 8e20c0ea2b..7faf1c3db5 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 9a2101ad39..d0158a07fb 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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){