Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: more cleanup, and fixed navigation code 

removed loiter code
This commit is contained in:
Andrew Tridgell 2012-11-28 09:20:20 +11:00
parent a16ba57467
commit 691d19dd98
14 changed files with 62 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
APMrover2/APM_Config_HILmode.h
View File

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

18
APMrover2/APM_Config_Rover.h
View File

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

145
APMrover2/APMrover2.pde
View File

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

21
APMrover2/GCS_Mavlink.pde
View File

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

3
APMrover2/Parameters.h
View File

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

1
APMrover2/Parameters.pde
View File

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

13
APMrover2/commands.pde
View File

@ -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(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing_cd(&current_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(&current_loc, &next_WP);
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();

93
APMrover2/commands_logic.pde
View File

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

13
APMrover2/config.h
View File

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

2
APMrover2/defines.h
View File

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

2
APMrover2/events.pde
View File

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

61
APMrover2/navigation.pde
View File

@ -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(&current_loc, &next_WP);
if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> 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

7
APMrover2/setup.pde
View File

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

1
APMrover2/test.pde
View File

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