diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index c2536cc2b1..20197cfb0a 100644 --- a/ArduSub/ArduSub.cpp +++ b/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), diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0ac49101e2..c6d783d497 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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); diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index f2d719ae23..cfa6bfa92a 100644 --- a/ArduSub/Sub.cpp +++ b/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), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 39cca5480c..1897b98e05 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 86fe277eb2..ed28dbd69a 100644 --- a/ArduSub/commands_logic.cpp +++ b/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; } diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index ae3e559764..39aa9a40f5 100644 --- a/ArduSub/control_auto.cpp +++ b/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) { diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 70e656f399..a101fe8e64 100644 --- a/ArduSub/fence.cpp +++ b/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); diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index f2eb1a7079..560563d1a9 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -53,8 +53,7 @@ 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); hal.util->set_soft_armed(true); diff --git a/ArduSub/navigation.cpp b/ArduSub/navigation.cpp deleted file mode 100644 index d7f7acc939..0000000000 --- a/ArduSub/navigation.cpp +++ /dev/null @@ -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(); - } -}