From 103e2cc7112e7f91a5bddaa5aa9a38135813da0a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Dec 2017 12:28:32 +1100 Subject: [PATCH] Copter: calculate wp bearing and distance on demand Move responsibility for calculating wp bearing/distance into the FlightMode object doing the navigation Calculating these variables was being done at 50Hz where they were used at 10Hz max. --- ArduCopter/Copter.cpp | 2 - ArduCopter/Copter.h | 8 ---- ArduCopter/FlightMode.h | 49 +++++++++++++++++++- ArduCopter/GCS_Mavlink.cpp | 4 +- ArduCopter/commands_logic.cpp | 4 +- ArduCopter/control_guided.cpp | 29 ++++++++++++ ArduCopter/flight_mode.cpp | 6 +++ ArduCopter/motors.cpp | 2 +- ArduCopter/navigation.cpp | 85 +---------------------------------- 9 files changed, 87 insertions(+), 102 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index b356c28ec6..9bc05ffd44 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -29,10 +29,8 @@ Copter::Copter(void) flightmode(&flightmode_stabilize), control_mode(STABILIZE), scaleLongDown(1), - wp_bearing(0), home_bearing(0), home_distance(0), - wp_distance(0), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), super_simple_last_bearing(0), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c31adf890c..a4321b5461 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -361,14 +361,10 @@ 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 vehicle in centi-degrees int32_t home_bearing; // distance between vehicle and home in cm int32_t home_distance; - // distance between vehicle and next waypoint in cm. - uint32_t wp_distance; LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending) struct { @@ -818,11 +814,7 @@ private: void motors_output(); void lost_vehicle_check(); void run_nav_updates(void); - void calc_distance_and_bearing(); - void calc_wp_distance(); - void calc_wp_bearing(); void calc_home_distance_and_bearing(); - void run_autopilot(); Vector3f pv_location_to_vector(const Location& loc); float pv_alt_above_origin(float alt_above_home_cm); float pv_alt_above_home(float alt_above_origin_cm); diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index c57e3e50b6..f213523c2d 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -50,6 +50,12 @@ protected: // returns a string for this flightmode, exactly 4 bytes virtual const char *name4() const = 0; + // navigation support functions: + void update_navigation(); + virtual void run_autopilot() {} + virtual uint32_t wp_distance() const { return 0; } + virtual int32_t wp_bearing() const { return 0; } + Copter &_copter; // convenience references to avoid code churn in conversion: @@ -297,7 +303,7 @@ public: virtual bool allows_arming(bool from_gcs) const override { return false; }; // Auto - AutoMode mode() { return _mode; } + AutoMode mode() const { return _mode; } bool loiter_start(); void rtl_start(); @@ -321,6 +327,14 @@ protected: const char *name() const override { return "AUTO"; } const char *name4() const override { return "AUTO"; } + uint32_t wp_distance() const override { + return wp_nav->get_wp_distance_to_destination(); + } + int32_t wp_bearing() const override { + return wp_nav->get_wp_bearing_to_destination(); + } + void run_autopilot() override { mission.update(); } + // void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); private: @@ -371,6 +385,13 @@ protected: const char *name() const override { return "CIRCLE"; } const char *name4() const override { return "CIRC"; } + uint32_t wp_distance() const override { + return wp_nav->get_loiter_distance_to_target(); + } + int32_t wp_bearing() const override { + return wp_nav->get_loiter_bearing_to_target(); + } + private: // Circle @@ -406,6 +427,13 @@ protected: const char *name() const override { return "LOITER"; } const char *name4() const override { return "LOIT"; } + uint32_t wp_distance() const override { + return wp_nav->get_loiter_distance_to_target(); + } + int32_t wp_bearing() const override { + return wp_nav->get_loiter_bearing_to_target(); + } + #if PRECISION_LANDING == ENABLED bool do_precision_loiter(); void precision_loiter_xy(); @@ -454,7 +482,7 @@ public: bool takeoff_start(float final_alt_above_home); - GuidedMode mode() { return guided_mode; } + GuidedMode mode() const { return guided_mode; } void angle_control_start(); void angle_control_run(); @@ -464,6 +492,9 @@ protected: const char *name() const override { return "GUIDED"; } const char *name4() const override { return "GUID"; } + uint32_t wp_distance() const override; + int32_t wp_bearing() const override; + private: void pos_control_start(); @@ -551,6 +582,13 @@ protected: const char *name() const override { return "RTL"; } const char *name4() const override { return "RTL "; } + uint32_t wp_distance() const override { + return wp_nav->get_wp_distance_to_destination(); + } + int32_t wp_bearing() const override { + return wp_nav->get_wp_bearing_to_destination(); + } + void descent_start(); void descent_run(); void land_start(); @@ -1021,6 +1059,13 @@ protected: const char *name() const override { return "SMARTRTL"; } const char *name4() const override { return "SRTL"; } + uint32_t wp_distance() const override { + return wp_nav->get_wp_distance_to_destination(); + } + int32_t wp_bearing() const override { + return wp_nav->get_wp_bearing_to_destination(); + } + private: void wait_cleanup_run(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 853129e2df..94abf7a9b7 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -166,8 +166,8 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) targets.x * 1.0e-2f, targets.y * 1.0e-2f, targets.z * 1.0e-2f, - wp_bearing * 1.0e-2f, - MIN(wp_distance * 1.0e-2f, UINT16_MAX), + flightmode->wp_bearing() * 1.0e-2f, + MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX), pos_control->get_alt_error() * 1.0e-2f, 0, 0); diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index bfeff4110b..1e4c041c7c 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -1060,9 +1060,7 @@ bool Copter::verify_wait_delay() bool Copter::verify_within_distance() { - // update distance calculation - calc_wp_distance(); - if (wp_distance < (uint32_t)MAX(condition_value,0)) { + if (flightmode->wp_distance() < (uint32_t)MAX(condition_value,0)) { condition_value = 0; return true; } diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 1efca123fb..10eea7272e 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -774,3 +774,32 @@ bool Copter::FlightMode_GUIDED::limit_check() // if we got this far we must be within limits return false; } + + +uint32_t Copter::FlightMode_GUIDED::wp_distance() const +{ + switch(mode()) { + case Guided_WP: + return wp_nav->get_wp_distance_to_destination(); + break; + case Guided_PosVel: + return pos_control->get_distance_to_target(); + break; + default: + return 0; + } +} + +int32_t Copter::FlightMode_GUIDED::wp_bearing() const +{ + switch(mode()) { + case Guided_WP: + return wp_nav->get_wp_bearing_to_destination(); + break; + case Guided_PosVel: + return pos_control->get_bearing_to_target(); + break; + default: + return 0; + } +} diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index ce0be59cd1..53012d8b3f 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -236,3 +236,9 @@ void Copter::notify_flight_mode() { AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); notify.set_flight_mode_str(flightmode->name4()); } + +void Copter::FlightMode::update_navigation() +{ + // run autopilot to make high level decisions about control modes + run_autopilot(); +} diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index f096d52a0b..a46c8d0b68 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -177,7 +177,7 @@ bool Copter::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(false); } - calc_distance_and_bearing(); + calc_home_distance_and_bearing(); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point g2.smart_rtl.reset_path(position_ok()); diff --git a/ArduCopter/navigation.cpp b/ArduCopter/navigation.cpp index 2194946b7c..e94804e337 100644 --- a/ArduCopter/navigation.cpp +++ b/ArduCopter/navigation.cpp @@ -6,82 +6,9 @@ void Copter::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 Copter::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 Copter::calc_wp_distance() -{ - // get target from loiter or wpinav controller - switch (control_mode) { - case LOITER: - case CIRCLE: - wp_distance = wp_nav->get_loiter_distance_to_target(); - break; - - case AUTO: - case RTL: - case SMART_RTL: - wp_distance = wp_nav->get_wp_distance_to_destination(); - break; - - case GUIDED: - if (flightmode_guided.mode() == Guided_WP) { - wp_distance = wp_nav->get_wp_distance_to_destination(); - break; - } - if (flightmode_guided.mode() == Guided_PosVel) { - wp_distance = pos_control->get_distance_to_target(); - break; - } - FALLTHROUGH; - default: - wp_distance = 0; - break; - } -} - -// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions -void Copter::calc_wp_bearing() -{ - // get target from loiter or wpinav controller - switch (control_mode) { - case LOITER: - case CIRCLE: - wp_bearing = wp_nav->get_loiter_bearing_to_target(); - break; - - case AUTO: - case RTL: - case SMART_RTL: - wp_bearing = wp_nav->get_wp_bearing_to_destination(); - break; - - case GUIDED: - if (flightmode_guided.mode() == Guided_WP) { - wp_bearing = wp_nav->get_wp_bearing_to_destination(); - break; - } - if (flightmode_guided.mode() == Guided_PosVel) { - wp_bearing = pos_control->get_bearing_to_target(); - break; - } - FALLTHROUGH; - default: - wp_bearing = 0; - break; - } + flightmode->update_navigation(); } // calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions @@ -98,13 +25,3 @@ void Copter::calc_home_distance_and_bearing() update_super_simple_bearing(false); } } - -// run_autopilot - highest level call to process mission commands -void Copter::run_autopilot() -{ - if (control_mode == AUTO) { - // update state of mission - // may call commands_process.pde's start_command and verify_command functions - mission.update(); - } -}