diff --git a/APMrover2/APM_Config_HILmode.h b/APMrover2/APM_Config_HILmode.h index a4d90e8af6..d24b022c5a 100644 --- a/APMrover2/APM_Config_HILmode.h +++ b/APMrover2/APM_Config_HILmode.h @@ -404,25 +404,7 @@ // mainly intended to allow users to start using the APM without running the // WaypointWriter first. // -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -// USE_CURRENT_ALT OPTIONAL -// ALT_HOLD_HOME OPTIONAL -// -// When the user performs a factory reset on the APM, set the flag for weather -// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch. -// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// #define WP_RADIUS_DEFAULT 1 // meters -#define LOITER_RADIUS_DEFAULT 5 -#define USE_CURRENT_ALT TRUE -#define ALT_HOLD_HOME 0 ////////////////////////////////////////////////////////////////////////////// // INPUT_VOLTAGE OPTIONAL diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h index 413c8457e9..4af365a1eb 100644 --- a/APMrover2/APM_Config_Rover.h +++ b/APMrover2/APM_Config_Rover.h @@ -412,25 +412,7 @@ This feature works only if the ROV_AWPR_NAV is set to 0 // mainly intended to allow users to start using the APM without running the // WaypointWriter first. // -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -// USE_CURRENT_ALT OPTIONAL -// ALT_HOLD_HOME OPTIONAL -// -// When the user performs a factory reset on the APM, set the flag for weather -// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch. -// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// #define WP_RADIUS_DEFAULT 1 // meters -#define LOITER_RADIUS_DEFAULT 5 // 60 -#define USE_CURRENT_ALT TRUE -#define ALT_HOLD_HOME 0 ////////////////////////////////////////////////////////////////////////////// // INPUT_VOLTAGE OPTIONAL diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 8c232d3255..85c5fdec24 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -318,18 +318,18 @@ static const char *comma = ","; static const char* flight_mode_strings[] = { "Manual", - "Circle", + "", "Learning", "", "", - "FBW_A", - "FBW_B", + "", + "", "", "", "", "Auto", "RTL", - "Loiter", + "", "", "", "", @@ -374,7 +374,7 @@ static bool rc_override_active = false; //////////////////////////////////////////////////////////////////////////////// // A tracking variable for type of failsafe active // Used for failsafe based on loss of RC signal or GCS signal -static int failsafe; +static int16_t failsafe; // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold // RC receiver should be set up to output a low throttle value when signal is lost static bool ch3_failsafe; @@ -407,7 +407,7 @@ static byte ground_start_count = 5; // Used to compute a speed estimate from the first valid gps fixes to decide if we are // on the ground or in the air. Used to decide if a ground start is appropriate if we // booted with an air start. -static int ground_start_avg; +static int16_t ground_start_avg; static int32_t gps_base_alt; //////////////////////////////////////////////////////////////////////////////// @@ -418,18 +418,15 @@ const float radius_of_earth = 6378100; // meters const float gravity = 9.81; // meters/ sec^2 // This is the currently calculated direction to fly. // deg * 100 : 0 to 360 -static long nav_bearing; -// This is the direction to the next waypoint or loiter center +static int32_t nav_bearing; +// This is the direction to the next waypoint // deg * 100 : 0 to 360 -static long target_bearing; +static int32_t target_bearing; //This is the direction from the last waypoint to the next waypoint // deg * 100 : 0 to 360 -static long crosstrack_bearing; +static int32_t crosstrack_bearing; // A gain scaler to account for ground speed/headwind/tailwind static float nav_gain_scaler = 1; -// 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 long hold_course = -1; // deg * 100 dir of plane static bool rtl_complete = false; // There may be two active commands in Auto mode. @@ -443,7 +440,7 @@ static byte non_nav_command_ID = NO_COMMAND; static float groundspeed_error; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel -static int throttle_nudge = 0; +static int16_t throttle_nudge = 0; // The distance as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_dist; static bool obstacle = false; @@ -452,17 +449,17 @@ static bool obstacle = false; // Ground speed //////////////////////////////////////////////////////////////////////////////// // The amount current ground speed is below min ground speed. Centimeters per second -static long groundspeed_undershoot = 0; -static long ground_speed = 0; -static int throttle_last = 0, throttle = 500; +static int32_t groundspeed_undershoot = 0; +static int32_t ground_speed = 0; +static int16_t throttle_last = 0, throttle = 500; //////////////////////////////////////////////////////////////////////////////// // Location Errors //////////////////////////////////////////////////////////////////////////////// // Difference between current bearing and desired bearing. Hundredths of a degree -static long bearing_error; +static int32_t bearing_error; // Difference between current altitude and desired altitude. Centimeters -static long altitude_error; +static int32_t altitude_error; // Distance perpandicular to the course line that we are off trackline. Meters static float crosstrack_error; @@ -495,42 +492,26 @@ static float current_total1; //static float current_total2; // Totalized current (Amp-hours) from battery 2 // JLN Update -unsigned long timesw = 0; +uint32_t timesw = 0; static bool speed_boost = false; -//////////////////////////////////////////////////////////////////////////////// -// Loiter management -//////////////////////////////////////////////////////////////////////////////// -// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree -static long old_target_bearing; -// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees -static int loiter_total; -// The amount in degrees we have turned since recording old_target_bearing -static int loiter_delta; -// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees -static int loiter_sum; -// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds. -static long loiter_time; -// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. -static int loiter_time_max; - //////////////////////////////////////////////////////////////////////////////// // Navigation control variables //////////////////////////////////////////////////////////////////////////////// // The instantaneous desired bank angle. Hundredths of a degree -static long nav_roll; +static int32_t nav_roll; // The instantaneous desired pitch angle. Hundredths of a degree -static long nav_pitch; +static int32_t nav_pitch; // Calculated radius for the wp turn based on ground speed and max turn angle -static long wp_radius; +static int32_t wp_radius; //////////////////////////////////////////////////////////////////////////////// // Waypoint distances //////////////////////////////////////////////////////////////////////////////// // Distance between plane and next waypoint. Meters -static long wp_distance; +static int32_t wp_distance; // Distance between previous and next waypoint. Meters -static long wp_totalDistance; +static int32_t wp_totalDistance; //////////////////////////////////////////////////////////////////////////////// // repeating event control @@ -538,27 +519,27 @@ static long wp_totalDistance; // Flag indicating current event type static byte event_id; // when the event was started in ms -static long event_timer; +static int32_t event_timer; // how long to delay the next firing of event in millis static uint16_t event_delay; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int event_repeat = 0; +static int16_t event_repeat = 0; // per command value, such as PWM for servos -static int event_value; +static int16_t event_value; // the value used to cycle events (alternate value to event_value) -static int event_undo_value; +static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Conditional command //////////////////////////////////////////////////////////////////////////////// // A value used in condition commands (eg delay, change alt, etc.) // For example in a change altitude command, it is the altitude to change to. -static long condition_value; +static int32_t condition_value; // A starting value used to check the status of a conditional command. // For example in a delay command the condition_start records that start time for the delay -static long condition_start; +static int32_t condition_start; // A value used in condition commands. For example the rate at which to change altitude. -static int condition_rate; +static int16_t condition_rate; //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors @@ -572,7 +553,7 @@ static bool home_is_set; static struct Location prev_WP; // The plane's current location static struct Location current_loc; -// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. +// The location of the current/active waypoint. Used for track following static struct Location next_WP; // The location of the active waypoint in Guided mode. static struct Location guided_WP; @@ -593,30 +574,30 @@ static float G_Dt = 0.02; // Performance monitoring //////////////////////////////////////////////////////////////////////////////// // Timer used to accrue data and trigger recording of the performanc monitoring log message -static long perf_mon_timer; +static int32_t perf_mon_timer; // The maximum main loop execution time recorded in the current performance monitoring interval -static int G_Dt_max = 0; +static int16_t G_Dt_max = 0; // The number of gps fixes recorded in the current performance monitoring interval -static int gps_fix_count = 0; +static int16_t gps_fix_count = 0; // A variable used by developers to track performanc metrics. // Currently used to record the number of GCS heartbeat messages received -static int pmTest1 = 0; +static int16_t pmTest1 = 0; //////////////////////////////////////////////////////////////////////////////// // System Timers //////////////////////////////////////////////////////////////////////////////// // Time in miliseconds of start of main control loop. Milliseconds -static unsigned long fast_loopTimer; +static uint32_t fast_loopTimer; // Time Stamp when fast loop was complete. Milliseconds -static unsigned long fast_loopTimeStamp; +static uint32_t fast_loopTimeStamp; // Number of milliseconds used in last main loop cycle static uint8_t delta_ms_fast_loop; // Counter of main loop executions. Used for performance monitoring and failsafe processing static uint16_t mainLoop_count; // Time in miliseconds of start of medium control loop. Milliseconds -static unsigned long medium_loopTimer; +static uint32_t medium_loopTimer; // Counters for branching from main control loop to slower loops static byte medium_loopCounter; // Number of milliseconds used in last medium loop cycle @@ -977,43 +958,21 @@ static void update_GPS(void) static void update_current_flight_mode(void) { - if(control_mode == AUTO) { - switch (nav_command_ID) { - hold_course = -1; - calc_nav_roll(); - calc_throttle(); - break; - } - }else{ - switch(control_mode){ - case RTL: - hold_course = -1; - calc_nav_roll(); - calc_throttle(); - break; + switch(control_mode){ + case AUTO: + case RTL: + calc_nav_roll(); + calc_throttle(); + break; - case LEARNING: - nav_roll = 0; - nav_pitch = 0; - #if X_PLANE == ENABLED - // servo_out is for Sim control only - // --------------------------------- - g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle(); - #endif - // throttle is passthrough - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle(); - break; - - } + case LEARNING: + case MANUAL: + nav_roll = 0; + nav_pitch = 0; + g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle(); + break; } } @@ -1028,10 +987,8 @@ static void update_navigation() }else{ switch(control_mode){ - case LOITER: case RTL: // no loitering around the wp with the rover, goes direct to the wp position case GUIDED: - // update_loiter(); calc_nav_roll(); calc_bearing_error(); if(verify_RTL()) { diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 6b8863b2cf..4aee49021c 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -46,7 +46,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) break; case AUTO: case RTL: - case LOITER: case GUIDED: case CIRCLE: base_mode = MAV_MODE_FLAG_GUIDED_ENABLED; @@ -215,7 +214,7 @@ 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:hold_course) / 100; + int16_t bearing = nav_bearing / 100; mavlink_msg_nav_controller_output_send( chan, nav_roll / 1.0e2, @@ -1106,7 +1105,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; - // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) if (packet.seq == (uint16_t)g.command_index) @@ -1129,16 +1127,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: param1 = tell_command.p1; break; - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1*10; // APM loiter time is in ten second increments - break; - case MAV_CMD_CONDITION_CHANGE_ALT: x=0; // Clear fields loaded above that we don't want sent for this command y=0; @@ -1348,11 +1341,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_RETURN_TO_LAUNCH: break; - case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: tell_command.p1 = packet.param1; @@ -1362,10 +1353,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command.lat = packet.param1; break; - case MAV_CMD_NAV_LOITER_TIME: - tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments - break; - case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DISTANCE: tell_command.lat = packet.param1; @@ -1807,8 +1794,8 @@ GCS_MAVLINK::queued_waypoint_send() */ static void mavlink_delay(unsigned long t) { - unsigned long tstart; - static unsigned long last_1hz, last_50hz; + uint32_t tstart; + static uint32_t last_1hz, last_50hz; if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by @@ -1821,7 +1808,7 @@ static void mavlink_delay(unsigned long t) tstart = millis(); do { - unsigned long tnow = millis(); + uint32_t tnow = millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_message(MSG_HEARTBEAT); diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 7d2589c072..29950bd8f2 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -161,7 +161,7 @@ public: k_param_command_total, k_param_command_index, k_param_waypoint_radius, - k_param_loiter_radius, + k_param_loiter_radius, // unused k_param_fence_action, k_param_fence_total, k_param_fence_channel, @@ -262,7 +262,6 @@ public: AP_Int8 command_total; AP_Int8 command_index; AP_Int8 waypoint_radius; - AP_Int8 loiter_radius; // Fly-by-wire // diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 2fb1a49dd5..ba11a99a2b 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -32,7 +32,6 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(command_total, "CMD_TOTAL", 0), GSCALAR(command_index, "CMD_INDEX", 0), GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT), - GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT), GSCALAR(throttle_min, "THR_MIN", THROTTLE_MIN), GSCALAR(throttle_max, "THR_MAX", THROTTLE_MAX), diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index c20e01f104..28a4ea6c4d 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -120,21 +120,12 @@ static void set_next_WP(struct Location *wp) prev_WP = current_loc; } - // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; - // this is handy for the groundstation wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; target_bearing = get_bearing_cd(¤t_loc, &next_WP); nav_bearing = target_bearing; - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); @@ -155,10 +146,6 @@ static void set_guided_WP(void) wp_distance = wp_totalDistance; target_bearing = get_bearing_cd(¤t_loc, &next_WP); - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index b8db2df034..53925a6aa9 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -21,18 +21,6 @@ handle_process_nav_cmd() do_nav_wp(); break; - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - do_loiter_unlimited(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times - do_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: - do_loiter_time(); - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; @@ -139,32 +127,16 @@ static bool verify_nav_command() // Returns true if command complete case MAV_CMD_NAV_TAKEOFF: return verify_takeoff(); - break; case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: - return verify_loiter_unlim(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: - return verify_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: - return verify_loiter_time(); - break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: return verify_RTL(); - break; default: gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); return false; - break; } } @@ -220,24 +192,6 @@ static void do_nav_wp() set_next_WP(&next_nav_command); } -static void do_loiter_unlimited() -{ - set_next_WP(&next_nav_command); -} - -static void do_loiter_turns() -{ - set_next_WP(&next_nav_command); - loiter_total = next_nav_command.p1 * 360; -} - -static void do_loiter_time() -{ - set_next_WP(&next_nav_command); - loiter_time = millis(); - loiter_time_max = next_nav_command.p1; // units are (seconds * 10) -} - /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ @@ -254,7 +208,6 @@ static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_ static bool verify_nav_wp() { - hold_course = -1; update_crosstrack(); if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { @@ -273,13 +226,7 @@ static bool verify_nav_wp() } } - // have we circled around the waypoint? - if (loiter_sum > 300) { - gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); - return true; - } - - // have we flown past the waypoint? + // have we gone past the waypoint? if (location_passed_point(current_loc, prev_WP, next_WP)) { gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"), (unsigned)nav_command_index, @@ -290,37 +237,6 @@ static bool verify_nav_wp() return false; } -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) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); - return true; - } - return false; -} - -static bool verify_loiter_turns() -{ - update_loiter(); - calc_bearing_error(); - if(loiter_sum > loiter_total) { - loiter_total = 0; - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); - // clear the command queue; - return true; - } - return false; -} - static bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { @@ -361,7 +277,7 @@ static void do_within_distance() static bool verify_wait_delay() { - if ((unsigned)(millis() - condition_start) > condition_value){ + if ((uint32_t)(millis() - condition_start) > (uint32_t)condition_value){ condition_value = 0; return true; } @@ -390,11 +306,6 @@ static bool verify_within_distance() // Do (Now) commands /********************************************************************************/ -static void do_loiter_at_location() -{ - next_WP = current_loc; -} - static void do_jump() { struct Location temp; diff --git a/APMrover2/config.h b/APMrover2/config.h index fccb9ff771..2ea6b30945 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -649,19 +649,6 @@ # define WP_RADIUS_DEFAULT 30 #endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 60 -#endif - -#ifndef ALT_HOLD_HOME -# define ALT_HOLD_HOME 100 -#endif -#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100 - -#ifndef USE_CURRENT_ALT -# define USE_CURRENT_ALT FALSE -#endif - ////////////////////////////////////////////////////////////////////////////// // Developer Items // diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 8f167145c4..4cf121259c 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -12,7 +12,6 @@ #define ToDeg(x) (x*57.2957795131) // *180/pi #define DEBUG 0 -#define LOITER_RANGE 60 // for calculating power outside of loiter radius #define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. // failsafe @@ -98,7 +97,6 @@ #define AUTO 10 #define RTL 11 -#define LOITER 12 #define GUIDED 15 #define INITIALISING 16 // in startup routines #define HEADALT 17 // Lock the current heading and altitude diff --git a/APMrover2/events.pde b/APMrover2/events.pde index d5c9fffcb6..707c6cc8f9 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -15,7 +15,6 @@ static void failsafe_short_on_event(int fstype) case AUTO: case GUIDED: - case LOITER: if(g.short_fs_action == 1) { set_mode(RTL); } @@ -45,7 +44,6 @@ static void failsafe_long_on_event(int fstype) case AUTO: case GUIDED: - case LOITER: if(g.long_fs_action == 1) { set_mode(RTL); } diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index ff3bf0029c..c6a4bd3eb1 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -13,23 +13,16 @@ static void navigate() return; } -#if HIL_MODE != HIL_MODE_ATTITUDE - if((next_WP.lat == 0)||(home_is_set==false)){ -#else - if(next_WP.lat == 0){ -#endif + if ((next_WP.lat == 0)||(home_is_set==false)){ return; } - if(control_mode < INITIALISING) { - // waypoint distance from plane // ---------------------------- wp_distance = get_distance(¤t_loc, &next_WP); if (wp_distance < 0){ gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //cliSerial->println(wp_distance,DEC); return; } @@ -41,34 +34,12 @@ static void navigate() // ------------------------------------------ nav_bearing = target_bearing; - // check if we have missed the WP - loiter_delta = (target_bearing - old_target_bearing)/100; - - // reset the old value - old_target_bearing = target_bearing; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); - } - // control mode specific updates to nav_bearing // -------------------------------------------- update_navigation(); } -#if 0 -// Disabled for now -void calc_distance_error() -{ - distance_estimate += (float)ground_speed * .0002 * cos(radians(bearing_error * .01)); - distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); - wp_distance = max(distance_estimate,10); -} -#endif - static void calc_gndspeed_undershoot() { if (g_gps->status() == GPS::GPS_OK) { @@ -97,36 +68,6 @@ static long wrap_180(long error) return error; } -static void update_loiter() -{ - float power; - - if(wp_distance <= g.loiter_radius){ - power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); - nav_bearing += (int)(9000.0 * (2.0 + power)); - }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - loiter_time = 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 * asin(g.loiter_radius / wp_distance); - } - - update_crosstrack(); -*/ - nav_bearing = wrap_360(nav_bearing); -} - static void update_crosstrack(void) { // Crosstrack Error diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index a98c1bab73..acad0019ef 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -228,12 +228,11 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode != CIRCLE && mode != LEARNING && mode != AUTO && - mode != RTL && - mode != LOITER) + mode != RTL) { if (mode < MANUAL) - mode = LOITER; - else if (mode >LOITER) + mode = RTL; + else if (mode > RTL) mode = MANUAL; else mode += radioInputSwitch; diff --git a/APMrover2/test.pde b/APMrover2/test.pde index 1e845fbfd6..82ffbac0fa 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -316,7 +316,6 @@ test_wp(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total); cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); - cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); for(byte i = 0; i <= g.command_total; i++){ struct Location temp = get_cmd_with_index(i);