From 45f2312bfe7c339c95a4c1752dd201c130a9aeb4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 16 Mar 2018 12:23:28 +1100 Subject: [PATCH] Copter: move home state into AP_AHRS --- ArduCopter/AP_Arming.cpp | 5 ----- ArduCopter/AP_Arming.h | 2 -- ArduCopter/AP_State.cpp | 22 ---------------------- ArduCopter/Copter.h | 3 --- ArduCopter/GCS_Mavlink.cpp | 4 ++-- ArduCopter/Log.cpp | 2 +- ArduCopter/commands.cpp | 16 +++++----------- ArduCopter/inertia.cpp | 2 +- ArduCopter/motors.cpp | 4 ++-- 9 files changed, 11 insertions(+), 49 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 3f6bfde674..7061eda544 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -702,11 +702,6 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) return AP_Arming::arm_checks(arming_from_gcs); } -enum HomeState AP_Arming_Copter::home_status() const -{ - return copter.ap.home_state; -} - void AP_Arming_Copter::set_pre_arm_check(bool b) { copter.ap.pre_arm_check = b; diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index a5230ac6b2..054a1cc353 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -47,8 +47,6 @@ protected: void set_pre_arm_check(bool b); - enum HomeState home_status() const override; - private: const AP_InertialNav_NavEKF &_inav; diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index d2789a8416..a48b30667e 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -1,27 +1,5 @@ #include "Copter.h" -// set_home_state - update home state -void Copter::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 Copter::home_is_set() -{ - return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED); -} - // --------------------------------------------- void Copter::set_auto_armed(bool b) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3c4d071042..aeaf3e882b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -315,7 +315,6 @@ private: uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS uint8_t gps_glitching : 1; // 17 // true if the gps is glitching - enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked) uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position @@ -630,8 +629,6 @@ private: static const struct LogStructure log_structure[]; // AP_State.cpp - void set_home_state(enum HomeState new_home_state); - bool home_is_set(); void set_auto_armed(bool b); void set_simple_mode(uint8_t b); void set_failsafe_radio(bool b); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 2785c6915e..d96c6e2d0f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -860,7 +860,7 @@ void GCS_MAVLINK_Copter::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 (copter.ap.home_state == HOME_UNSET) { + if (!AP::ahrs().home_is_set()) { // cannot use relative altitude if home is not set break; } @@ -1126,7 +1126,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_GET_HOME_POSITION: - if (copter.ap.home_state != HOME_UNSET) { + if (AP::ahrs().home_is_set()) { send_home(copter.ahrs.get_home()); Location ekf_origin; if (copter.ahrs.get_origin(ekf_origin)) { diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 7a27819ba7..9bd625814c 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -378,7 +378,7 @@ void Copter::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/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index bb95a58aed..08b04fec61 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -1,17 +1,10 @@ #include "Copter.h" -/* - * the home_state has a number of possible values (see enum HomeState in defines.h's) - * HOME_UNSET = home is not set, no GPS positions yet received - * HOME_SET_NOT_LOCKED = home is set to EKF origin or armed location (can be moved) - * HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command - */ - // checks if we should update ahrs/RTL home position from the EKF void Copter::update_home_from_EKF() { // exit immediately if home already set - if (ap.home_state != HOME_UNSET) { + if (ahrs.home_is_set()) { return; } @@ -83,11 +76,12 @@ bool Copter::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); #if MODE_AUTO_ENABLED == ENABLED // log new home position which mission library will pull from ahrs @@ -102,7 +96,7 @@ bool Copter::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/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index 2b180ff773..60fafff124 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -16,7 +16,7 @@ void Copter::read_inertia() } // without home return alt above the EKF origin - if (ap.home_state == HOME_UNSET) { + if (!ahrs.home_is_set()) { // with inertial nav we can update the altitude and climb rate at 50hz current_loc.alt = inertial_nav.get_altitude(); } else { diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index b4097d6596..2330bdbcb1 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -176,14 +176,14 @@ bool Copter::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) ahrs.resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); // we have reset height, so arming height is zero arming_altitude_m = 0; - } 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);