From 8644f4d76ebc0a6b99bd91c2130bbfc7e15d7a1f Mon Sep 17 00:00:00 2001 From: murata Date: Mon, 25 Feb 2019 23:32:52 +0900 Subject: [PATCH] Copter: Disable option code Copter: Revert change Copter: Change mistakes --- ArduCopter/config.h | 12 ++++++++++++ ArduCopter/mode_brake.cpp | 4 ++++ ArduCopter/mode_guided.cpp | 6 +++++- ArduCopter/mode_guided_nogps.cpp | 4 ++++ ArduCopter/mode_loiter.cpp | 4 ++++ ArduCopter/mode_poshold.cpp | 4 ++++ ArduCopter/mode_rtl.cpp | 4 ++++ ArduCopter/mode_sport.cpp | 4 ++++ 8 files changed, 41 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 73f03fed52..f49d0aa768 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -704,6 +704,18 @@ #error Helicopter frame requires acro mode support which is disabled #endif +#if MODE_SMARTRTL_ENABLED && !MODE_RTL_ENABLED + #error SmartRTL requires ModeRTL which is disabled +#endif + +#if ADSB_ENABLED && !MODE_GUIDED_ENABLED + #error ADSB requires ModeGuided which is disabled +#endif + +#if MODE_FOLLOW_ENABLED && !MODE_GUIDED_ENABLED + #error Follow requires ModeGuided which is disabled +#endif + ////////////////////////////////////////////////////////////////////////////// // Developer Items // diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 8d64d618e3..08a925b900 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_BRAKE_ENABLED == ENABLED + /* * Init and run calls for brake flight mode */ @@ -79,3 +81,5 @@ void Copter::ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) _timeout_start = millis(); _timeout_ms = timeout_ms; } + +#endif diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 0a744b75bb..1476b9656e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_GUIDED_ENABLED == ENABLED + /* * Init and run calls for guided flight mode */ @@ -666,7 +668,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const // limit the velocity to prevent fence violations copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt); // get avoidance adjusted climb rate - curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); + curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); #endif // update position controller with new target @@ -785,3 +787,5 @@ float Copter::ModeGuided::crosstrack_error() const return 0; } } + +#endif diff --git a/ArduCopter/mode_guided_nogps.cpp b/ArduCopter/mode_guided_nogps.cpp index 920532256e..994589f90c 100644 --- a/ArduCopter/mode_guided_nogps.cpp +++ b/ArduCopter/mode_guided_nogps.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_GUIDED_NOGPS_ENABLED == ENABLED + /* * Init and run calls for guided_nogps flight mode */ @@ -19,3 +21,5 @@ void Copter::ModeGuidedNoGPS::run() // run angle controller Copter::ModeGuided::angle_control_run(); } + +#endif diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index f63c593cd1..f110e91375 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_LOITER_ENABLED == ENABLED + /* * Init and run calls for loiter flight mode */ @@ -238,3 +240,5 @@ int32_t Copter::ModeLoiter::wp_bearing() const { return loiter_nav->get_bearing_to_target(); } + +#endif diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 7d24c33a00..a732084d90 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_POSHOLD_ENABLED == ENABLED + /* * Init and run calls for PosHold flight mode * PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller @@ -667,3 +669,5 @@ void Copter::ModePosHold::poshold_pitch_controller_to_pilot_override() // store final loiter outputs for mixing with pilot input poshold.controller_final_pitch = poshold.pitch; } + +#endif diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index c1da00deeb..520025427d 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_RTL_ENABLED == ENABLED + /* * Init and run calls for RTL flight mode * @@ -492,3 +494,5 @@ int32_t Copter::ModeRTL::wp_bearing() const { return wp_nav->get_wp_bearing_to_destination(); } + +#endif diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 9c95cfcd83..76a079812f 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_SPORT_ENABLED == ENABLED + /* * Init and run calls for sport flight mode */ @@ -165,3 +167,5 @@ void Copter::ModeSport::run() break; } } + +#endif