From 0aab17505100e16d0ae3cc78d18db2c722115df8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 14 Oct 2016 14:02:29 +0900 Subject: [PATCH] Copter: move proximity to g2 --- ArduCopter/Copter.cpp | 2 +- ArduCopter/Copter.h | 4 ---- ArduCopter/GCS_Mavlink.cpp | 4 ++-- ArduCopter/Log.cpp | 18 +++++++++--------- ArduCopter/Parameters.cpp | 17 +++++++++-------- ArduCopter/Parameters.h | 6 +++++- ArduCopter/arming_checks.cpp | 2 +- ArduCopter/sensors.cpp | 4 ++-- 8 files changed, 29 insertions(+), 28 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index aa0370eae6..2d87fadc7e 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -76,7 +76,7 @@ Copter::Copter(void) : pos_control(ahrs, inertial_nav, motors, attitude_control, g.p_alt_hold, g.p_vel_z, g.pid_accel_z, g.p_pos_xy, g.pi_vel_xy), - avoid(ahrs, inertial_nav, fence, proximity), + avoid(ahrs, inertial_nav, fence, g2.proximity), wp_nav(inertial_nav, ahrs, pos_control, attitude_control), circle_nav(inertial_nav, ahrs, pos_control), pmTest1(0), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2cb820bd3e..6ccb0c879d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -197,10 +197,6 @@ private: LowPassFilterFloat alt_cm_filt; // altitude filter } rangefinder_state = { false, false, 0, 0 }; -#if PROXIMITY_ENABLED == ENABLED - AP_Proximity proximity {serial_manager}; -#endif - AP_RPM rpm_sensor; // Inertial Navigation EKF diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6a8da19b45..b88e4ec7f3 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -144,7 +144,7 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) control_sensors_present |= MAV_SYS_STATUS_LOGGING; } #if PROXIMITY_ENABLED == ENABLED - if (copter.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { + if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } #endif @@ -232,7 +232,7 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) } #if PROXIMITY_ENABLED == ENABLED - if (copter.proximity.get_status() < AP_Proximity::Proximity_Good) { + if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; } #endif diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 0312ab88a6..39189726de 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -770,19 +770,19 @@ void Copter::Log_Write_Proximity() { #if PROXIMITY_ENABLED == ENABLED float sector_distance[8] = {0,0,0,0,0,0,0,0}; - proximity.get_horizontal_distance(0, sector_distance[0]); - proximity.get_horizontal_distance(45, sector_distance[1]); - proximity.get_horizontal_distance(90, sector_distance[2]); - proximity.get_horizontal_distance(135, sector_distance[3]); - proximity.get_horizontal_distance(180, sector_distance[4]); - proximity.get_horizontal_distance(225, sector_distance[5]); - proximity.get_horizontal_distance(270, sector_distance[6]); - proximity.get_horizontal_distance(315, sector_distance[7]); + g2.proximity.get_horizontal_distance(0, sector_distance[0]); + g2.proximity.get_horizontal_distance(45, sector_distance[1]); + g2.proximity.get_horizontal_distance(90, sector_distance[2]); + g2.proximity.get_horizontal_distance(135, sector_distance[3]); + g2.proximity.get_horizontal_distance(180, sector_distance[4]); + g2.proximity.get_horizontal_distance(225, sector_distance[5]); + g2.proximity.get_horizontal_distance(270, sector_distance[6]); + g2.proximity.get_horizontal_distance(315, sector_distance[7]); struct log_Proximity pkt = { LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), time_us : AP_HAL::micros64(), - health : (uint8_t)proximity.get_status(), + health : (uint8_t)g2.proximity.get_status(), dist0 : sector_distance[0], dist45 : sector_distance[1], dist90 : sector_distance[2], diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 0ca27d4eda..a6d76b8bc4 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -911,12 +911,6 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter), -#if PROXIMITY_ENABLED == ENABLED - // @Group: PRX_ - // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp - GOBJECT(proximity, "PRX_", AP_Proximity), -#endif - // @Param: AUTOTUNE_AXES // @DisplayName: Autotune axis bitmask // @Description: 1-byte bitmap of axes to autotune @@ -1013,7 +1007,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0:ADSBMavlinkProcessing // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), - + +#if PROXIMITY_ENABLED == ENABLED + // @Group: PRX_ + // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp + AP_SUBGROUPINFO(proximity, "PRX_", 8, ParametersG2, AP_Proximity), +#endif + AP_GROUPEND }; @@ -1022,8 +1022,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { constructor for g2 object */ ParametersG2::ParametersG2(void) + : proximity(copter.serial_manager) #if ADVANCED_FAILSAFE == ENABLED - : afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) + ,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) #endif { AP_Param::setup_object_defaults(this, var_info); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7d2e34cb39..968f413829 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -362,7 +362,6 @@ public: k_param_rtl_climb_min, k_param_rpm_sensor, k_param_autotune_min_d, // 251 - k_param_proximity, k_param_DataFlash = 253, // 253 - Logging Group // 254,255: reserved @@ -559,6 +558,11 @@ public: // ground effect compensation enable/disable AP_Int8 gndeffect_comp_enabled; +#if PROXIMITY_ENABLED == ENABLED + // proximity (aka object avoidance) library + AP_Proximity proximity; +#endif + #if ADVANCED_FAILSAFE == ENABLED // advanced failsafe library AP_AdvancedFailsafe_Copter afs; diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 992d9cf43d..bbdf7cad3e 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -380,7 +380,7 @@ bool Copter::parameter_checks(bool display_failure) #if PROXIMITY_ENABLED == ENABLED // check proximity sensor if enabled - if (copter.proximity.get_status() == AP_Proximity::Proximity_NoData) { + if (copter.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check proximity sensor"); } diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 0601369027..dd94d9888b 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -258,7 +258,7 @@ void Copter::button_update(void) void Copter::init_proximity(void) { #if PROXIMITY_ENABLED == ENABLED - proximity.init(); + g2.proximity.init(); #endif } @@ -266,7 +266,7 @@ void Copter::init_proximity(void) void Copter::update_proximity(void) { #if PROXIMITY_ENABLED == ENABLED - proximity.update(); + g2.proximity.update(); #endif }