From 8aebeac6d72a911f64f492df3b1b69fc66415ab2 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 25 Nov 2016 15:01:07 -0500 Subject: [PATCH] Sub: Remove precision landing --- ArduSub/APM_Config.h | 1 - ArduSub/Log.cpp | 43 ----------------------------------- ArduSub/Parameters.cpp | 6 ----- ArduSub/Parameters.h | 3 --- ArduSub/Sub.h | 15 ------------ ArduSub/defines.h | 2 +- ArduSub/make.inc | 2 -- ArduSub/precision_landing.cpp | 32 -------------------------- ArduSub/system.cpp | 5 ---- ArduSub/wscript | 2 -- 10 files changed, 1 insertion(+), 110 deletions(-) delete mode 100644 ArduSub/precision_landing.cpp diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index ad2ce35da9..4cb03d13bf 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -29,7 +29,6 @@ //#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands //#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space -#define PRECISION_LANDING DISABLED // enable precision landing using companion computer or IRLock sensor #define TRANSECT_ENABLED DISABLED // features below are disabled by default on all boards diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 392820aea7..280d821f83 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -625,47 +625,6 @@ struct PACKED log_Heli { float main_rotor_speed; }; -// precision landing logging -struct PACKED log_Precland { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t healthy; - float bf_angle_x; - float bf_angle_y; - float ef_angle_x; - float ef_angle_y; - float pos_x; - float pos_y; -}; - -// Write an optical flow packet -void Sub::Log_Write_Precland() -{ - #if PRECISION_LANDING == ENABLED - // exit immediately if not enabled - if (!precland.enabled()) { - return; - } - - const Vector2f &bf_angle = precland.last_bf_angle_to_target(); - const Vector2f &ef_angle = precland.last_ef_angle_to_target(); - const Vector3f &target_pos_ofs = precland.last_target_pos_offset(); - struct log_Precland pkt = { - LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG), - time_us : AP_HAL::micros64(), - healthy : precland.healthy(), - bf_angle_x : degrees(bf_angle.x), - bf_angle_y : degrees(bf_angle.y), - ef_angle_x : degrees(ef_angle.x), - ef_angle_y : degrees(ef_angle.y), - pos_x : target_pos_ofs.x, - pos_y : target_pos_ofs.y - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); - #endif // PRECISION_LANDING == ENABLED -} - -// precision landing logging struct PACKED log_GuidedTarget { LOG_PACKET_HEADER; uint64_t time_us; @@ -731,8 +690,6 @@ const struct LogStructure Sub::log_structure[] = { "ERR", "QBB", "TimeUS,Subsys,ECode" }, { LOG_HELI_MSG, sizeof(log_Heli), "HELI", "Qff", "TimeUS,DRRPM,ERRPM" }, - { LOG_PRECLAND_MSG, sizeof(log_Precland), - "PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, }; diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 7ad7a5e82d..febfbcf1ad 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -1015,12 +1015,6 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(optflow, "FLOW", OpticalFlow), #endif -#if PRECISION_LANDING == ENABLED - // @Group: PLND_ - // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp - GOBJECT(precland, "PLND_", AC_PrecLand), -#endif - // @Group: RPM // @Path: ../libraries/AP_RPM/AP_RPM.cpp GOBJECT(rpm_sensor, "RPM", AP_RPM), diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 33549c88d9..d9e7cdcc23 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -117,9 +117,6 @@ public: k_param_adsb, // 72 k_param_notify, // 73 - // 74: precision landing object - k_param_precland = 74, - // // 90: misc2 // diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index d61514200f..031a920716 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -104,10 +104,6 @@ #if GRIPPER_ENABLED == ENABLED #include // gripper stuff #endif -#if PRECISION_LANDING == ENABLED -#include -#include -#endif // Local modules #include "Parameters.h" @@ -260,9 +256,6 @@ private: control_mode_t prev_control_mode; mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; - - uint32_t precland_last_update_ms; - RCMapper rcmap; // board specific config @@ -501,11 +494,6 @@ private: AP_Terrain terrain; #endif - // Precision Landing -#if PRECISION_LANDING == ENABLED - AC_PrecLand precland; -#endif - AP_ADSB adsb {ahrs}; // use this to prevent recursion during sensor init @@ -615,7 +603,6 @@ private: void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); void Log_Write_Home_And_Origin(); void Log_Sensor_Health(); - void Log_Write_Precland(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); @@ -849,8 +836,6 @@ private: void init_compass(); void init_optflow(); void update_optical_flow(void); - void init_precland(); - void update_precland(); void read_battery(void); void read_receiver_rssi(void); void gripper_update(); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index f52c11ecf6..2bd2e8e267 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -286,7 +286,7 @@ enum ThrowModeState { #define LOG_MOTBATT_MSG 0x1E #define LOG_PARAMTUNE_MSG 0x1F #define LOG_HELI_MSG 0x20 -#define LOG_PRECLAND_MSG 0x21 +//#define LOG_PRECLAND_MSG 0x21 // Remove #define LOG_GUIDEDTARGET_MSG 0x22 #define LOG_PROXIMITY_MSG 0x24 diff --git a/ArduSub/make.inc b/ArduSub/make.inc index 81b6929c27..94c1051bf6 100644 --- a/ArduSub/make.inc +++ b/ArduSub/make.inc @@ -54,8 +54,6 @@ LIBRARIES += AP_Frsky_Telem LIBRARIES += AC_Sprayer LIBRARIES += AP_Terrain LIBRARIES += AP_RPM -LIBRARIES += AC_PrecLand -LIBRARIES += AP_IRLock LIBRARIES += AC_InputManager LIBRARIES += AP_ADSB LIBRARIES += AP_JSButton diff --git a/ArduSub/precision_landing.cpp b/ArduSub/precision_landing.cpp deleted file mode 100644 index 4381caeb41..0000000000 --- a/ArduSub/precision_landing.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// -// functions to support precision landing -// - -#include "Sub.h" - -#if PRECISION_LANDING == ENABLED - -void Sub::init_precland() -{ - sub.precland.init(); -} - -void Sub::update_precland() -{ - int32_t height_above_ground_cm = current_loc.alt; - - // use range finder altitude if it is valid, else try to get terrain alt - if (rangefinder_alt_ok()) { - height_above_ground_cm = rangefinder_state.alt_cm; - } else if (terrain_use()) { - current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm); - } - - sub.precland.update(height_above_ground_cm); - - // log output - Log_Write_Precland(); -} -#endif diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 9103672f94..47bba657fa 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -198,11 +198,6 @@ void Sub::init_ardupilot() camera_mount.init(&DataFlash, serial_manager); #endif -#if PRECISION_LANDING == ENABLED - // initialise precision landing - init_precland(); -#endif - #ifdef USERHOOK_INIT USERHOOK_INIT #endif diff --git a/ArduSub/wscript b/ArduSub/wscript index 260d1e5001..22b9219f08 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -13,11 +13,9 @@ def build(bld): 'AC_Fence', 'AC_Avoidance', 'AC_PID', - 'AC_PrecLand', 'AC_Sprayer', 'AC_WPNav', 'AP_Camera', - 'AP_IRLock', 'AP_InertialNav', 'AP_JSButton', 'AP_LeakDetector',