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

Copter: replace set_next_WP with direct call to wp_nav

This commit is contained in:
Randy Mackay 2013-03-21 18:29:38 +09:00
parent 0d70ba1030
commit 58ed8cd544
7 changed files with 21 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -668,14 +668,10 @@ static struct Location home;
static struct Location current_loc;
// Next WP is the desired location of the copter - the next waypoint or loiter location
static struct Location next_WP;
// Prev WP is used to get the optimum path from one WP to the next
static struct Location prev_WP;
// Holds the current loaded command from the EEPROM for navigation
static struct Location command_nav_queue;
// Holds the current loaded command from the EEPROM for conditional scripts
static struct Location command_cond_queue;
// Holds the current loaded command from the EEPROM for guided mode
static struct Location guided_WP;
////////////////////////////////////////////////////////////////////////////////

14
ArduCopter/GCS_Mavlink.pde
View File

@ -1695,15 +1695,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
guided_WP = tell_command;
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) {
guided_WP.alt += home.alt;
}
// switch to guided mode
set_mode(GUIDED);
// set wp_nav's destination
wp_nav.set_destination(pv_location_to_vector(tell_command));
// set altitude target
set_new_altitude(tell_command.alt);
// verify we recevied the command
mavlink_msg_mission_ack_send(
chan,

65
ArduCopter/commands.pde
View File

@ -113,63 +113,6 @@ static int32_t get_RTL_alt()
}
}
//********************************************************************************
// This function sets the waypoint and modes for Return to Launch
// It's not currently used
//********************************************************************************
/*
* This function sets the next waypoint command
* It precalculates all the necessary stuff.
*/
static void set_next_WP(const struct Location *wp)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
if (next_WP.lat == 0 || command_nav_index <= 1) {
prev_WP = current_loc;
}else{
if (get_distance_cm(&current_loc, &next_WP) < 500)
prev_WP = next_WP;
else
prev_WP = current_loc;
}
// Load the next_WP slot
// ---------------------
next_WP.options = wp->options;
next_WP.lat = wp->lat;
next_WP.lng = wp->lng;
// Set new target altitude so we can track it for climb_rate
// To-Do: remove the restriction on negative altitude targets (after testing)
set_new_altitude(max(wp->alt,100));
// recalculate waypoint distance and bearing
wp_distance = get_distance_cm(&current_loc, &next_WP);
wp_bearing = get_bearing_cd(&prev_WP, &next_WP);
original_wp_bearing = wp_bearing; // to check if we have missed the WP
}
// set_next_WP_latlon - update just lat and lon targets for nav controllers
static void set_next_WP_latlon(uint32_t lat, uint32_t lon)
{
// save current location as previous location
prev_WP = current_loc;
// Load the next_WP slot
next_WP.lat = lat;
next_WP.lng = lon;
// recalculate waypoint distance and bearing
// To-Do: these functions are too expensive to be called every time we update the next_WP
wp_distance = get_distance_cm(&current_loc, &next_WP);
wp_bearing = get_bearing_cd(&prev_WP, &next_WP);
original_wp_bearing = wp_bearing; // to check if we have missed the WP
}
// run this at setup on the ground
// -------------------------------
static void init_home()
@ -194,14 +137,6 @@ static void init_home()
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(&home);
scaleLongUp = 1.0f/scaleLongDown;
// Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.rtl_altitude;
}

7
ArduCopter/commands_logic.pde
View File

@ -276,6 +276,10 @@ static void do_nav_wp()
// Set wp navigation target
wp_nav.set_destination(pv_location_to_vector(command_nav_queue));
// initialise original_wp_bearing which is used to check if we have missed the waypoint
wp_bearing = wp_nav.get_bearing_to_destination();
original_wp_bearing = wp_bearing;
// this is our bitmask to verify we have met all conditions to move on
wp_verify_byte = 0;
@ -555,8 +559,7 @@ static bool verify_RTL()
// land
do_land();
// override landing location (do_land defaults to current location)
// To-do: ensure this location override is being sent to inav loiter controller
set_next_WP_latlon(home.lat, home.lng);
wp_nav.set_loiter_target(Vector3f(0,0,0));
// update RTL state
rtl_state = RTL_STATE_LAND;
}else{

13
ArduCopter/limits.pde
View File

@ -30,12 +30,13 @@ void set_recovery_home_alt() {
if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) {
return_altitude_cm_ahl = home.alt + 400;
}
guided_WP.id = 0;
guided_WP.p1 = 0;
guided_WP.options = 0;
guided_WP.lat = home.lat;
guided_WP.lng = home.lng;
guided_WP.alt = return_altitude_cm_ahl;
// To-Do: ensure target is fed into wp_nav controller. Note this function is currently not called
//guided_WP.id = 0;
//guided_WP.p1 = 0;
//guided_WP.options = 0;
//guided_WP.lat = home.lat;
//guided_WP.lng = home.lng;
//guided_WP.alt = return_altitude_cm_ahl;
}
static void limits_loop() {

7
ArduCopter/navigation.pde
View File

@ -98,11 +98,8 @@ static void run_autopilot()
verify_commands();
break;
case GUIDED:
// switch to loiter once we've reached the target location and altitude
// To-Do: this incorrectly checks verify_nav_wp even though the nav mode may be NAV_LOITER
if(verify_nav_wp()) {
set_nav_mode(NAV_LOITER);
}
// no need to do anything - wp_nav should take care of getting us to the desired location
break;
case RTL:
verify_RTL();
break;

6
ArduCopter/system.pde
View File

@ -439,7 +439,6 @@ static void set_mode(uint8_t mode)
set_yaw_mode(LOITER_YAW);
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(LOITER_THR);
set_next_WP(&current_loc);
set_nav_mode(LOITER_NAV);
break;
@ -449,7 +448,6 @@ static void set_mode(uint8_t mode)
set_yaw_mode(POSITION_YAW);
set_roll_pitch_mode(POSITION_RP);
set_throttle_mode(POSITION_THR);
set_next_WP(&current_loc);
set_nav_mode(POSITION_NAV);
break;
@ -460,9 +458,6 @@ static void set_mode(uint8_t mode)
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
set_nav_mode(GUIDED_NAV);
wp_verify_byte = 0;
guided_WP = current_loc;
set_next_WP(&guided_WP);
break;
case LAND:
@ -491,7 +486,6 @@ static void set_mode(uint8_t mode)
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_nav_mode(OF_LOITER_NAV);
set_next_WP(&current_loc);
break;
// THOR