From 3af4806d383d9e3b5ce0142c6235257df9aa88da Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 16 Mar 2018 13:12:42 +1100 Subject: [PATCH] Sub: move home state into AP_AHRS --- ArduSub/AP_Arming_Sub.cpp | 5 ----- ArduSub/AP_Arming_Sub.h | 1 - ArduSub/AP_State.cpp | 23 ----------------------- ArduSub/GCS_Mavlink.cpp | 4 ++-- ArduSub/Log.cpp | 2 +- ArduSub/Sub.h | 3 --- ArduSub/commands.cpp | 9 +++++---- ArduSub/motors.cpp | 4 ++-- 8 files changed, 10 insertions(+), 41 deletions(-) diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 739905feb6..84653d043e 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -1,11 +1,6 @@ #include "AP_Arming_Sub.h" #include "Sub.h" -enum HomeState AP_Arming_Sub::home_status() const -{ - return sub.ap.home_state; -} - bool AP_Arming_Sub::rc_calibration_checks(bool display_failure) { const RC_Channel *channels[] = { diff --git a/ArduSub/AP_Arming_Sub.h b/ArduSub/AP_Arming_Sub.h index b00fd6b328..1b7351acff 100644 --- a/ArduSub/AP_Arming_Sub.h +++ b/ArduSub/AP_Arming_Sub.h @@ -19,5 +19,4 @@ public: protected: bool ins_checks(bool report) override; - enum HomeState home_status() const override; }; diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index 15b4f35f93..c22a1615d2 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -1,24 +1 @@ #include "Sub.h" - -// set_home_state - update home state -void Sub::set_home_state(enum HomeState new_home_state) -{ - // if no change, exit immediately - if (ap.home_state == new_home_state) { - return; - } - - // update state - ap.home_state = new_home_state; - - // log if home has been set - if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) { - Log_Write_Event(DATA_SET_HOME); - } -} - -// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin) -bool Sub::home_is_set() -{ - return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED); -} diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index eae8e000f5..b8cf6edc9c 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -968,7 +968,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) new_home_loc.alt = packet.z * 100; // handle relative altitude if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - if (sub.ap.home_state == HOME_UNSET) { + if (!AP::ahrs().home_is_set()) { // cannot use relative altitude if home is not set break; } @@ -1194,7 +1194,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_GET_HOME_POSITION: - if (sub.ap.home_state != HOME_UNSET) { + if (AP::ahrs().home_is_set()) { send_home(sub.ahrs.get_home()); Location ekf_origin; if (sub.ahrs.get_origin(ekf_origin)) { diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 84f505c144..a5f8bb0b1c 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -291,7 +291,7 @@ void Sub::Log_Write_Home_And_Origin() } // log ahrs home if set - if (ap.home_state != HOME_UNSET) { + if (ahrs.home_is_set()) { DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); } } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 1aa0eafb54..204d84934b 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -235,7 +235,6 @@ private: uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes uint8_t system_time_set : 1; // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only) - enum HomeState home_state : 2; // home status (unset, set, locked) uint8_t at_bottom : 1; // true if we are at the bottom uint8_t at_surface : 1; // true if we are at the surface uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot @@ -459,8 +458,6 @@ private: void update_turn_counter(); void read_AHRS(void); void update_altitude(); - void set_home_state(enum HomeState new_home_state); - bool home_is_set(); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); float get_pilot_desired_yaw_rate(int16_t stick_angle); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 9117052b4d..e57905ea08 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -11,7 +11,7 @@ void Sub::update_home_from_EKF() { // exit immediately if home already set - if (ap.home_state != HOME_UNSET) { + if (ahrs.home_is_set()) { return; } @@ -73,11 +73,12 @@ bool Sub::set_home(const Location& loc, bool lock) ahrs.set_home(loc); // init inav and compass declination - if (ap.home_state == HOME_UNSET) { + if (!ahrs.home_is_set()) { // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); // record home is set - set_home_state(HOME_SET_NOT_LOCKED); + ahrs.set_home_status(HOME_SET_NOT_LOCKED); + Log_Write_Event(DATA_SET_HOME); // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { @@ -90,7 +91,7 @@ bool Sub::set_home(const Location& loc, bool lock) // lock home position if (lock) { - set_home_state(HOME_SET_AND_LOCKED); + ahrs.set_home_status(HOME_SET_AND_LOCKED); } // log ahrs home and ekf origin dataflash diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 8a9ac10b64..7c0b2849c5 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -44,13 +44,13 @@ bool Sub::init_arm_motors(bool arming_from_gcs) initial_armed_bearing = ahrs.yaw_sensor; - if (ap.home_state == HOME_UNSET) { + if (!ahrs.home_is_set()) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) // Always use absolute altitude for ROV // ahrs.resetHeightDatum(); // Log_Write_Event(DATA_EKF_ALT_RESET); - } else if (ap.home_state == HOME_SET_NOT_LOCKED) { + } else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(false); }