Sub: Refactor methods in navigation.cpp

ArduSub/ArduSub.cpp

@@ -33,7 +33,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(update_batt_compass,   10,    120),
     SCHED_TASK(read_rangefinder,      20,    100),
     SCHED_TASK(update_altitude,       10,    100),
-    SCHED_TASK(run_nav_updates,       50,    100),
     SCHED_TASK(three_hz_loop,          3,     75),
     SCHED_TASK(update_turn_counter,   10,     50),
     SCHED_TASK(compass_accumulate,   100,    100),

ArduSub/GCS_Mavlink.cpp

@@ -277,8 +277,8 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
         targets.x / 1.0e2f,
         targets.y / 1.0e2f,
         targets.z / 1.0e2f,
-        wp_bearing / 1.0e2f,
-        wp_distance / 1.0e2f,
+        wp_nav.get_wp_bearing_to_destination() / 1.0e2f,
+        wp_nav.get_wp_distance_to_destination() / 1.0e2f,
         pos_control.get_alt_error() / 1.0e2f,
         0,
         0);

ArduSub/Sub.cpp

@@ -29,10 +29,6 @@ Sub::Sub(void) :
           control_mode(MANUAL),
           motors(MAIN_LOOP_RATE),
           scaleLongDown(1),
-          wp_bearing(0),
-          home_bearing(0),
-          home_distance(0),
-          wp_distance(0),
           auto_mode(Auto_WP),
           guided_mode(Guided_WP),
           circle_pilot_yaw_override(false),

ArduSub/Sub.h

@@ -298,15 +298,6 @@ private:
     // Sometimes we need to remove the scaling for distance calcs
     float scaleLongDown;
 
-    // Location & Navigation
-    int32_t wp_bearing;
-    // The location of home in relation to the Sub in centi-degrees
-    int32_t home_bearing;
-    // distance between plane and home in cm
-    int32_t home_distance;
-    // distance between plane and next waypoint in cm.
-    uint32_t wp_distance;
-
     // Auto
     AutoMode auto_mode;   // controls which auto controller is run
 
@@ -650,13 +641,6 @@ private:
     bool init_arm_motors(bool arming_from_gcs);
     void init_disarm_motors();
     void motors_output();
-    void run_nav_updates(void);
-    void calc_position();
-    void calc_distance_and_bearing();
-    void calc_wp_distance();
-    void calc_wp_bearing();
-    void calc_home_distance_and_bearing();
-    void run_autopilot();
     void perf_info_reset();
     void perf_ignore_this_loop();
     void perf_info_check_loop_time(uint32_t time_in_micros);

ArduSub/commands_logic.cpp

@@ -149,8 +149,8 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
 // Verify command Handlers
 /********************************************************************************/
 
-// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
-//      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
+// check to see if current command goal has been acheived
+// called by mission library in mission.update()
 bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
     if (control_mode == AUTO) {
@@ -167,13 +167,10 @@ bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 }
 
 
-// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
-//  should return true once the active navigation command completes successfully
-//  called at 10hz or higher
+// check if current mission command has completed
 bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
 {
     switch (cmd.id) {
-
         //
         // navigation commands
         //
@@ -760,9 +757,7 @@ bool Sub::verify_wait_delay()
 
 bool Sub::verify_within_distance()
 {
-    // update distance calculation
-    calc_wp_distance();
-    if (wp_distance < (uint32_t)MAX(condition_value,0)) {
+    if (wp_nav.get_wp_distance_to_destination() < (uint32_t)MAX(condition_value,0)) {
         condition_value = 0;
         return true;
     }

ArduSub/control_auto.cpp

@@ -34,11 +34,13 @@ bool Sub::auto_init(bool ignore_checks)
     }
 }
 
-// auto_run - runs the auto controller
-//      should be called at 100hz or more
-//      relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
+// auto_run - runs the appropriate auto controller
+// according to the current auto_mode
+// should be called at 100hz or more
 void Sub::auto_run()
 {
+    mission.update();
+
     // call the correct auto controller
     switch (auto_mode) {
 

ArduSub/fence.cpp

@@ -11,6 +11,10 @@ void Sub::fence_check()
     uint8_t new_breaches; // the type of fence that has been breached
     uint8_t orig_breaches = fence.get_breaches();
 
+    Vector3f home = pv_location_to_vector(ahrs.get_home());
+    Vector3f curr = inertial_nav.get_position();
+    int32_t home_distance = pv_get_horizontal_distance_cm(curr, home);
+
     // give fence library our current distance from home in meters
     fence.set_home_distance(home_distance*0.01f);
 

ArduSub/motors.cpp

@@ -53,7 +53,6 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
         // Reset home position if it has already been set before (but not locked)
         set_home_to_current_location();
     }
-    calc_distance_and_bearing();
 	
     // enable gps velocity based centrefugal force compensation
     ahrs.set_correct_centrifugal(true);

ArduSub/navigation.cpp

@@ -1,69 +0,0 @@
-#include "Sub.h"
-
-// run_nav_updates - top level call for the autopilot
-// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
-// To-Do - rename and move this function to make it's purpose more clear
-void Sub::run_nav_updates(void)
-{
-    // calculate distance and bearing for reporting and autopilot decisions
-    calc_distance_and_bearing();
-
-    // run autopilot to make high level decisions about control modes
-    run_autopilot();
-}
-
-// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
-void Sub::calc_distance_and_bearing()
-{
-    calc_wp_distance();
-    calc_wp_bearing();
-    calc_home_distance_and_bearing();
-}
-
-// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
-void Sub::calc_wp_distance()
-{
-    // get target from loiter or wpinav controller
-    if (control_mode == CIRCLE) {
-        wp_distance = wp_nav.get_loiter_distance_to_target();
-    } else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
-        wp_distance = wp_nav.get_wp_distance_to_destination();
-    } else {
-        wp_distance = 0;
-    }
-}
-
-// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
-void Sub::calc_wp_bearing()
-{
-    // get target from loiter or wpinav controller
-    if (control_mode == CIRCLE) {
-        wp_bearing = wp_nav.get_loiter_bearing_to_target();
-    } else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
-        wp_bearing = wp_nav.get_wp_bearing_to_destination();
-    } else {
-        wp_bearing = 0;
-    }
-}
-
-// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
-void Sub::calc_home_distance_and_bearing()
-{
-    // calculate home distance and bearing
-    if (position_ok()) {
-        Vector3f home = pv_location_to_vector(ahrs.get_home());
-        Vector3f curr = inertial_nav.get_position();
-        home_distance = pv_get_horizontal_distance_cm(curr, home);
-        home_bearing = pv_get_bearing_cd(curr,home);
-    }
-}
-
-// run_autopilot - highest level call to process mission commands
-void Sub::run_autopilot()
-{
-    if (control_mode == AUTO) {
-        // update state of mission
-        // may call commands_process.pde's start_command and verify_command functions
-        mission.update();
-    }
-}