diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 4e7ab3fff6..3b5987ef8f 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -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 @@ */ 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) 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() 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();