diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d3a894dd11..6a21499430 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -57,6 +57,9 @@ #include #endif +#include +#include + // Pre-AP_HAL compatibility #include "compat.h" @@ -216,6 +219,8 @@ AP_AHRS_HIL ahrs(&ins, g_gps); AP_AHRS_DCM ahrs(&ins, g_gps); #endif +static AP_L1_Control L1_controller(&ahrs); + #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; #endif @@ -230,6 +235,9 @@ static bool training_manual_pitch; // user has manual pitch control GCS_MAVLINK gcs0; GCS_MAVLINK gcs3; +// selected navigation controller +static AP_Navigation *nav_controller = &L1_controller; + //////////////////////////////////////////////////////////////////////////////// // Analog Inputs //////////////////////////////////////////////////////////////////////////////// @@ -352,21 +360,10 @@ static bool have_position; // Constants const float radius_of_earth = 6378100; // meters -// This is the currently calculated direction to fly. -// deg * 100 : 0 to 360 -static int32_t nav_bearing_cd; - -// This is the direction to the next waypoint or loiter center -// deg * 100 : 0 to 360 -static int32_t target_bearing_cd; - -//This is the direction from the last waypoint to the next waypoint -// deg * 100 : 0 to 360 -static int32_t crosstrack_bearing_cd; - // Direction held during phases of takeoff and landing // deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use -static int32_t hold_course = -1; // deg * 100 dir of plane +// this is a 0..36000 value, or -1 for disabled +static int32_t hold_course_cd = -1; // deg * 100 dir of plane // There may be two active commands in Auto mode. // This indicates the active navigation command by index number @@ -412,18 +409,9 @@ static uint8_t receiver_rssi; // The amount current ground speed is below min ground speed. Centimeters per second static int32_t groundspeed_undershoot = 0; -//////////////////////////////////////////////////////////////////////////////// -// Location Errors -//////////////////////////////////////////////////////////////////////////////// -// Difference between current bearing and desired bearing. Hundredths of a degree -static int32_t bearing_error_cd; - // Difference between current altitude and desired altitude. Centimeters static int32_t altitude_error_cm; -// Distance perpandicular to the course line that we are off trackline. Meters -static float crosstrack_error; - //////////////////////////////////////////////////////////////////////////////// // Battery Sensors //////////////////////////////////////////////////////////////////////////////// @@ -467,21 +455,10 @@ static bool throttle_suppressed; //////////////////////////////////////////////////////////////////////////////// // Loiter management //////////////////////////////////////////////////////////////////////////////// -// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree -static int32_t old_target_bearing_cd; - -// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees -static int32_t loiter_total; // Direction for loiter. 1 for clockwise, -1 for counter-clockwise static int8_t loiter_direction = 1; -// The amount in degrees we have turned since recording old_target_bearing -static int16_t loiter_delta; - -// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees -static int32_t loiter_sum; - // The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds. static uint32_t loiter_time_ms; @@ -507,6 +484,18 @@ uint32_t wp_distance; // Distance between previous and next waypoint. Meters static uint32_t wp_totalDistance; +/* + meta data to support counting the number of circles in a loiter + */ +static struct { + int32_t old_target_bearing_cd; + int32_t loiter_sum_cd; + + // Total desired rotation in a loiter. Used for Loiter Turns commands. + int32_t loiter_total_cd; +} loiter; + + // event control state enum event_type { EVENT_TYPE_RELAY=0, @@ -758,9 +747,6 @@ static void fast_loop() ahrs.update(); - // uses the yaw from the DCM to give more accurate turns - calc_bearing_error(); - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); @@ -1030,7 +1016,7 @@ static void update_current_flight_mode(void) switch(nav_command_ID) { case MAV_CMD_NAV_TAKEOFF: - if (hold_course != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) { + if (hold_course_cd != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) { calc_nav_roll(); } else { nav_roll_cd = 0; @@ -1092,7 +1078,7 @@ static void update_current_flight_mode(void) default: // we are doing normal AUTO flight, the special cases // are for takeoff and landing - hold_course = -1; + hold_course_cd = -1; land_complete = false; calc_nav_roll(); calc_nav_pitch(); @@ -1101,7 +1087,7 @@ static void update_current_flight_mode(void) } }else{ // hold_course is only used in takeoff and landing - hold_course = -1; + hold_course_cd = -1; switch(control_mode) { case RTL: @@ -1246,7 +1232,6 @@ static void update_navigation() case RTL: case GUIDED: update_loiter(); - calc_bearing_error(); break; case MANUAL: diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 1e243d170f..de289d1d5b 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -325,8 +325,9 @@ static void calc_throttle() // ---------------------------------------------------------------------- static void calc_nav_yaw(float speed_scaler, float ch4_inf) { - if (hold_course != -1) { + if (hold_course_cd != -1) { // steering on or close to ground + int32_t bearing_error_cd = nav_controller->bearing_error_cd(); g.channel_rudder.servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) + g.kff_rudder_mix * g.channel_roll.servo_out; g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500); @@ -364,35 +365,7 @@ static void calc_nav_pitch() static void calc_nav_roll() { -#define NAV_ROLL_BY_RATE 0 -#if NAV_ROLL_BY_RATE - // Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a - // desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try - // to turn at 15 degrees per second. - float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01); - - // Use airspeed_cruise as an analogue for airspeed if we don't have airspeed. - float speed; - if (!ahrs.airspeed_estimate(&speed)) { - speed = g.airspeed_cruise_cm*0.01; - - // Floor the speed so that the user can't enter a bad value - if(speed < 6) { - speed = 6; - } - } - - // Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity. - nav_roll = ToDeg(atanf(speed*turn_rate/GRAVITY_MSS)*100); - -#else - // this is the old nav_roll calculation. We will use this for 2.50 - // then remove for a future release - float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed; - nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); - nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100 -#endif - + nav_roll_cd = nav_controller->nav_roll_cd(); nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 870e27766d..97074db223 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -271,17 +271,16 @@ static void NOINLINE send_location(mavlink_channel_t chan) static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) { - int16_t bearing = (hold_course==-1 ? nav_bearing_cd : hold_course) / 100; mavlink_msg_nav_controller_output_send( chan, nav_roll_cd * 0.01, nav_pitch_cd * 0.01, - bearing, - target_bearing_cd * 0.01, + nav_controller->nav_bearing_cd() * 0.01f, + nav_controller->target_bearing_cd() * 0.01f, wp_distance, altitude_error_cm * 0.01, airspeed_error_cm, - crosstrack_error); + nav_controller->crosstrack_error()); } static void NOINLINE send_gps_raw(mavlink_channel_t chan) diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 0d7a57e689..2e2a580a3f 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -489,8 +489,8 @@ static void Log_Write_Nav_Tuning() LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG), yaw : (uint16_t)ahrs.yaw_sensor, wp_distance : wp_distance, - target_bearing_cd : (uint16_t)target_bearing_cd, - nav_bearing_cd : (uint16_t)nav_bearing_cd, + target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), + nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), altitude_error_cm : (int16_t)altitude_error_cm, airspeed_cm : (int16_t)airspeed.get_airspeed_cm() }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f8e472540b..9e9f3d48f9 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -73,7 +73,7 @@ public: k_param_reset_mission_chan, k_param_land_flare_alt, k_param_land_flare_sec, - k_param_crosstrack_min_distance, + k_param_crosstrack_min_distance, // unused k_param_rudder_steer, k_param_throttle_nudge, k_param_alt_offset, @@ -83,6 +83,7 @@ public: k_param_takeoff_heading_hold, k_param_hil_servos, k_param_vtail_output, + k_param_nav_controller, // 110: Telemetry control // @@ -125,8 +126,8 @@ public: // // 150: Navigation parameters // - k_param_crosstrack_gain = 150, - k_param_crosstrack_entry_angle, + k_param_crosstrack_gain = 150, // unused + k_param_crosstrack_entry_angle, // unused k_param_roll_limit_cd, k_param_pitch_limit_max_cd, k_param_pitch_limit_min_cd, @@ -134,7 +135,7 @@ public: k_param_RTL_altitude_cm, k_param_inverted_flight_ch, k_param_min_gndspeed_cm, - k_param_crosstrack_use_wind, + k_param_crosstrack_use_wind, // unused // @@ -219,6 +220,7 @@ public: k_param_rollController, k_param_pitchController, k_param_yawController, + k_param_L1_controller, // // 240: PID Controllers @@ -255,12 +257,8 @@ public: // speed used for speed scaling AP_Float scaling_speed; - // Crosstrack navigation - // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; - AP_Int8 crosstrack_use_wind; - AP_Int16 crosstrack_min_distance; + // navigation controller type. See AP_Navigation::ControllerType + AP_Int8 nav_controller; // Estimation // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 0493fb4465..705ec3fd20 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -143,38 +143,12 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(land_flare_sec, "LAND_FLARE_SEC", 2.0), - // @Param: XTRK_GAIN_SC - // @DisplayName: Crosstrack Gain - // @Description: The scale between distance off the line and angle to meet the line (in Degrees * 100) - // @Range: 0 2000 - // @Increment: 1 - // @User: Standard - GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", XTRACK_GAIN_SCALED), - - // @Param: XTRK_ANGLE_CD - // @DisplayName: Crosstrack Entry Angle - // @Description: Maximum angle used to correct for track following. - // @Units: centi-Degrees - // @Range: 0 9000 - // @Increment: 1 - // @User: Standard - GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE), - - // @Param: XTRK_USE_WIND - // @DisplayName: Crosstrack Wind correction - // @Description: If enabled, use wind estimation for navigation crosstrack when using a compass for yaw - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND", 1), - - // @Param: XTRK_MIN_DIST - // @DisplayName: Crosstrack mininum distance - // @Description: Minimum distance in meters between waypoints to do crosstrack correction. - // @Units: Meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", 50), + // @Param: NAV_CONTROLLER + // @DisplayName: Navigation controller selection + // @Description: Which navigation controller to enable + // @Values: 0:Legacy,1:L1Controller + // @User: Standard + GSCALAR(nav_controller, "NAV_CONTROLLER", AP_Navigation::CONTROLLER_L1), // @Param: ALT_MIX // @DisplayName: Gps to Baro Mix @@ -208,7 +182,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @DisplayName: Waypoint Radius // @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit. // @Units: Meters - // @Range: 1 127 + // @Range: 1 32767 // @Increment: 1 // @User: Standard GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT), @@ -736,6 +710,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp GOBJECT(airspeed, "ARSPD_", AP_Airspeed), + // @Group: NAVL1_ + // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp + GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), + #if MOUNT == ENABLED // @Group: MNT_ // @Path: ../libraries/AP_Mount/AP_Mount.cpp diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 9fc499b482..00f4efa781 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -193,23 +193,13 @@ static void set_next_WP(const struct Location *wp) } // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; + loiter_angle_reset(); // this is handy for the groundstation wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; - target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP); - nav_bearing_cd = target_bearing_cd; - // to check if we have missed the WP - // ---------------------------- - old_target_bearing_cd = target_bearing_cd; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); + loiter_angle_reset(); } static void set_guided_WP(void) @@ -230,15 +220,8 @@ static void set_guided_WP(void) // this is handy for the groundstation wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; - target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP); - // to check if we have missed the WP - // ---------------------------- - old_target_bearing_cd = target_bearing_cd; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); + loiter_angle_reset(); } // run this at setup on the ground diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2bc7e8ccc6..2a554ce4f2 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -281,7 +281,7 @@ static void do_loiter_unlimited() static void do_loiter_turns() { set_next_WP(&next_nav_command); - loiter_total = next_nav_command.p1 * 360; + loiter.loiter_total_cd = next_nav_command.p1 * 36000UL; loiter_set_direction_wp(&next_nav_command); } @@ -299,24 +299,24 @@ static void do_loiter_time() static bool verify_takeoff() { if (ahrs.yaw_initialised()) { - if (hold_course == -1 && g.takeoff_heading_hold != 0) { + if (hold_course_cd == -1 && g.takeoff_heading_hold != 0) { // save our current course to take off - hold_course = ahrs.yaw_sensor; - gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course); + hold_course_cd = ahrs.yaw_sensor; + gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd); } } - if (hold_course != -1) { - // recalc bearing error with hold_course; - nav_bearing_cd = hold_course; - // recalc bearing error - calc_bearing_error(); + if (hold_course_cd != -1) { + // call navigation controller for heading hold + nav_controller->update_heading_hold(hold_course_cd); + } else { + nav_controller->update_level_flight(); } if (adjusted_altitude_cm() > takeoff_altitude) { - hold_course = -1; + hold_course_cd = -1; takeoff_complete = true; - next_WP = current_loc; + next_WP = prev_WP = current_loc; return true; } else { return false; @@ -337,18 +337,18 @@ static bool verify_land() land_complete = true; - if (hold_course == -1) { + if (hold_course_cd == -1) { // we have just reached the threshold of to flare for landing. // We now don't want to do any radical // turns, as rolling could put the wings into the runway. - // To prevent further turns we set hold_course to the + // To prevent further turns we set hold_course_cd to the // current heading. Previously we set this to // crosstrack_bearing, but the xtrack bearing can easily // be quite large at this point, and that could induce a // sudden large roll correction which is very nasty at // this point in the landing. - hold_course = ahrs.yaw_sensor; - gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course); + hold_course_cd = ahrs.yaw_sensor; + gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course_cd); } if (g_gps->ground_speed*0.01 < 3.0) { @@ -362,30 +362,30 @@ static bool verify_land() } } - if (hold_course != -1) { + if (hold_course_cd != -1) { // recalc bearing error with hold_course; - nav_bearing_cd = hold_course; - // recalc bearing error - calc_bearing_error(); + nav_controller->update_heading_hold(hold_course_cd); } else { - update_crosstrack(); + nav_controller->update_waypoint(prev_WP, next_WP); } return false; } static bool verify_nav_wp() { - hold_course = -1; - update_crosstrack(); - if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) { + hold_course_cd = -1; + + nav_controller->update_waypoint(prev_WP, next_WP); + + if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) { gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"), (unsigned)nav_command_index, (unsigned)get_distance(¤t_loc, &next_WP)); return true; - } + } // have we circled around the waypoint? - if (loiter_sum > 300) { + if (loiter.loiter_sum_cd > 30000) { gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); return true; } @@ -404,14 +404,12 @@ static bool verify_nav_wp() static bool verify_loiter_unlim() { update_loiter(); - calc_bearing_error(); return false; } static bool verify_loiter_time() { update_loiter(); - calc_bearing_error(); if ((millis() - loiter_time_ms) > loiter_time_max_ms) { gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); return true; @@ -422,9 +420,8 @@ static bool verify_loiter_time() static bool verify_loiter_turns() { update_loiter(); - calc_bearing_error(); - if(loiter_sum > loiter_total) { - loiter_total = 0; + if (loiter.loiter_sum_cd > loiter.loiter_total_cd) { + loiter.loiter_total_cd = 0; gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); // clear the command queue; return true; @@ -434,12 +431,14 @@ static bool verify_loiter_turns() static bool verify_RTL() { - if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) { - gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); - return true; - }else{ + update_loiter(); + if (wp_distance <= (uint32_t)max(g.waypoint_radius,0) || + nav_controller->reached_loiter_target()) { + gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); + return true; + } else { return false; - } + } } /********************************************************************************/ diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 75d96e219d..a55223c4b9 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -834,6 +834,6 @@ #endif #ifndef SERIAL_BUFSIZE -# define SERIAL_BUFSIZE 256 + # define SERIAL_BUFSIZE 256 #endif diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 7754b41b60..b0dbd49b9a 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -1,17 +1,59 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// set the nav_controller pointer to the right controller +static void set_nav_controller(void) +{ + switch ((AP_Navigation::ControllerType)g.nav_controller.get()) { + case AP_Navigation::CONTROLLER_L1: + nav_controller = &L1_controller; + break; + } +} + +/* + reset the total loiter angle + */ +static void loiter_angle_reset(void) +{ + loiter.loiter_sum_cd = 0; + loiter.loiter_total_cd = 0; +} + +/* + update the total angle we have covered in a loiter. Used to support + commands to do N circles of loiter + */ +static void loiter_angle_update(void) +{ + int32_t target_bearing_cd = nav_controller->target_bearing_cd(); + int32_t loiter_delta_cd; + if (loiter.loiter_sum_cd == 0) { + loiter_delta_cd = 0; + } else { + loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd; + } + loiter.old_target_bearing_cd = target_bearing_cd; + loiter_delta_cd = wrap_180_cd(loiter_delta_cd); + + loiter.loiter_sum_cd += loiter_delta_cd; +} + //**************************************************************** // Function that will calculate the desired direction to fly and distance //**************************************************************** static void navigate() { + // allow change of nav controller mid-flight + set_nav_controller(); + // do not navigate with corrupt data // --------------------------------- if (!have_position) { return; } - if(next_WP.lat == 0) { + if (next_WP.lat == 0) { return; } @@ -24,24 +66,8 @@ static void navigate() return; } - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP); - - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing_cd = target_bearing_cd; - - // check if we have missed the WP - loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100; - - // reset the old value - old_target_bearing_cd = target_bearing_cd; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); + // update total loiter angle + loiter_angle_update(); // control mode specific updates to nav_bearing // -------------------------------------------- @@ -49,16 +75,6 @@ static void navigate() } -#if 0 -// Disabled for now -void calc_distance_error() -{ - distance_estimate += (float)g_gps->ground_speed * .0002f * cosf(radians(bearing_error_cd * .01f)); - distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); - wp_distance = max(distance_estimate,10); -} -#endif - static void calc_airspeed_errors() { float aspeed_cm = airspeed.get_airspeed_cm(); @@ -105,12 +121,6 @@ static void calc_gndspeed_undershoot() } } -static void calc_bearing_error() -{ - bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor; - bearing_error_cd = wrap_180_cd(bearing_error_cd); -} - static void calc_altitude_error() { if (control_mode == AUTO && offset_altitude_cm != 0) { @@ -132,63 +142,6 @@ static void calc_altitude_error() static void update_loiter() { - float power; - - if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) { - power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); - nav_bearing_cd += 9000.0 * (2.0 + power) * loiter_direction; - } else if(wp_distance < (uint32_t)max((g.loiter_radius + LOITER_RANGE),0)) { - power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing_cd -= power * 9000 * loiter_direction; - } else{ - update_crosstrack(); - loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit - - } -/* - * if (wp_distance < g.loiter_radius){ - * nav_bearing += 9000; - * }else{ - * nav_bearing -= 100 * M_PI / 180 * asinf(g.loiter_radius / wp_distance); - * } - * - * update_crosstrack(); - */ - nav_bearing_cd = wrap_360_cd(nav_bearing_cd); -} - -static void update_crosstrack(void) -{ - // if we are using a compass for navigation, then adjust the - // heading to account for wind - if (g.crosstrack_use_wind && compass.use_for_yaw()) { - Vector3f wind = ahrs.wind_estimate(); - Vector2f wind2d = Vector2f(wind.x, wind.y); - float speed; - if (ahrs.airspeed_estimate(&speed)) { - Vector2f nav_vector = Vector2f(cosf(radians(nav_bearing_cd*0.01)), sinf(radians(nav_bearing_cd*0.01))) * speed; - Vector2f nav_adjusted = nav_vector - wind2d; - nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100; - } - } - - // Crosstrack Error - // ---------------- - // If we are too far off or too close we don't do track following - if (wp_totalDistance >= (uint32_t)max(g.crosstrack_min_distance,0) && - abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { - // Meters we are off track line - crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; - nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing_cd = wrap_360_cd(nav_bearing_cd); - } - -} - -static void reset_crosstrack() -{ - crosstrack_bearing_cd = get_bearing_cd(&prev_WP, &next_WP); // Used for track following + nav_controller->update_loiter(next_WP, g.loiter_radius, loiter_direction); } diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index f9c0bc5102..eca2938ff5 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -110,7 +110,6 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_radio(); report_batt_monitor(); report_gains(); - report_xtrack(); report_throttle(); report_flight_modes(); report_ins(); @@ -544,19 +543,6 @@ static void report_gains() print_blanks(2); } -static void report_xtrack() -{ - //print_blanks(2); - cliSerial->printf_P(PSTR("Crosstrack\n")); - print_divider(); - // radio - cliSerial->printf_P(PSTR("XTRACK: %4.2f\n" - "XTRACK angle: %d\n"), - (float)g.crosstrack_gain, - (int)g.crosstrack_entry_angle); - print_blanks(2); -} - static void report_throttle() { //print_blanks(2); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 5760ce5abe..b1824ad569 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -259,6 +259,9 @@ static void init_ardupilot() Log_Write_Startup(TYPE_GROUNDSTART_MSG); } + // choose the nav controller + set_nav_controller(); + set_mode(MANUAL); // set the correct flight mode @@ -352,10 +355,12 @@ static void set_mode(enum FlightMode mode) break; case AUTO: + prev_WP = current_loc; update_auto(); break; case RTL: + prev_WP = current_loc; do_RTL(); break; @@ -368,6 +373,7 @@ static void set_mode(enum FlightMode mode) break; default: + prev_WP = current_loc; do_RTL(); break; }