|
|
|
@ -1,10 +1,10 @@
@@ -1,10 +1,10 @@
|
|
|
|
|
#include "Rover.h" |
|
|
|
|
|
|
|
|
|
/* Functions in this file:
|
|
|
|
|
void set_next_WP(const AP_Mission::Mission_Command& cmd) |
|
|
|
|
void set_guided_WP(void) |
|
|
|
|
void init_home() |
|
|
|
|
void restart_nav() |
|
|
|
|
void set_next_WP(const AP_Mission::Mission_Command& cmd) |
|
|
|
|
void set_guided_WP(void) |
|
|
|
|
void init_home() |
|
|
|
|
void restart_nav() |
|
|
|
|
************************************************************
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
@ -14,13 +14,13 @@
@@ -14,13 +14,13 @@
|
|
|
|
|
*/ |
|
|
|
|
void Rover::set_next_WP(const struct Location& loc) |
|
|
|
|
{ |
|
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
|
// ---------------------------------------
|
|
|
|
|
prev_WP = next_WP; |
|
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
|
// ---------------------------------------
|
|
|
|
|
prev_WP = next_WP; |
|
|
|
|
|
|
|
|
|
// Load the next_WP slot
|
|
|
|
|
// ---------------------
|
|
|
|
|
next_WP = loc; |
|
|
|
|
// Load the next_WP slot
|
|
|
|
|
// ---------------------
|
|
|
|
|
next_WP = loc; |
|
|
|
|
|
|
|
|
|
// are we already past the waypoint? This happens when we jump
|
|
|
|
|
// waypoints, and it can cause us to skip a waypoint. If we are
|
|
|
|
@ -32,24 +32,24 @@ void Rover::set_next_WP(const struct Location& loc)
@@ -32,24 +32,24 @@ void Rover::set_next_WP(const struct Location& loc)
|
|
|
|
|
prev_WP = current_loc; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// this is handy for the groundstation
|
|
|
|
|
wp_totalDistance = get_distance(current_loc, next_WP); |
|
|
|
|
wp_distance = wp_totalDistance; |
|
|
|
|
// this is handy for the groundstation
|
|
|
|
|
wp_totalDistance = get_distance(current_loc, next_WP); |
|
|
|
|
wp_distance = wp_totalDistance; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::set_guided_WP(void) |
|
|
|
|
{ |
|
|
|
|
// copy the current location into the OldWP slot
|
|
|
|
|
// ---------------------------------------
|
|
|
|
|
prev_WP = current_loc; |
|
|
|
|
// copy the current location into the OldWP slot
|
|
|
|
|
// ---------------------------------------
|
|
|
|
|
prev_WP = current_loc; |
|
|
|
|
|
|
|
|
|
// Load the next_WP slot
|
|
|
|
|
// ---------------------
|
|
|
|
|
next_WP = guided_WP; |
|
|
|
|
// Load the next_WP slot
|
|
|
|
|
// ---------------------
|
|
|
|
|
next_WP = guided_WP; |
|
|
|
|
|
|
|
|
|
// this is handy for the groundstation
|
|
|
|
|
wp_totalDistance = get_distance(current_loc, next_WP); |
|
|
|
|
wp_distance = wp_totalDistance; |
|
|
|
|
// this is handy for the groundstation
|
|
|
|
|
wp_totalDistance = get_distance(current_loc, next_WP); |
|
|
|
|
wp_distance = wp_totalDistance; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// run this at setup on the ground
|
|
|
|
@ -61,27 +61,27 @@ void Rover::init_home()
@@ -61,27 +61,27 @@ void Rover::init_home()
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Init HOME"); |
|
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Init HOME"); |
|
|
|
|
|
|
|
|
|
ahrs.set_home(gps.location()); |
|
|
|
|
home_is_set = HOME_SET_NOT_LOCKED; |
|
|
|
|
Log_Write_Home_And_Origin(); |
|
|
|
|
home_is_set = HOME_SET_NOT_LOCKED; |
|
|
|
|
Log_Write_Home_And_Origin(); |
|
|
|
|
GCS_MAVLINK::send_home_all(gps.location()); |
|
|
|
|
|
|
|
|
|
// Save Home to EEPROM
|
|
|
|
|
mission.write_home_to_storage(); |
|
|
|
|
// Save Home to EEPROM
|
|
|
|
|
mission.write_home_to_storage(); |
|
|
|
|
|
|
|
|
|
// Save prev loc
|
|
|
|
|
// -------------
|
|
|
|
|
next_WP = prev_WP = home; |
|
|
|
|
// Save prev loc
|
|
|
|
|
// -------------
|
|
|
|
|
next_WP = prev_WP = home; |
|
|
|
|
|
|
|
|
|
// Load home for a default guided_WP
|
|
|
|
|
// -------------
|
|
|
|
|
guided_WP = home; |
|
|
|
|
// Load home for a default guided_WP
|
|
|
|
|
// -------------
|
|
|
|
|
guided_WP = home; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::restart_nav() |
|
|
|
|
{
|
|
|
|
|
{ |
|
|
|
|
g.pidSpeedThrottle.reset_I(); |
|
|
|
|
prev_WP = current_loc; |
|
|
|
|
mission.start_or_resume(); |
|
|
|
|