From a233024e0555a6d1b28b50b61db6ab99cab29d90 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 3 Jan 2017 11:36:26 +1100 Subject: [PATCH] Copter: fix compilation when fence andd proximity are disabled This adds AC_AVOID_ENABLED; avoidance must be disabled if either of fence or proximity are disabled. Parameter definitions have been reordered to avoid compiler warnings; this make sthe numbering non-linear --- ArduCopter/Copter.cpp | 2 ++ ArduCopter/Copter.h | 3 ++- ArduCopter/Parameters.cpp | 16 ++++++++++------ ArduCopter/arming_checks.cpp | 2 ++ ArduCopter/config.h | 13 ++++++++++++- ArduCopter/control_althold.cpp | 2 ++ ArduCopter/control_guided.cpp | 2 ++ ArduCopter/switches.cpp | 2 +- ArduCopter/system.cpp | 2 ++ 9 files changed, 35 insertions(+), 9 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 94d8925111..432667e084 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -91,7 +91,9 @@ Copter::Copter(void) : #if AC_FENCE == ENABLED fence(ahrs, inertial_nav), #endif +#if AC_AVOID_ENABLED == ENABLED avoid(ahrs, inertial_nav, fence, g2.proximity), +#endif #if AC_RALLY == ENABLED rally(ahrs), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 50d6fb961b..007881c17a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -538,8 +538,9 @@ private: #if AC_FENCE == ENABLED AC_Fence fence; #endif +#if AC_AVOID_ENABLED == ENABLED AC_Avoid avoid; - +#endif // Rally library #if AC_RALLY == ENABLED AP_Rally_Copter rally; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 3363cc8449..0ec7d253f6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -820,7 +820,9 @@ const AP_Param::Info Copter::var_info[] = { // @Group: AVOID_ // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp +#if AC_AVOID_ENABLED == ENABLED GOBJECT(avoid, "AVOID_", AC_Avoid), +#endif #if AC_RALLY == ENABLED // @Group: RALLY_ @@ -1001,6 +1003,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), + // @Group: BCN + // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp + AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), + #if PROXIMITY_ENABLED == ENABLED // @Group: PRX // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp @@ -1039,10 +1045,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), #endif - // @Group: BCN - // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp - AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), - // @Param: FRAME_CLASS // @DisplayName: Frame Class // @Description: Controls major frame class for multicopter component @@ -1058,8 +1060,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { constructor for g2 object */ ParametersG2::ParametersG2(void) - : proximity(copter.serial_manager), - beacon(copter.serial_manager) + : beacon(copter.serial_manager) +#if PROXIMITY_ENABLED == ENABLED + , proximity(copter.serial_manager) +#endif #if ADVANCED_FAILSAFE == ENABLED ,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) #endif diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 3868205695..47b30f9bf3 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -635,6 +635,7 @@ bool Copter::pre_arm_proximity_check(bool display_failure) } // get closest object if we might use it for avoidance +#if AC_AVOID_ENABLED == ENABLED float angle_deg, distance; if (avoid.proximity_avoidance_enabled() && g2.proximity.get_closest_object(angle_deg, distance)) { // display error if something is within 60cm @@ -645,6 +646,7 @@ bool Copter::pre_arm_proximity_check(bool display_failure) return false; } } +#endif return true; #else diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 92088ae62d..2116087535 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -642,7 +642,7 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Fence, Rally and Terrain defaults +// Fence, Rally and Terrain and AC_Avoidance defaults // // Enable/disable Fence @@ -662,6 +662,17 @@ #error Terrain relies on Rally which is disabled #endif +#ifndef AC_AVOID_ENABLED + #define AC_AVOID_ENABLED ENABLED +#endif + +#if AC_AVOID_ENABLED && !PROXIMITY_ENABLED + #error AC_Avoidance relies on PROXIMITY_ENABLED which is disabled +#endif +#if AC_AVOID_ENABLED && !AC_FENCE + #error AC_Avoidance relies on AC_FENCE which is disabled +#endif + ////////////////////////////////////////////////////////////////////////////// // Developer Items // diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 0173b514b5..45f0deea2d 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -135,8 +135,10 @@ void Copter::althold_run() case AltHold_Flying: motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +#if AC_AVOID_ENABLED == ENABLED // apply avoidance avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max); +#endif // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index f47d7fa209..eb6deb3d9f 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -632,8 +632,10 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto float vel_delta_z_max = G_Dt * pos_control.get_accel_z(); curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max); +#if AC_AVOID_ENABLED // limit the velocity to prevent fence violations avoid.adjust_velocity(pos_control.get_pos_xy_kP(), pos_control.get_accel_xy(), curr_vel_des); +#endif // update position controller with new target pos_control.set_desired_velocity(curr_vel_des); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 39e95e80c7..7aaafd59cc 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -572,7 +572,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; case AUXSW_AVOID_PROXIMITY: -#if PROXIMITY_ENABLED == ENABLED +#if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: avoid.proximity_avoidance_enable(true); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 922620d430..a8176adcce 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -210,7 +210,9 @@ void Copter::init_ardupilot() Location_Class::set_terrain(&terrain); wp_nav.set_terrain(&terrain); #endif +#if AC_AVOID_ENABLED == ENABLED wp_nav.set_avoidance(&avoid); +#endif attitude_control.parameter_sanity_check(); pos_control.set_dt(MAIN_LOOP_SECONDS);