From a7c64e0a944e5c835664794f7fe09545195b3d26 Mon Sep 17 00:00:00 2001 From: muramura Date: Tue, 9 Apr 2024 06:27:48 +0900 Subject: [PATCH] working --- ArduCopter/AP_Arming.cpp | 16 +-- ArduCopter/Copter.cpp | 28 ++--- ArduCopter/Copter.h | 104 ++++++++-------- ArduCopter/GCS_Copter.cpp | 4 +- ArduCopter/GCS_Mavlink.cpp | 64 +++++----- ArduCopter/Parameters.cpp | 116 +++++++++--------- ArduCopter/Parameters.h | 80 ++++++------ ArduCopter/RC_Channel.cpp | 92 +++++++------- ArduCopter/autoyaw.cpp | 11 +- ArduCopter/avoidance_adsb.cpp | 12 +- ArduCopter/commands.cpp | 8 +- ArduCopter/crash_check.cpp | 4 +- ArduCopter/events.cpp | 12 +- ArduCopter/land_detector.cpp | 4 +- ArduCopter/mode.cpp | 88 ++++++------- ArduCopter/mode.h | 40 +++--- ArduCopter/mode_acro.cpp | 3 +- ArduCopter/mode_acro_heli.cpp | 4 +- ArduCopter/mode_auto.cpp | 16 ++- ArduCopter/mode_autotune.cpp | 4 +- ArduCopter/mode_brake.cpp | 4 +- ArduCopter/mode_circle.cpp | 4 +- ArduCopter/mode_drift.cpp | 3 +- ArduCopter/mode_flip.cpp | 4 +- ArduCopter/mode_flowhold.cpp | 4 +- ArduCopter/mode_follow.cpp | 4 +- ArduCopter/mode_guided.cpp | 8 +- ArduCopter/mode_guided_nogps.cpp | 4 +- ArduCopter/mode_loiter.cpp | 4 +- ArduCopter/mode_poshold.cpp | 4 +- ArduCopter/mode_rtl.cpp | 4 +- ArduCopter/mode_smart_rtl.cpp | 4 +- ArduCopter/mode_sport.cpp | 4 +- ArduCopter/mode_throw.cpp | 4 +- ArduCopter/mode_turtle.cpp | 4 +- ArduCopter/mode_zigzag.cpp | 4 +- ArduCopter/sensors.cpp | 32 ++--- ArduCopter/surface_tracking.cpp | 10 +- ArduCopter/system.cpp | 16 +-- ArduCopter/terrain.cpp | 4 +- ArduCopter/toy_mode.cpp | 16 +-- ArduCopter/tuning.cpp | 4 +- ArduPlane/ArduPlane.cpp | 4 +- ArduPlane/GCS_Mavlink.cpp | 12 +- ArduPlane/Log.cpp | 12 +- ArduPlane/Parameters.cpp | 12 +- ArduPlane/Parameters.h | 4 +- ArduPlane/Plane.h | 20 +-- ArduPlane/RC_Channel.cpp | 4 +- ArduPlane/commands_logic.cpp | 8 +- ArduPlane/events.cpp | 12 +- ArduPlane/is_flying.cpp | 4 +- ArduPlane/mode.cpp | 4 +- ArduPlane/mode_guided.cpp | 8 +- ArduPlane/navigation.cpp | 8 +- ArduPlane/parachute.cpp | 7 +- ArduPlane/radio.cpp | 4 +- ArduPlane/servos.cpp | 4 +- ArduPlane/tuning.cpp | 4 +- ArduPlane/tuning.h | 4 +- ArduSub/GCS_Mavlink.cpp | 4 +- ArduSub/GCS_Sub.cpp | 4 +- ArduSub/Parameters.cpp | 8 +- ArduSub/Parameters.h | 4 +- ArduSub/Sub.h | 12 +- ArduSub/commands_logic.cpp | 8 +- ArduSub/joystick.cpp | 4 +- ArduSub/sensors.cpp | 20 +-- ArduSub/system.cpp | 8 +- ArduSub/terrain.cpp | 4 +- Rover/RC_Channel.cpp | 4 +- Rover/Rover.h | 8 +- Rover/mode.cpp | 4 +- Rover/mode.h | 4 +- Rover/mode_auto.cpp | 8 +- Rover/mode_follow.cpp | 4 +- Tools/AP_Periph/AP_Periph.cpp | 8 +- Tools/AP_Periph/AP_Periph.h | 4 +- Tools/AP_Periph/Parameters.cpp | 4 +- Tools/AP_Periph/airspeed.cpp | 4 +- Tools/ardupilotwaf/boards.py | 2 +- libraries/AC_Fence/AC_Fence.h | 4 +- libraries/AC_Fence/AC_PolyFence_loader.cpp | 4 +- libraries/AC_Fence/AC_PolyFence_loader.h | 8 +- libraries/AP_ADSB/AP_ADSB.cpp | 4 +- libraries/AP_Arming/AP_Arming.cpp | 68 +++++----- libraries/AP_Baro/AP_Baro.cpp | 72 +++++------ libraries/AP_Baro/AP_Baro_BMP085.cpp | 4 +- libraries/AP_Baro/AP_Baro_BMP085.h | 4 +- libraries/AP_Baro/AP_Baro_BMP280.cpp | 4 +- libraries/AP_Baro/AP_Baro_BMP280.h | 4 +- libraries/AP_Baro/AP_Baro_BMP388.cpp | 4 +- libraries/AP_Baro/AP_Baro_BMP388.h | 4 +- libraries/AP_Baro/AP_Baro_DPS280.cpp | 4 +- libraries/AP_Baro/AP_Baro_DPS280.h | 5 +- libraries/AP_Baro/AP_Baro_FBM320.cpp | 4 +- libraries/AP_Baro/AP_Baro_FBM320.h | 4 +- libraries/AP_Baro/AP_Baro_LPS2XH.cpp | 4 +- libraries/AP_Baro/AP_Baro_LPS2XH.h | 4 +- libraries/AP_Baro/AP_Baro_MS5611.cpp | 4 +- libraries/AP_Baro/AP_Baro_MS5611.h | 4 +- libraries/AP_Baro/AP_Baro_SPL06.cpp | 4 +- libraries/AP_Baro/AP_Baro_SPL06.h | 4 +- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 52 ++++---- libraries/AP_BattMonitor/AP_BattMonitor.h | 12 +- .../AP_BattMonitor_AD7091R5.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_AD7091R5.h | 4 +- .../AP_BattMonitor/AP_BattMonitor_Analog.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_Backend.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_Backend.h | 8 +- .../AP_BattMonitor/AP_BattMonitor_Bebop.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_EFI.cpp | 4 +- libraries/AP_BattMonitor/AP_BattMonitor_EFI.h | 3 +- .../AP_BattMonitor_FuelFlow.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_FuelFlow.h | 4 +- .../AP_BattMonitor_FuelLevel_Analog.cpp | 4 +- .../AP_BattMonitor_FuelLevel_Analog.h | 4 +- .../AP_BattMonitor_FuelLevel_PWM.cpp | 4 +- .../AP_BattMonitor_FuelLevel_PWM.h | 4 +- .../AP_BattMonitor/AP_BattMonitor_INA239.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_INA239.h | 2 +- .../AP_BattMonitor/AP_BattMonitor_INA2xx.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_INA2xx.h | 4 +- .../AP_BattMonitor/AP_BattMonitor_LTC2946.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_LTC2946.h | 2 +- .../AP_BattMonitor_Scripting.cpp | 4 +- .../AP_BattMonitor/AP_BattMonitor_Scripting.h | 4 +- .../AP_BattMonitor/AP_BattMonitor_Sum.cpp | 4 +- libraries/AP_BattMonitor/AP_BattMonitor_Sum.h | 4 +- .../AP_BattMonitor_Synthetic_Current.cpp | 4 +- .../AP_BattMonitor_Synthetic_Current.h | 3 +- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 12 +- libraries/AP_BoardConfig/AP_BoardConfig.h | 12 +- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 10 +- libraries/AP_Filesystem/AP_Filesystem.cpp | 32 ++--- .../AP_Filesystem/AP_Filesystem_Mission.cpp | 20 ++- .../AP_Filesystem/AP_Filesystem_Mission.h | 4 +- .../AP_Filesystem/AP_Filesystem_Param.cpp | 4 +- libraries/AP_Filesystem/AP_Filesystem_Param.h | 4 +- .../AP_Filesystem/AP_Filesystem_ROMFS.cpp | 4 +- libraries/AP_Filesystem/AP_Filesystem_ROMFS.h | 4 +- libraries/AP_Filesystem/AP_Filesystem_Sys.cpp | 4 +- libraries/AP_Filesystem/AP_Filesystem_Sys.h | 4 +- libraries/AP_GPS/AP_GPS.cpp | 4 +- libraries/AP_HAL_ChibiOS/RCInput.cpp | 16 +-- libraries/AP_HAL_ChibiOS/Util.cpp | 4 +- libraries/AP_HAL_ESP32/RCInput.cpp | 8 +- libraries/AP_Hott_Telem/AP_Hott_Telem.cpp | 4 +- libraries/AP_Logger/AP_Logger_File.cpp | 12 +- libraries/AP_Logger/LogFile.cpp | 8 +- libraries/AP_Logger/LoggerMessageWriter.cpp | 12 +- libraries/AP_Logger/LoggerMessageWriter.h | 16 +-- libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 16 +-- libraries/AP_Mission/AP_Mission.cpp | 12 +- libraries/AP_Mission/AP_Mission.h | 8 +- .../AP_Mission/AP_Mission_ChangeDetector.cpp | 4 +- .../AP_Mission/AP_Mission_ChangeDetector.h | 4 +- libraries/AP_Mission/AP_Mission_Commands.cpp | 8 +- libraries/AP_Mount/AP_Mount_Siyi.cpp | 4 +- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 10 +- libraries/AP_Mount/AP_Mount_Xacti.cpp | 20 +-- libraries/AP_NMEA_Output/AP_NMEA_Output.cpp | 8 +- libraries/AP_OSD/AP_OSD.cpp | 4 +- libraries/AP_OSD/AP_OSD.h | 4 +- libraries/AP_OSD/AP_OSD_Screen.cpp | 8 +- libraries/AP_OpenDroneID/AP_OpenDroneID.cpp | 3 +- libraries/AP_OpenDroneID/AP_OpenDroneID.h | 4 +- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 32 ++--- libraries/AP_RCProtocol/AP_RCProtocol.h | 16 +-- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 4 +- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 4 +- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 8 +- .../AP_RCProtocol_MAVLinkRadio.cpp | 5 +- .../AP_RCProtocol_MAVLinkRadio.h | 5 +- .../AP_RCTelemetry/AP_Spektrum_Telem.cpp | 14 +-- libraries/AP_RTC/AP_RTC.cpp | 4 +- libraries/AP_RTC/AP_RTC.h | 4 +- libraries/AP_Scripting/AP_Scripting.cpp | 4 +- libraries/AP_Scripting/AP_Scripting.h | 4 +- libraries/AP_Scripting/lua_bindings.cpp | 4 +- .../AP_SerialManager/AP_SerialManager.cpp | 4 +- libraries/AP_Stats/AP_Stats.cpp | 4 +- .../AP_TemperatureSensor.cpp | 4 +- .../AP_TemperatureSensor.h | 4 +- .../AP_TemperatureSensor_Backend.cpp | 4 +- .../AP_TemperatureSensor_Backend.h | 4 +- .../AP_TemperatureSensor_Params.cpp | 4 +- libraries/AP_Terrain/TerrainMission.cpp | 4 +- libraries/AP_Tuning/AP_Tuning.cpp | 4 +- libraries/AP_Tuning/AP_Tuning.h | 4 +- libraries/AP_Vehicle/AP_Vehicle.cpp | 28 ++--- libraries/AP_Vehicle/AP_Vehicle.h | 8 +- libraries/GCS_MAVLink/GCS.cpp | 8 +- libraries/GCS_MAVLink/GCS.h | 4 +- libraries/GCS_MAVLink/GCS_Common.cpp | 116 +++++++++--------- libraries/RC_Channel/RC_Channel.cpp | 12 +- libraries/RC_Channel/RC_Channel.h | 4 +- libraries/RC_Channel/RC_Channels.cpp | 4 +- libraries/RC_Channel/RC_Channels_VarInfo.h | 4 +- libraries/SRV_Channel/SRV_Channel_aux.cpp | 8 +- libraries/StorageManager/StorageManager.cpp | 16 ++- libraries/StorageManager/StorageManager.h | 4 +- 202 files changed, 1006 insertions(+), 1182 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 3d46be9b983b96..08a1c3231990e2 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -247,7 +247,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) #endif // HELI_FRAME // checks when using range finder for RTL -#if MODE_RTL_ENABLED == ENABLED + if (copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::RTL_ALTTYPE_TERRAIN) { // get terrain source from wpnav const char *failure_template = "RTL_ALT_TYPE is above-terrain but %s"; @@ -272,7 +272,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) break; } } -#endif + // check adsb avoidance failsafe #if HAL_ADSB_ENABLED @@ -734,9 +734,9 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ copter.update_super_simple_bearing(false); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point -#if MODE_SMARTRTL_ENABLED == ENABLED + copter.g2.smart_rtl.set_home(copter.position_ok()); -#endif + hal.util->set_soft_armed(true); @@ -814,14 +814,14 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che } } -#if AUTOTUNE_ENABLED == ENABLED + // save auto tuned parameters if (copter.flightmode == &copter.mode_autotune) { copter.mode_autotune.save_tuning_gains(); } else { copter.mode_autotune.reset(); } -#endif + // we are not in the air copter.set_land_complete(true); @@ -830,10 +830,10 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che // send disarm command to motors copter.motors->armed(false); -#if MODE_AUTO_ENABLED == ENABLED + // reset the mission copter.mode_auto.mission.reset(); -#endif + #if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(false); diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index b34577af284568..f3ab08976bd2b9 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -164,9 +164,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif SCHED_TASK(auto_disarm_check, 10, 50, 27), SCHED_TASK(auto_trim, 10, 75, 30), -#if RANGEFINDER_ENABLED == ENABLED + SCHED_TASK(read_rangefinder, 20, 100, 33), -#endif + #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36), #endif @@ -176,9 +176,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(update_altitude, 10, 100, 42), SCHED_TASK(run_nav_updates, 50, 100, 45), SCHED_TASK(update_throttle_hover,100, 90, 48), -#if MODE_SMARTRTL_ENABLED == ENABLED + SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51), -#endif + #if HAL_SPRAYER_ENABLED SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54), #endif @@ -274,7 +274,7 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; -#if MODE_GUIDED_ENABLED == ENABLED + // start takeoff to given altitude (for use by scripting) bool Copter::start_takeoff(float alt) { @@ -385,9 +385,9 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false); return true; } -#endif -#if MODE_CIRCLE_ENABLED == ENABLED + + // circle mode controls bool Copter::get_circle_radius(float &radius_m) { @@ -400,7 +400,7 @@ bool Copter::set_circle_rate(float rate_dps) circle_nav->set_rate(rate_dps); return true; } -#endif + // set desired speed (m/s). Used for scripting. bool Copter::set_desired_speed(float speed) @@ -408,7 +408,7 @@ bool Copter::set_desired_speed(float speed) return flightmode->set_speed_xy(speed * 100.0f); } -#if MODE_AUTO_ENABLED == ENABLED + // returns true if mode supports NAV_SCRIPT_TIME mission commands bool Copter::nav_scripting_enable(uint8_t mode) { @@ -434,7 +434,7 @@ void Copter::nav_script_time_done(uint16_t id) return mode_auto.nav_script_time_done(id); } -#endif + // returns true if the EKF failsafe has triggered. Only used by Lua scripts bool Copter::has_ekf_failsafed() const @@ -458,11 +458,11 @@ bool Copter::is_taking_off() const bool Copter::current_mode_requires_mission() const { -#if MODE_AUTO_ENABLED == ENABLED + return flightmode == &mode_auto; -#else - return false; -#endif + + + } // rc_loops - reads user input from transmitter/receiver diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0dc0ef263e38d1..8ab284e13d8e2f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -114,15 +114,15 @@ # include # include -#if MODE_FOLLOW_ENABLED == ENABLED + # include -#endif + # include -#if RANGEFINDER_ENABLED == ENABLED + # include -#endif + #include @@ -490,9 +490,9 @@ class Copter : public AP_Vehicle { AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()}; #endif -#if MODE_CIRCLE_ENABLED == ENABLED + AC_Circle *circle_nav; -#endif + // System Timers // -------------- @@ -528,9 +528,9 @@ class Copter : public AP_Vehicle { #endif // Parachute release -#if PARACHUTE == ENABLED + AP_Parachute parachute; -#endif + // Landing Gear Controller @@ -667,7 +667,7 @@ class Copter : public AP_Vehicle { uint8_t &task_count, uint32_t &log_bit) override; -#if MODE_GUIDED_ENABLED == ENABLED + bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override; @@ -676,17 +676,17 @@ class Copter : public AP_Vehicle { bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override; bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; -#endif -#if MODE_CIRCLE_ENABLED == ENABLED + + bool get_circle_radius(float &radius_m) override; bool set_circle_rate(float rate_dps) override; -#endif + bool set_desired_speed(float speed) override; -#if MODE_AUTO_ENABLED == ENABLED + bool nav_scripting_enable(uint8_t mode) override; bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; -#endif + // lua scripts use this to retrieve EKF failsafe state // returns true if the EKF failsafe has triggered bool has_ekf_failsafed() const override; @@ -971,83 +971,83 @@ class Copter : public AP_Vehicle { void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag); void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag); -#if MODE_ACRO_ENABLED == ENABLED + #if FRAME_CONFIG == HELI_FRAME ModeAcro_Heli mode_acro; #else ModeAcro mode_acro; #endif -#endif + ModeAltHold mode_althold; -#if MODE_AUTO_ENABLED == ENABLED + ModeAuto mode_auto; -#endif -#if AUTOTUNE_ENABLED == ENABLED + + ModeAutoTune mode_autotune; -#endif -#if MODE_BRAKE_ENABLED == ENABLED + + ModeBrake mode_brake; -#endif -#if MODE_CIRCLE_ENABLED == ENABLED + + ModeCircle mode_circle; -#endif -#if MODE_DRIFT_ENABLED == ENABLED + + ModeDrift mode_drift; -#endif -#if MODE_FLIP_ENABLED == ENABLED + + ModeFlip mode_flip; -#endif -#if MODE_FOLLOW_ENABLED == ENABLED + + ModeFollow mode_follow; -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + ModeGuided mode_guided; -#endif + ModeLand mode_land; -#if MODE_LOITER_ENABLED == ENABLED + ModeLoiter mode_loiter; -#endif -#if MODE_POSHOLD_ENABLED == ENABLED + + ModePosHold mode_poshold; -#endif -#if MODE_RTL_ENABLED == ENABLED + + ModeRTL mode_rtl; -#endif + #if FRAME_CONFIG == HELI_FRAME ModeStabilize_Heli mode_stabilize; #else ModeStabilize mode_stabilize; #endif -#if MODE_SPORT_ENABLED == ENABLED + ModeSport mode_sport; -#endif + #if MODE_SYSTEMID_ENABLED == ENABLED ModeSystemId mode_systemid; #endif #if HAL_ADSB_ENABLED ModeAvoidADSB mode_avoid_adsb; #endif -#if MODE_THROW_ENABLED == ENABLED + ModeThrow mode_throw; -#endif -#if MODE_GUIDED_NOGPS_ENABLED == ENABLED + + ModeGuidedNoGPS mode_guided_nogps; -#endif -#if MODE_SMARTRTL_ENABLED == ENABLED + + ModeSmartRTL mode_smartrtl; -#endif -#if MODE_FLOWHOLD_ENABLED == ENABLED + + ModeFlowHold mode_flowhold; -#endif -#if MODE_ZIGZAG_ENABLED == ENABLED + + ModeZigZag mode_zigzag; -#endif + #if MODE_AUTOROTATE_ENABLED == ENABLED ModeAutorotate mode_autorotate; #endif -#if MODE_TURTLE_ENABLED == ENABLED + ModeTurtle mode_turtle; -#endif + // mode.cpp Mode *mode_from_mode_num(const Mode::Number mode); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 0dd1dbb50886e6..e255e071e6c90d 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -91,7 +91,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if RANGEFINDER_ENABLED == ENABLED + const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; @@ -102,7 +102,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } -#endif + if (copter.precland.enabled()) { diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 36c72f0aeb18ef..c80171cbf2d768 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -151,7 +151,7 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() void GCS_MAVLINK_Copter::send_position_target_local_ned() { -#if MODE_GUIDED_ENABLED == ENABLED + if (!copter.flightmode->in_guided_mode()) { return; } @@ -210,7 +210,7 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() -target_accel.z,// afz in m/s/s NED frame 0.0f, // yaw 0.0f); // yaw_rate -#endif + } void GCS_MAVLINK_Copter::send_nav_controller_output() const @@ -615,11 +615,11 @@ MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission) bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd) { -#if MODE_AUTO_ENABLED == ENABLED + return copter.mode_auto.do_guided(cmd); -#else - return false; -#endif + + + } void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status, @@ -632,10 +632,10 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status, copter.avoidance_adsb.handle_msg(msg); } #endif -#if MODE_FOLLOW_ENABLED == ENABLED + // pass message to follow library copter.g2.follow.handle_msg(msg); -#endif + GCS_MAVLINK::packetReceived(status, msg); } @@ -720,7 +720,7 @@ bool GCS_MAVLINK_Copter::set_home(const Location& loc, bool _lock) { MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { -#if MODE_GUIDED_ENABLED == ENABLED + const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; if (!copter.flightmode->in_guided_mode() && !change_modes) { return MAV_RESULT_DENIED; @@ -757,9 +757,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_co } return MAV_RESULT_ACCEPTED; -#else - return MAV_RESULT_UNSUPPORTED; -#endif + + + } MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) @@ -772,7 +772,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i case MAV_CMD_DO_CHANGE_SPEED: return handle_MAV_CMD_DO_CHANGE_SPEED(packet); -#if MODE_FOLLOW_ENABLED == ENABLED + case MAV_CMD_DO_FOLLOW: // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { @@ -780,7 +780,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i return MAV_RESULT_ACCEPTED; } return MAV_RESULT_DENIED; -#endif + case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); @@ -796,10 +796,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i case MAV_CMD_NAV_VTOL_TAKEOFF: return handle_MAV_CMD_NAV_TAKEOFF(packet); -#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: return handle_MAV_CMD_DO_PARACHUTE(packet); -#endif + #if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED // Solo user presses pause button @@ -813,10 +813,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i return handle_MAV_CMD_SOLO_BTN_FLY_CLICK(packet); #endif -#if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_MISSION_START: return handle_MAV_CMD_MISSION_START(packet); -#endif + case MAV_CMD_DO_WINCH: @@ -842,13 +842,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i } return MAV_RESULT_ACCEPTED; -#if MODE_AUTO_ENABLED == ENABLED + case MAV_CMD_DO_LAND_START: if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; -#endif + default: return GCS_MAVLINK::handle_command_int_packet(packet, msg); @@ -952,7 +952,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_comm return MAV_RESULT_FAILED; } -#if MODE_AUTO_ENABLED == ENABLED + MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) { if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { @@ -964,11 +964,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_comman } return MAV_RESULT_FAILED; } -#endif -#if PARACHUTE == ENABLED + + MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) { // configure or release parachute @@ -986,7 +986,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command } return MAV_RESULT_FAILED; } -#endif + MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) { @@ -1081,15 +1081,15 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink bool shot_mode = (!is_zero(packet.param1) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS)); if (!shot_mode) { -#if MODE_BRAKE_ENABLED == ENABLED + if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) { copter.mode_brake.timeout_to_loiter_ms(2500); } else { copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); } -#else - copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND); -#endif + + + } else { // SoloLink is expected to handle pause in shots } @@ -1171,7 +1171,7 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) { -#if MODE_GUIDED_ENABLED == ENABLED + // for mavlink SET_POSITION_TARGET messages constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = POSITION_TARGET_TYPEMASK_X_IGNORE | @@ -1194,11 +1194,11 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_FORCE_SET = POSITION_TARGET_TYPEMASK_FORCE_SET; -#endif + switch (msg.msgid) { -#if MODE_GUIDED_ENABLED == ENABLED + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { // decode packet @@ -1485,7 +1485,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) break; } -#endif + case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 86553bdc469cd8..be5fed64649bc9 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -97,7 +97,7 @@ const AP_Param::Info Copter::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), -#if MODE_RTL_ENABLED == ENABLED + // @Param: RTL_ALT // @DisplayName: RTL Altitude // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude. @@ -158,7 +158,7 @@ const AP_Param::Info Copter::var_info[] = { // @Values: 0:Relative to Home, 1:Terrain // @User: Standard GSCALAR(rtl_alt_type, "RTL_ALT_TYPE", 0), -#endif + // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable @@ -357,7 +357,7 @@ const AP_Param::Info Copter::var_info[] = { // @User: Advanced ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), -#if MODE_POSHOLD_ENABLED == ENABLED + // @Param: PHLD_BRAKE_RATE // @DisplayName: PosHold braking rate // @Description: PosHold flight mode's rotation rate during braking in deg/sec @@ -374,7 +374,7 @@ const AP_Param::Info Copter::var_info[] = { // @Range: 2000 4500 // @User: Advanced GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE", POSHOLD_BRAKE_ANGLE_DEFAULT), -#endif + // @Param: LAND_REPOSITION // @DisplayName: Land repositioning @@ -433,14 +433,14 @@ const AP_Param::Info Copter::var_info[] = { // ACRO_RP_EXPO moved to Command Model class -#if MODE_ACRO_ENABLED == ENABLED + // @Param: ACRO_TRAINER // @DisplayName: Acro Trainer // @Description: Type of trainer used in acro mode // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited // @User: Advanced GSCALAR(acro_trainer, "ACRO_TRAINER", (uint8_t)ModeAcro::Trainer::LIMITED), -#endif + // variables not in the g class which contain EEPROM saved variables @@ -456,11 +456,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(relay, "RELAY", AP_Relay), -#if PARACHUTE == ENABLED + // @Group: CHUTE_ // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp GOBJECT(parachute, "CHUTE_", AP_Parachute), -#endif + // @Group: LGR_ @@ -490,11 +490,11 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter), -#if MODE_CIRCLE_ENABLED == ENABLED + // @Group: CIRCLE_ // @Path: ../libraries/AC_WPNav/AC_Circle.cpp GOBJECTPTR(circle_nav, "CIRCLE_", AC_Circle), -#endif + // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -631,21 +631,21 @@ const AP_Param::Info Copter::var_info[] = { GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), #endif -#if MODE_AUTO_ENABLED == ENABLED + // @Group: MIS_ // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), -#endif + // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), -#if RANGEFINDER_ENABLED == ENABLED + // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), -#endif + // @Group: TERRAIN_ @@ -685,7 +685,7 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_Notify/AP_Notify.cpp GOBJECT(notify, "NTF_", AP_Notify), -#if MODE_THROW_ENABLED == ENABLED + // @Param: THROW_MOT_START // @DisplayName: Start motors before throwing is detected // @Description: Used by Throw mode. Controls whether motors will run at the speed set by MOT_SPIN_MIN or will be stopped when armed and waiting for the throw. @@ -706,7 +706,7 @@ const AP_Param::Info Copter::var_info[] = { // @Units: m // @User: Advanced GSCALAR(throw_altitude_max, "THROW_ALT_MAX", 0), -#endif + #if OSD_ENABLED || OSD_PARAM_ENABLED // @Group: OSD @@ -749,7 +749,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPPTR(button_ptr, "BTN_", 2, ParametersG2, AP_Button), #endif -#if MODE_THROW_ENABLED == ENABLED + // @Param: THROW_NEXTMODE // @DisplayName: Throw mode's follow up mode // @Description: Vehicle will switch to this mode after the throw is successfully completed. Default is to stay in throw mode (18) @@ -763,7 +763,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Values: 0:Upward Throw,1:Drop // @User: Standard AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, (float)ModeThrow::ThrowType::Upward), -#endif + // @Param: GND_EFFECT_COMP // @DisplayName: Ground Effect Compensation Enable/Disable @@ -799,14 +799,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // ACRO_Y_EXPO (9) moved to Command Model Class -#if MODE_ACRO_ENABLED == ENABLED + // @Param: ACRO_THR_MID // @DisplayName: Acro Thr Mid // @Description: Acro Throttle Mid // @Range: 0 1 // @User: Advanced AP_GROUPINFO("ACRO_THR_MID", 10, ParametersG2, acro_thr_mid, ACRO_THR_MID_DEFAULT), -#endif + // @Param: SYSID_ENFORCE // @DisplayName: GCS sysid enforcement @@ -849,11 +849,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode), #endif -#if MODE_SMARTRTL_ENABLED == ENABLED + // @Group: SRTL_ // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), -#endif + // 22 was AP_WheelEncoder @@ -881,27 +881,27 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), -#if MODE_FLOWHOLD_ENABLED == ENABLED + // @Group: FHLD // @Path: mode_flowhold.cpp AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold), -#endif -#if MODE_FOLLOW_ENABLED == ENABLED + + // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), -#endif + #if USER_PARAMS_ENABLED == ENABLED AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters), #endif -#if AUTOTUNE_ENABLED == ENABLED + // @Group: AUTOTUNE_ // @Path: ../libraries/AC_AutoTune/AC_AutoTune_Multi.cpp,../libraries/AC_AutoTune/AC_AutoTune_Heli.cpp AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune), -#endif + // 30 was AP_Scripting @@ -950,38 +950,38 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation), #endif -#if MODE_ZIGZAG_ENABLED == ENABLED + // @Group: ZIGZ_ // @Path: mode_zigzag.cpp AP_SUBGROUPPTR(mode_zigzag_ptr, "ZIGZ_", 38, ParametersG2, ModeZigZag), -#endif -#if MODE_ACRO_ENABLED == ENABLED + + // @Param: ACRO_OPTIONS // @DisplayName: Acro mode options // @Description: A range of options that can be applied to change acro mode behaviour. Air-mode enables ATC_THR_MIX_MAN at all times (air-mode has no effect on helicopters). Rate Loop Only disables the use of angle stabilization and uses angular rate stabilization only. // @Bitmask: 0:Air-mode,1:Rate Loop Only // @User: Advanced AP_GROUPINFO("ACRO_OPTIONS", 39, ParametersG2, acro_options, 0), -#endif -#if MODE_AUTO_ENABLED == ENABLED + + // @Param: AUTO_OPTIONS // @DisplayName: Auto mode options // @Description: A range of options that can be applied to change auto mode behaviour. Allow Arming allows the copter to be armed in Auto. Allow Takeoff Without Raising Throttle allows takeoff without the pilot having to raise the throttle. Ignore pilot yaw overrides the pilot's yaw stick being used while in auto. // @Bitmask: 0:Allow Arming,1:Allow Takeoff Without Raising Throttle,2:Ignore pilot yaw,7:Allow weathervaning // @User: Advanced AP_GROUPINFO("AUTO_OPTIONS", 40, ParametersG2, auto_options, 0), -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + // @Param: GUID_OPTIONS // @DisplayName: Guided mode options // @Description: Options that can be applied to change guided mode behaviour // @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY,6:Waypoint navigation used for position targets,7:Allow weathervaning // @User: Advanced AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0), -#endif + // @Param: FS_GCS_TIMEOUT // @DisplayName: GCS failsafe timeout @@ -992,14 +992,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5), -#if MODE_RTL_ENABLED == ENABLED + // @Param: RTL_OPTIONS // @DisplayName: RTL mode options // @Description: Options that can be applied to change RTL mode behaviour // @Bitmask: 2:Ignore pilot yaw // @User: Advanced AP_GROUPINFO("RTL_OPTIONS", 43, ParametersG2, rtl_options, 0), -#endif + // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options @@ -1008,7 +1008,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0), -#if RANGEFINDER_ENABLED == ENABLED + // @Param: RNGFND_FILT // @DisplayName: Rangefinder filter // @Description: Rangefinder filter to smooth distance. Set to zero to disable filtering @@ -1018,9 +1018,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard // @RebootRequired: True AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT), -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + // @Param: GUID_TIMEOUT // @DisplayName: Guided mode timeout // @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller. Only applicable during any combination of velocity, acceleration, angle control, and/or angular rate control @@ -1028,7 +1028,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Range: 0.1 5 // @User: Advanced AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0), -#endif + // ACRO_PR_RATE (47), ACRO_Y_RATE (48), PILOT_Y_RATE (49) and PILOT_Y_EXPO (50) moved to command model class @@ -1147,11 +1147,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("TKOFF_RPM_MIN", 58, ParametersG2, takeoff_rpm_min, 0), #endif -#if WEATHERVANE_ENABLED == ENABLED + // @Group: WVANE_ // @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp AP_SUBGROUPINFO(weathervane, "WVANE_", 59, ParametersG2, AC_WeatherVane), -#endif + // ID 60 is reserved for the SHIP_OPS @@ -1254,21 +1254,21 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED ,afs() #endif -#if MODE_SMARTRTL_ENABLED == ENABLED + ,smart_rtl() -#endif -#if MODE_FLOWHOLD_ENABLED == ENABLED + + ,mode_flowhold_ptr(&copter.mode_flowhold) -#endif -#if MODE_FOLLOW_ENABLED == ENABLED + + ,follow() -#endif + #if USER_PARAMS_ENABLED == ENABLED ,user_parameters() #endif -#if AUTOTUNE_ENABLED == ENABLED + ,autotune_ptr(&copter.mode_autotune.autotune) -#endif + #if MODE_SYSTEMID_ENABLED == ENABLED ,mode_systemid_ptr(&copter.mode_systemid) #endif @@ -1278,9 +1278,9 @@ ParametersG2::ParametersG2(void) #if HAL_BUTTON_ENABLED ,button_ptr(&copter.button) #endif -#if MODE_ZIGZAG_ENABLED == ENABLED + ,mode_zigzag_ptr(&copter.mode_zigzag) -#endif + #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED ,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f) @@ -1290,9 +1290,9 @@ ParametersG2::ParametersG2(void) ,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f) #endif -#if WEATHERVANE_ENABLED == ENABLED + ,weathervane() -#endif + { AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info2); @@ -1345,10 +1345,10 @@ void Copter::load_parameters(void) convert_lgr_parameters(); -#if MODE_RTL_ENABLED == ENABLED + // PARAMETER_CONVERSION - Added: Sep-2021 g.rtl_altitude.convert_parameter_width(AP_PARAM_INT16); -#endif + // PARAMETER_CONVERSION - Added: Mar-2022 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0a7ba777c1d3b7..a89bdd38014087 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -6,12 +6,12 @@ #include "RC_Channel.h" #include -#if MODE_FOLLOW_ENABLED == ENABLED + # include -#endif -#if WEATHERVANE_ENABLED == ENABLED + + #include -#endif + // Global parameter class. // @@ -398,7 +398,7 @@ class Parameters { AP_Int16 throttle_behavior; AP_Float pilot_takeoff_alt; -#if MODE_RTL_ENABLED == ENABLED + AP_Int32 rtl_altitude; AP_Int16 rtl_speed_cms; AP_Float rtl_cone_slope; @@ -406,7 +406,7 @@ class Parameters { AP_Int16 rtl_climb_min; // rtl minimum climb in cm AP_Int32 rtl_loiter_time; AP_Enum rtl_alt_type; -#endif + AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position @@ -415,10 +415,10 @@ class Parameters { AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions -#if MODE_POSHOLD_ENABLED == ENABLED + AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees -#endif + // Waypoints // @@ -459,11 +459,11 @@ class Parameters { AP_Float fs_ekf_thresh; AP_Int16 gcs_pid_mask; -#if MODE_THROW_ENABLED == ENABLED + AP_Enum throw_motor_start; AP_Int16 throw_altitude_min; // minimum altitude in m above which a throw can be detected AP_Int16 throw_altitude_max; // maximum altitude in m below which a throw can be detected -#endif + AP_Int16 rc_speed; // speed of fast RC Channels in Hz @@ -473,10 +473,10 @@ class Parameters { AP_Float acro_balance_pitch; #endif -#if MODE_ACRO_ENABLED == ENABLED + // Acro parameters AP_Int8 acro_trainer; -#endif + // Note: keep initializers here in the same order as they are declared // above. @@ -504,11 +504,11 @@ class ParametersG2 { AP_Button *button_ptr; #endif -#if MODE_THROW_ENABLED == ENABLED + // Throw mode parameters AP_Int8 throw_nextmode; AP_Enum throw_type; -#endif + // ground effect compensation enable/disable AP_Int8 gndeffect_comp_enabled; @@ -539,9 +539,9 @@ class ParametersG2 { // developer options AP_Int32 dev_options; -#if MODE_ACRO_ENABLED == ENABLED + AP_Float acro_thr_mid; -#endif + // frame class AP_Int8 frame_class; @@ -552,10 +552,10 @@ class ParametersG2 { // control over servo output ranges SRV_Channels servo_channels; -#if MODE_SMARTRTL_ENABLED == ENABLED + // Safe RTL library AP_SmartRTL smart_rtl; -#endif + // wheel encoder and winch @@ -572,25 +572,25 @@ class ParametersG2 { ToyMode toy_mode; #endif -#if MODE_FLOWHOLD_ENABLED + // we need a pointer to the mode for the G2 table void *mode_flowhold_ptr; -#endif -#if MODE_FOLLOW_ENABLED == ENABLED + + // follow AP_Follow follow; -#endif + #if USER_PARAMS_ENABLED == ENABLED // User custom parameters UserParameters user_parameters; #endif -#if AUTOTUNE_ENABLED == ENABLED + // we need a pointer to autotune for the G2 table void *autotune_ptr; -#endif + AP_Float tuning_min; AP_Float tuning_max; @@ -616,10 +616,10 @@ class ParametersG2 { AC_Autorotation arot; #endif -#if MODE_ZIGZAG_ENABLED == ENABLED + // we need a pointer to the mode for the G2 table void *mode_zigzag_ptr; -#endif + // command model parameters #if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED @@ -632,33 +632,33 @@ class ParametersG2 { AC_CommandModel command_model_pilot; -#if MODE_ACRO_ENABLED == ENABLED + AP_Int8 acro_options; -#endif -#if MODE_AUTO_ENABLED == ENABLED + + AP_Int32 auto_options; -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + AP_Int32 guided_options; -#endif + AP_Float fs_gcs_timeout; -#if MODE_RTL_ENABLED == ENABLED + AP_Int32 rtl_options; -#endif + AP_Int32 flight_options; -#if RANGEFINDER_ENABLED == ENABLED + AP_Float rangefinder_filt; -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + AP_Float guided_timeout; -#endif + AP_Int8 surftrak_mode; AP_Int8 failsafe_dr_enable; @@ -676,9 +676,9 @@ class ParametersG2 { // EKF variance filter cutoff AP_Float fs_ekf_filt_hz; -#if WEATHERVANE_ENABLED == ENABLED + AC_WeatherVane weathervane; -#endif + // payload place parameters AP_Float pldp_thrust_placed_fraction; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index be99ead698fb6d..cd7f0f85aaf53b 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -188,9 +188,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc } case AUX_FUNC::RTL: -#if MODE_RTL_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::RTL, ch_flag); -#endif + break; case AUX_FUNC::SAVE_TRIM: @@ -202,7 +202,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::SAVE_WP: -#if MODE_AUTO_ENABLED == ENABLED + // save waypoint when switch is brought high if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) { @@ -250,29 +250,29 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP); } } -#endif + break; case AUX_FUNC::AUTO: -#if MODE_AUTO_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::AUTO, ch_flag); -#endif + break; case AUX_FUNC::RANGEFINDER: // enable or disable the rangefinder -#if RANGEFINDER_ENABLED == ENABLED + if ((ch_flag == AuxSwitchPos::HIGH) && copter.rangefinder.has_orientation(ROTATION_PITCH_270)) { copter.rangefinder_state.enabled = true; } else { copter.rangefinder_state.enabled = false; } -#endif + break; case AUX_FUNC::ACRO_TRAINER: -#if MODE_ACRO_ENABLED == ENABLED + switch(ch_flag) { case AuxSwitchPos::LOW: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF); @@ -287,13 +287,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED); break; } -#endif + break; case AUX_FUNC::AUTOTUNE: -#if AUTOTUNE_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag); -#endif + break; case AUX_FUNC::LAND: @@ -313,22 +313,22 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::PARACHUTE_ENABLE: -#if PARACHUTE == ENABLED + // Parachute enable/disable copter.parachute.enabled(ch_flag == AuxSwitchPos::HIGH); -#endif + break; case AUX_FUNC::PARACHUTE_RELEASE: -#if PARACHUTE == ENABLED + if (ch_flag == AuxSwitchPos::HIGH) { copter.parachute_manual_release(); } -#endif + break; case AUX_FUNC::PARACHUTE_3POS: -#if PARACHUTE == ENABLED + // Parachute disable, enable, release with 3 position switch switch (ch_flag) { case AuxSwitchPos::LOW: @@ -342,7 +342,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc copter.parachute_manual_release(); break; } -#endif + break; case AUX_FUNC::ATTCON_FEEDFWD: @@ -384,15 +384,15 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::BRAKE: -#if MODE_BRAKE_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::BRAKE, ch_flag); -#endif + break; case AUX_FUNC::THROW: -#if MODE_THROW_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::THROW, ch_flag); -#endif + break; case AUX_FUNC::PRECISION_LOITER: @@ -412,9 +412,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::SMART_RTL: -#if MODE_SMARTRTL_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::SMART_RTL, ch_flag); -#endif + break; case AUX_FUNC::INVERTED: @@ -472,13 +472,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc #endif case AUX_FUNC::ZIGZAG: -#if MODE_ZIGZAG_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::ZIGZAG, ch_flag); -#endif + break; case AUX_FUNC::ZIGZAG_SaveWP: -#if MODE_ZIGZAG_ENABLED == ENABLED + if (copter.flightmode == &copter.mode_zigzag) { // initialize zigzag auto copter.mode_zigzag.init_auto(); @@ -494,7 +494,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } } -#endif + break; case AUX_FUNC::STABILIZE: @@ -502,9 +502,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::POSHOLD: -#if MODE_POSHOLD_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag); -#endif + break; case AUX_FUNC::ALTHOLD: @@ -513,27 +513,27 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc case AUX_FUNC::ACRO: -#if MODE_ACRO_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::ACRO, ch_flag); -#endif + break; case AUX_FUNC::FLOWHOLD: -#if MODE_FLOWHOLD_ENABLED + do_aux_function_change_mode(Mode::Number::FLOWHOLD, ch_flag); -#endif + break; case AUX_FUNC::CIRCLE: -#if MODE_CIRCLE_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag); -#endif + break; case AUX_FUNC::DRIFT: -#if MODE_DRIFT_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::DRIFT, ch_flag); -#endif + break; case AUX_FUNC::STANDBY: { @@ -567,7 +567,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::ZIGZAG_Auto: -#if MODE_ZIGZAG_ENABLED == ENABLED + if (copter.flightmode == &copter.mode_zigzag) { switch (ch_flag) { case AuxSwitchPos::HIGH: @@ -578,7 +578,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } } -#endif + break; case AUX_FUNC::AIRMODE: @@ -593,15 +593,15 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; case AUX_FUNC::AUTO_RTL: -#if MODE_AUTO_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag); -#endif + break; case AUX_FUNC::TURTLE: -#if MODE_TURTLE_ENABLED == ENABLED + do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag); -#endif + break; case AUX_FUNC::SIMPLE_HEADING_RESET: @@ -624,7 +624,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; #endif -#if WEATHERVANE_ENABLED == ENABLED + case AUX_FUNC::WEATHER_VANE_ENABLE: { switch (ch_flag) { case AuxSwitchPos::HIGH: @@ -638,7 +638,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc } break; } -#endif + default: return RC_Channel::do_aux_function(ch_option, ch_flag); diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 749a830f4dce65..58b1efbda27d8e 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -239,11 +239,11 @@ float Mode::AutoYaw::yaw_cd() break; case Mode::CIRCLE: -#if MODE_CIRCLE_ENABLED + if (copter.circle_nav->is_active()) { _yaw_angle_cd = copter.circle_nav->get_yaw(); } -#endif + break; case Mode::ANGLE_RATE:{ @@ -319,9 +319,9 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() auto_yaw.set_mode(AutoYaw::Mode::HOLD); } -#if WEATHERVANE_ENABLED == ENABLED + update_weathervane(_pilot_yaw_rate_cds); -#endif + AC_AttitudeControl::HeadingCommand heading; heading.yaw_angle_cd = auto_yaw.yaw_cd(); @@ -350,7 +350,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() // handle the interface to the weathervane library // pilot_yaw can be an angle or a rate or rcin from yaw channel. It just needs to represent a pilot's request to yaw the vehicle to enable pilot overrides. -#if WEATHERVANE_ENABLED == ENABLED + void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds) { if (!copter.flightmode->allows_weathervaning()) { @@ -378,4 +378,3 @@ void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds) } } } -#endif // WEATHERVANE_ENABLED == ENABLED diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index bd541c355117af..5213caa437a026 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -25,9 +25,9 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O // take no action in some flight modes if (copter.flightmode->mode_number() == Mode::Number::LAND || -#if MODE_THROW_ENABLED == ENABLED + copter.flightmode->mode_number() == Mode::Number::THROW || -#endif + copter.flightmode->mode_number() == Mode::Number::FLIP) { actual_action = MAV_COLLISION_ACTION_NONE; } @@ -148,12 +148,12 @@ void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode) int32_t AP_Avoidance_Copter::get_altitude_minimum() const { -#if MODE_RTL_ENABLED == ENABLED + // do not descend if below RTL alt return copter.g.rtl_altitude; -#else - return 0; -#endif + + + } // check flight mode is avoid_adsb diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index e5c2f152d03b10..ac0f3013b6b758 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -30,9 +30,9 @@ void Copter::set_home_to_current_location_inflight() { return; } // we have successfully set AHRS home, set it for SmartRTL -#if MODE_SMARTRTL_ENABLED == ENABLED + g2.smart_rtl.set_home(true); -#endif + } } @@ -45,9 +45,9 @@ bool Copter::set_home_to_current_location(bool lock) { return false; } // we have successfully set AHRS home, set it for SmartRTL -#if MODE_SMARTRTL_ENABLED == ENABLED + g2.smart_rtl.set_home(true); -#endif + return true; } return false; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 60c9ca34661860..fe13e3effd7d78 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -229,7 +229,7 @@ void Copter::yaw_imbalance_check() } } -#if PARACHUTE == ENABLED + // Code to detect a crash main ArduCopter code #define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute @@ -372,5 +372,3 @@ void Copter::parachute_manual_release() // if we get this far release parachute parachute_release(); } - -#endif // PARACHUTE == ENABLED diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index aa3dd55d7a5484..ac17875e87436c 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -286,10 +286,10 @@ void Copter::failsafe_terrain_on_event() if (should_disarm_on_failsafe()) { arming.disarm(AP_Arming::Method::TERRAINFAILSAFE); -#if MODE_RTL_ENABLED == ENABLED + } else if (flightmode->mode_number() == Mode::Number::RTL) { mode_rtl.restart_without_terrain(); -#endif + } else { set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE); } @@ -420,12 +420,12 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason) // This can come from failsafe or RC option void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason) { -#if MODE_AUTO_ENABLED == ENABLED + if (set_mode(Mode::Number::AUTO_RTL, reason)) { AP_Notify::events.failsafe_mode_change = 1; return; } -#endif + gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode"); set_mode_RTL_or_land_with_pause(reason); @@ -435,12 +435,12 @@ void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason) // This can come from failsafe or RC option void Copter::set_mode_brake_or_land_with_pause(ModeReason reason) { -#if MODE_BRAKE_ENABLED == ENABLED + if (set_mode(Mode::Number::BRAKE, reason)) { AP_Notify::events.failsafe_mode_change = 1; return; } -#endif + gcs().send_text(MAV_SEVERITY_WARNING, "Trying Land Mode"); set_mode_land_with_pause(reason); diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 5223d6985b78e9..59c07ff46e96e2 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -22,10 +22,10 @@ void Copter::update_land_and_crash_detectors() update_land_detector(); -#if PARACHUTE == ENABLED + // check parachute parachute_check(); -#endif + crash_check(); thrust_loss_check(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 3ab6d28be63ab0..56b09bab6b9554 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -32,14 +32,14 @@ PayloadPlace Mode::payload_place; // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) { - Mode *ret = nullptr; + Mode *ret = nullptr; //[] = {&mode_stabilize, &mode_acro, &mode_althold, &mode_auto, nullptr, &mode_loiter, nullptr, &mode_circle,}; switch (mode) { -#if MODE_ACRO_ENABLED == ENABLED + case Mode::Number::ACRO: ret = &mode_acro; break; -#endif + case Mode::Number::STABILIZE: ret = &mode_stabilize; @@ -49,81 +49,81 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) ret = &mode_althold; break; -#if MODE_AUTO_ENABLED == ENABLED + case Mode::Number::AUTO: ret = &mode_auto; break; -#endif -#if MODE_CIRCLE_ENABLED == ENABLED + + case Mode::Number::CIRCLE: ret = &mode_circle; break; -#endif -#if MODE_LOITER_ENABLED == ENABLED + + case Mode::Number::LOITER: ret = &mode_loiter; break; -#endif -#if MODE_GUIDED_ENABLED == ENABLED + + case Mode::Number::GUIDED: ret = &mode_guided; break; -#endif + case Mode::Number::LAND: ret = &mode_land; break; -#if MODE_RTL_ENABLED == ENABLED + case Mode::Number::RTL: ret = &mode_rtl; break; -#endif -#if MODE_DRIFT_ENABLED == ENABLED + + case Mode::Number::DRIFT: ret = &mode_drift; break; -#endif -#if MODE_SPORT_ENABLED == ENABLED + + case Mode::Number::SPORT: ret = &mode_sport; break; -#endif -#if MODE_FLIP_ENABLED == ENABLED + + case Mode::Number::FLIP: ret = &mode_flip; break; -#endif -#if AUTOTUNE_ENABLED == ENABLED + + case Mode::Number::AUTOTUNE: ret = &mode_autotune; break; -#endif -#if MODE_POSHOLD_ENABLED == ENABLED + + case Mode::Number::POSHOLD: ret = &mode_poshold; break; -#endif -#if MODE_BRAKE_ENABLED == ENABLED + + case Mode::Number::BRAKE: ret = &mode_brake; break; -#endif -#if MODE_THROW_ENABLED == ENABLED + + case Mode::Number::THROW: ret = &mode_throw; break; -#endif + #if HAL_ADSB_ENABLED case Mode::Number::AVOID_ADSB: @@ -131,35 +131,35 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) break; #endif -#if MODE_GUIDED_NOGPS_ENABLED == ENABLED + case Mode::Number::GUIDED_NOGPS: ret = &mode_guided_nogps; break; -#endif -#if MODE_SMARTRTL_ENABLED == ENABLED + + case Mode::Number::SMART_RTL: ret = &mode_smartrtl; break; -#endif -#if MODE_FLOWHOLD_ENABLED == ENABLED + + case Mode::Number::FLOWHOLD: ret = (Mode *)g2.mode_flowhold_ptr; break; -#endif -#if MODE_FOLLOW_ENABLED == ENABLED + + case Mode::Number::FOLLOW: ret = &mode_follow; break; -#endif -#if MODE_ZIGZAG_ENABLED == ENABLED + + case Mode::Number::ZIGZAG: ret = &mode_zigzag; break; -#endif + #if MODE_SYSTEMID_ENABLED == ENABLED case Mode::Number::SYSTEMID: @@ -167,17 +167,17 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) break; #endif -#if MODE_AUTOROTATE_ENABLED == ENABLED +#if (MODE_AUTOROTATE_ENABLED == ENABLED) && (FRAME_CONFIG == HELI_FRAME) case Mode::Number::AUTOROTATE: ret = &mode_autorotate; break; #endif -#if MODE_TURTLE_ENABLED == ENABLED + case Mode::Number::TURTLE: ret = &mode_turtle; break; -#endif + default: break; @@ -273,12 +273,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return false; } -#if MODE_AUTO_ENABLED == ENABLED + if (mode == Mode::Number::AUTO_RTL) { // Special case for AUTO RTL, not a true mode, just AUTO in disguise return mode_auto.jump_to_landing_sequence_auto_RTL(reason); } -#endif + Mode *new_flightmode = mode_from_mode_num(mode); if (new_flightmode == nullptr) { @@ -314,11 +314,11 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // (e.g. user arms in guided, raises throttle to 1300 (not enough to // trigger auto takeoff), then switches into manual): bool user_throttle = new_flightmode->has_manual_throttle(); -#if MODE_DRIFT_ENABLED == ENABLED + if (new_flightmode == &mode_drift) { user_throttle = true; } -#endif + if (!ignore_checks && ap.land_complete && user_throttle && diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b948b537540445..3094f8f002b93d 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -185,9 +185,9 @@ class Mode { virtual bool resume() { return false; }; // true if weathervaning is allowed in the current mode -#if WEATHERVANE_ENABLED == ENABLED + virtual bool allows_weathervaning() const { return false; } -#endif + protected: @@ -327,9 +327,9 @@ class Mode { bool reached_fixed_yaw_target(); -#if WEATHERVANE_ENABLED == ENABLED + void update_weathervane(const int16_t pilot_yaw_cds); -#endif + AC_AttitudeControl::HeadingCommand get_heading(); @@ -384,7 +384,7 @@ class Mode { }; -#if MODE_ACRO_ENABLED == ENABLED + class ModeAcro : public Mode { public: @@ -430,7 +430,7 @@ class ModeAcro : public Mode { private: bool disable_air_mode_reset; }; -#endif + #if FRAME_CONFIG == HELI_FRAME class ModeAcro_Heli : public ModeAcro { @@ -564,9 +564,9 @@ class ModeAuto : public Mode { AP_Mission_ChangeDetector mis_change_detector; // true if weathervaning is allowed in auto -#if WEATHERVANE_ENABLED == ENABLED + bool allows_weathervaning(void) const override; -#endif + protected: @@ -631,9 +631,9 @@ class ModeAuto : public Mode { void do_set_home(const AP_Mission::Mission_Command& cmd); void do_roi(const AP_Mission::Mission_Command& cmd); void do_mount_control(const AP_Mission::Mission_Command& cmd); -#if PARACHUTE == ENABLED + void do_parachute(const AP_Mission::Mission_Command& cmd); -#endif + void do_winch(const AP_Mission::Mission_Command& cmd); @@ -730,7 +730,7 @@ class ModeAuto : public Mode { } desired_speed_override; }; -#if AUTOTUNE_ENABLED == ENABLED + /* wrapper class for AC_AutoTune */ @@ -787,7 +787,7 @@ class ModeAutoTune : public Mode { AutoTune autotune; }; -#endif + class ModeBrake : public Mode { @@ -918,7 +918,7 @@ class ModeFlip : public Mode { }; -#if MODE_FLOWHOLD_ENABLED == ENABLED + /* class to support FLOWHOLD mode, which is a position hold mode using optical flow directly, avoiding the need for a rangefinder @@ -1004,7 +1004,7 @@ class ModeFlowHold : public Mode { // last time there was significant stick input uint32_t last_stick_input_ms; }; -#endif // MODE_FLOWHOLD_ENABLED + class ModeGuided : public Mode { @@ -1099,9 +1099,9 @@ class ModeGuided : public Mode { bool resume() override; // true if weathervaning is allowed in guided -#if WEATHERVANE_ENABLED == ENABLED + bool allows_weathervaning(void) const override; -#endif + protected: @@ -1716,7 +1716,7 @@ class ModeThrow : public Mode { float free_fall_start_velz; // vertical velocity when free fall was detected }; -#if MODE_TURTLE_ENABLED == ENABLED + class ModeTurtle : public Mode { public: @@ -1747,7 +1747,7 @@ class ModeTurtle : public Mode { Vector2f motors_input; uint32_t last_throttle_warning_output_ms; }; -#endif + // modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order) @@ -1777,7 +1777,7 @@ class ModeAvoidADSB : public ModeGuided { }; -#if MODE_FOLLOW_ENABLED == ENABLED + class ModeFollow : public ModeGuided { public: @@ -1807,7 +1807,7 @@ class ModeFollow : public ModeGuided { uint32_t last_log_ms; // system time of last time desired velocity was logging }; -#endif + class ModeZigZag : public Mode { diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 03160ab2a05400..646cc13f4b1f66 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -2,7 +2,7 @@ #include "mode.h" -#if MODE_ACRO_ENABLED == ENABLED + /* * Init and run calls for acro flight mode @@ -197,4 +197,3 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa pitch_out = rate_bf_request_cd.y; yaw_out = rate_bf_request_cd.z; } -#endif diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 17dc05bad264d2..4e20c29a5709ee 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_ACRO_ENABLED == ENABLED + #if FRAME_CONFIG == HELI_FRAME /* @@ -153,4 +153,4 @@ void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &ya } #endif //HELI_FRAME -#endif //MODE_ACRO_ENABLED == ENABLED + diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 82fba3c83dac28..895e8c09627af7 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_AUTO_ENABLED == ENABLED + /* * Init and run calls for auto flight mode @@ -206,12 +206,12 @@ bool ModeAuto::allows_arming(AP_Arming::Method method) const return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL; }; -#if WEATHERVANE_ENABLED == ENABLED + bool ModeAuto::allows_weathervaning() const { return (copter.g2.auto_options & (uint32_t)Options::AllowWeatherVaning) != 0; } -#endif + // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) @@ -1798,11 +1798,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED + nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif + + + } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } @@ -2283,5 +2283,3 @@ bool ModeAuto::paused() const { return wp_nav->paused(); } - -#endif diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 0a7da76b07932f..cf6fac6e7dd17f 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -4,7 +4,7 @@ autotune mode is a wrapper around the AC_AutoTune library */ -#if AUTOTUNE_ENABLED == ENABLED + bool AutoTune::init() { @@ -142,5 +142,3 @@ void ModeAutoTune::reset() { autotune.reset(); } - -#endif // AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 65a353ebc78296..ef30892e6c5b56 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_BRAKE_ENABLED == ENABLED + /* * Init and run calls for brake flight mode @@ -84,5 +84,3 @@ void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) _timeout_start = millis(); _timeout_ms = timeout_ms; } - -#endif diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index bb9a6612f948c5..e6a6000bc08932 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -1,7 +1,7 @@ #include "Copter.h" #include -#if MODE_CIRCLE_ENABLED == ENABLED + /* * Init and run calls for circle flight mode @@ -134,5 +134,3 @@ int32_t ModeCircle::wp_bearing() const { return copter.circle_nav->get_bearing_to_target(); } - -#endif diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 9b48478c5a52c0..e8094b143ee304 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_DRIFT_ENABLED == ENABLED + /* * Init and run calls for drift flight mode @@ -140,4 +140,3 @@ float ModeDrift::get_throttle_assist(float velz, float pilot_throttle_scaled) return constrain_float(pilot_throttle_scaled + thr_assist, 0.0f, 1.0f); } -#endif diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 938b44db7db10e..54e8457418d1a8 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_FLIP_ENABLED == ENABLED + /* * Init and run calls for flip flight mode @@ -213,5 +213,3 @@ void ModeFlip::run() // output pilot's throttle without angle boost attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt); } - -#endif diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index a746d16e359050..b15bea99b5e65b 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -1,7 +1,7 @@ #include "Copter.h" #include -#if MODE_FLOWHOLD_ENABLED == ENABLED + /* implement FLOWHOLD mode, for position hold using optical flow @@ -513,5 +513,3 @@ void ModeFlowHold::update_height_estimate(void) delta_velocity_ne.zero(); last_ins_height = ins_height; } - -#endif // MODE_FLOWHOLD_ENABLED diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 9d9ef816a0e866..f53ac219ac12d0 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_FOLLOW_ENABLED == ENABLED + /* * mode_follow.cpp - follow another mavlink-enabled vehicle by system id @@ -171,5 +171,3 @@ bool ModeFollow::get_wp(Location &loc) const loc.offset_bearing(bearing, dist); return true; } - -#endif // MODE_FOLLOW_ENABLED == ENABLED diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 353af22252b111..365d56c663dd00 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_GUIDED_ENABLED == ENABLED + /* * Init and run calls for guided flight mode @@ -108,12 +108,12 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0; }; -#if WEATHERVANE_ENABLED == ENABLED + bool ModeGuided::allows_weathervaning() const { return (copter.g2.guided_options.get() & (uint32_t)Options::AllowWeatherVaning) != 0; } -#endif + // initialises position controller to implement take-off // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available @@ -1164,5 +1164,3 @@ bool ModeGuided::resume() _paused = false; return true; } - -#endif diff --git a/ArduCopter/mode_guided_nogps.cpp b/ArduCopter/mode_guided_nogps.cpp index b74aaf8f2bf720..4460417dbfec20 100644 --- a/ArduCopter/mode_guided_nogps.cpp +++ b/ArduCopter/mode_guided_nogps.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_GUIDED_NOGPS_ENABLED == ENABLED + /* * Init and run calls for guided_nogps flight mode @@ -21,5 +21,3 @@ void ModeGuidedNoGPS::run() // run angle controller ModeGuided::angle_control_run(); } - -#endif diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index cdc8a27d38d7f7..55a15252891d72 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_LOITER_ENABLED == ENABLED + /* * Init and run calls for loiter flight mode @@ -212,5 +212,3 @@ int32_t 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 7b47eeb7c3592a..3cff3108acc7e4 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_POSHOLD_ENABLED == ENABLED + /* * Init and run calls for PosHold flight mode @@ -634,5 +634,3 @@ void ModePosHold::pitch_controller_to_pilot_override() // store final loiter outputs for mixing with pilot input controller_final_pitch = pitch; } - -#endif diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 8116cbcfd82da0..6cfd22bc4808dc 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_RTL_ENABLED == ENABLED + /* * Init and run calls for RTL flight mode @@ -571,5 +571,3 @@ bool ModeRTL::set_speed_down(float speed_down_cms) copter.wp_nav->set_speed_down(speed_down_cms); return true; } - -#endif diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index dacd7cc5f29ab0..e95d86d6860408 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_SMARTRTL_ENABLED == ENABLED + /* * Init and run calls for Smart_RTL flight mode @@ -198,5 +198,3 @@ bool ModeSmartRTL::use_pilot_yaw() const const bool final_landing = smart_rtl_state == SubMode::LAND; return g2.smart_rtl.use_pilot_yaw() || land_repositioning || final_landing; } - -#endif diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 954a5a2b984d06..8e76498ff3c45d 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_SPORT_ENABLED == ENABLED + /* * Init and run calls for sport flight mode @@ -122,5 +122,3 @@ void ModeSport::run() // run the vertical position controller and set output throttle pos_control->update_z_controller(); } - -#endif diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 884b7a22be88d5..98a93d65ae488c 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_THROW_ENABLED == ENABLED + // throw_init - initialise throw controller bool ModeThrow::init(bool ignore_checks) @@ -321,5 +321,3 @@ bool ModeThrow::throw_position_good() const // check that our horizontal position error is within 50cm return (pos_control->get_pos_error_xy_cm() < 50.0f); } - -#endif diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 94728d6a29be9d..718bef82450653 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_TURTLE_ENABLED == ENABLED + #define CRASH_FLIP_EXPO 35.0f #define CRASH_FLIP_STICK_MINF 0.15f @@ -203,5 +203,3 @@ void ModeTurtle::output_to_motors() motors->rc_write(i, pwm); } } - -#endif diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index fbf43aa50ed458..1d227eab54907b 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if MODE_ZIGZAG_ENABLED == ENABLED + /* * Init and run calls for zigzag flight mode @@ -595,5 +595,3 @@ float ModeZigZag::crosstrack_error() const { return is_auto ? wp_nav->crosstrack_error() : 0; } - -#endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 589561b8cfe406..cdf1a88d4a5f43 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -10,7 +10,7 @@ void Copter::read_barometer(void) void Copter::init_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED + rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt); @@ -19,13 +19,13 @@ void Copter::init_rangefinder(void) // upward facing range finder rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt); rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90); -#endif + } // return rangefinder altitude in centimeters void Copter::read_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED + rangefinder.update(); #if RANGEFINDER_TILT_CORRECTION == ENABLED @@ -113,17 +113,17 @@ void Copter::read_rangefinder(void) #endif } -#else - // downward facing rangefinder - rangefinder_state.enabled = false; - rangefinder_state.alt_healthy = false; - rangefinder_state.alt_cm = 0; - - // upward facing rangefinder - rangefinder_up_state.enabled = false; - rangefinder_up_state.alt_healthy = false; - rangefinder_up_state.alt_cm = 0; -#endif + + + + + + + + + + + } // return true if rangefinder_alt can be used @@ -150,9 +150,9 @@ void Copter::update_rangefinder_terrain_offset() if (rangefinder_state.alt_healthy || (AP_HAL::millis() - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS)) { wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm); -#if MODE_CIRCLE_ENABLED + circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm); -#endif + } } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 99b05660fbb7d9..b037e548c8b70b 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -4,7 +4,7 @@ // level measured using the range finder. void Copter::SurfaceTracking::update_surface_offset() { -#if RANGEFINDER_ENABLED == ENABLED + // check for timeout const uint32_t now_ms = millis(); const bool timeout = (now_ms - last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS; @@ -42,10 +42,10 @@ void Copter::SurfaceTracking::update_surface_offset() reset_target = true; } } -#else - copter.pos_control->set_pos_offset_z_cm(0); - copter.pos_control->set_pos_offset_target_z_cm(0); -#endif + + + + } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index bbf1bf27133ac4..04ac16c544bd62 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -133,10 +133,10 @@ void Copter::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); -#if RANGEFINDER_ENABLED == ENABLED + // initialise rangefinder init_rangefinder(); -#endif + #if HAL_PROXIMITY_ENABLED // init proximity sensor @@ -153,15 +153,15 @@ void Copter::init_ardupilot() rpm_sensor.init(); -#if MODE_AUTO_ENABLED == ENABLED + // initialise mission library mode_auto.mission.init(); -#endif -#if MODE_SMARTRTL_ENABLED == ENABLED + + // initialize SmartRTL g2.smart_rtl.init(); -#endif + #if HAL_LOGGING_ENABLED // initialise AP_Logger library @@ -465,13 +465,13 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); -#if MODE_CIRCLE_ENABLED == ENABLED + circle_nav = new AC_Circle(inertial_nav, *ahrs_view, *pos_control); if (circle_nav == nullptr) { AP_BoardConfig::allocation_error("CircleNav"); } AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info); -#endif + // reload lines from the defaults file that may now be accessible AP_Param::reload_defaults_file(true); diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index 4b49c2c0d6908c..fda02b9d246847 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -8,12 +8,12 @@ void Copter::terrain_update() // tell the rangefinder our height, so it can go into power saving // mode if available -#if RANGEFINDER_ENABLED == ENABLED + float height; if (terrain.height_above_terrain(height, true)) { rangefinder.set_estimated_terrain_height(height); } -#endif + } diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index d2d444b5bd84de..18958ab1f0c89c 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -490,11 +490,11 @@ void ToyMode::update() break; case ACTION_MODE_ACRO: -#if MODE_ACRO_ENABLED == ENABLED + new_mode = Mode::Number::ACRO; -#else - gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ACRO is disabled"); -#endif + + + break; case ACTION_MODE_ALTHOLD: @@ -542,11 +542,11 @@ void ToyMode::update() break; case ACTION_MODE_THROW: -#if MODE_THROW_ENABLED == ENABLED + new_mode = Mode::Number::THROW; -#else - gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: THROW is disabled"); -#endif + + + break; case ACTION_MODE_FLIP: diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index c2cea3c72f2c38..2230f3d0de93af 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -142,11 +142,11 @@ void Copter::tuning() compass.set_declination(ToRad(tuning_value), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact break; -#if MODE_CIRCLE_ENABLED == ENABLED + case TUNING_CIRCLE_RATE: circle_nav->set_rate(tuning_value); break; -#endif + case TUNING_RC_FEEL_RP: attitude_control->set_input_tc(tuning_value); diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ed72d42aed954d..3aa3a011d19a28 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -558,9 +558,9 @@ void Plane::update_alt() // low pass the sink rate to take some of the noise out auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; -#if PARACHUTE == ENABLED + parachute.set_sink_rate(auto_state.sink_rate); -#endif + update_flight_stage(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index ef5cee9f6caa69..e040774a309e71 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -869,7 +869,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl { switch(packet.command) { -#if OFFBOARD_GUIDED == ENABLED + case MAV_CMD_GUIDED_CHANGE_SPEED: { // command is only valid in guided mode if (plane.control_mode != &plane.mode_guided) { @@ -998,7 +998,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_heading_time_ms = AP_HAL::millis(); return MAV_RESULT_ACCEPTED; } -#endif // OFFBOARD_GUIDED == ENABLED + } @@ -1044,10 +1044,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_CHANGE_SPEED: return handle_command_DO_CHANGE_SPEED(packet); -#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: return handle_MAV_CMD_DO_PARACHUTE(packet); -#endif + #if HAL_QUADPLANE_ENABLED case MAV_CMD_DO_MOTOR_TEST: @@ -1166,7 +1166,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_co return MAV_RESULT_ACCEPTED; } -#if PARACHUTE == ENABLED + MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet) { // configure or release parachute @@ -1196,7 +1196,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_ } return MAV_RESULT_FAILED; } -#endif + #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c712b2db1756b3..d0c1b4d29296cb 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -121,7 +121,7 @@ void Plane::Log_Write_Control_Tuning() logger.WriteBlock(&pkt, sizeof(pkt)); } -#if OFFBOARD_GUIDED == ENABLED + struct PACKED log_OFG_Guided { LOG_PACKET_HEADER; uint64_t time_us; @@ -150,7 +150,7 @@ void Plane::Log_Write_OFG_Guided() }; logger.WriteBlock(&pkt, sizeof(pkt)); } -#endif + struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; @@ -263,7 +263,7 @@ void Plane::Log_Write_RC(void) void Plane::Log_Write_Guided(void) { -#if OFFBOARD_GUIDED == ENABLED + if (control_mode != &mode_guided) { return; } @@ -275,7 +275,7 @@ void Plane::Log_Write_Guided(void) if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) { Log_Write_OFG_Guided(); } -#endif // OFFBOARD_GUIDED == ENABLED + } // incoming-to-vehicle mavlink COMMAND_INT can be logged @@ -469,7 +469,7 @@ const struct LogStructure Plane::log_structure[] = { { LOG_AETR_MSG, sizeof(log_AETR), "AETR", "Qfffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true }, -#if OFFBOARD_GUIDED == ENABLED + // @LoggerMessage: OFG // @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s. // @Field: TimeUS: Time since system startup @@ -482,7 +482,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: HdgA: target heading lim { LOG_OFG_MSG, sizeof(log_OFG_Guided), "OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, -#endif + }; uint8_t Plane::get_num_log_structures() const diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 7da4314daf0335..103a764c0586c1 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -761,11 +761,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(relay, "RELAY", AP_Relay), -#if PARACHUTE == ENABLED + // @Group: CHUTE_ // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp GOBJECT(parachute, "CHUTE_", AP_Parachute), -#endif + // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -800,11 +800,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(quadplane, "Q_", QuadPlane), #endif -#if AP_TUNING_ENABLED + // @Group: TUNE_ // @Path: tuning.cpp,../libraries/AP_Tuning/AP_Tuning.cpp GOBJECT(tuning, "TUNE_", AP_Tuning_Plane), -#endif + #if HAL_QUADPLANE_ENABLED // @Group: Q_A_ @@ -1193,11 +1193,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0), -#if OFFBOARD_GUIDED == ENABLED + // @Group: GUIDED_ // @Path: ../libraries/AC_PID/AC_PID.cpp AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID), -#endif // OFFBOARD_GUIDED == ENABLED + // @Param: MAN_EXPO_ROLL // @DisplayName: Manual control expo for roll diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 82bd48e28b9377..662b5a0f46af05 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -552,10 +552,10 @@ class ParametersG2 { } fwd_batt_cmp; -#if OFFBOARD_GUIDED == ENABLED + // guided yaw heading PID AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.0}; -#endif + #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED AP_Follow follow; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 10d346c5752b84..44e7abfa466073 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -96,9 +96,9 @@ #include "GCS_Plane.h" #include "quadplane.h" #include -#if AP_TUNING_ENABLED + #include "tuning.h" -#endif + // Configuration #include "config.h" @@ -543,7 +543,7 @@ class Plane : public AP_Vehicle { float forced_throttle; uint32_t last_forced_throttle_ms; -#if OFFBOARD_GUIDED == ENABLED + // airspeed adjustments float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed. float target_airspeed_accel; @@ -562,7 +562,7 @@ class Plane : public AP_Vehicle { uint32_t target_heading_time_ms; guided_heading_type_t target_heading_type; bool target_heading_limit; -#endif // OFFBOARD_GUIDED == ENABLED + } guided_state; @@ -639,9 +639,9 @@ class Plane : public AP_Vehicle { FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)}; -#if PARACHUTE == ENABLED + AP_Parachute parachute; -#endif + // terrain handling @@ -805,10 +805,10 @@ class Plane : public AP_Vehicle { QuadPlane quadplane{ahrs}; #endif -#if AP_TUNING_ENABLED + // support for transmitter tuning AP_Tuning_Plane tuning; -#endif + static const struct LogStructure log_structure[]; @@ -1140,11 +1140,11 @@ class Plane : public AP_Vehicle { // parachute.cpp void parachute_check(); -#if PARACHUTE == ENABLED + void do_parachute(const AP_Mission::Mission_Command& cmd); void parachute_release(); bool parachute_manual_release(); -#endif + // soaring.cpp #if HAL_SOARING_ENABLED diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 1fc5b0ad222a19..6284016d91423a 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -362,11 +362,11 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch break; case AUX_FUNC::PARACHUTE_RELEASE: -#if PARACHUTE == ENABLED + if (ch_flag == AuxSwitchPos::HIGH) { plane.parachute_manual_release(); } -#endif + break; case AUX_FUNC::MODE_SWITCH_RESET: diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7d187797a921a8..d1e85a2689302f 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -539,11 +539,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay.time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED + nav_delay.time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay.time_max_ms = 0; -#endif + + + } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay.time_max_ms/1000)); } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index c7fabb0172529f..99259a1ced5513 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -138,11 +138,11 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason break; } if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { -#if PARACHUTE == ENABLED + parachute_release(); //stop motors to avoid parachute tangling plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); -#endif + } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { @@ -189,11 +189,11 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::GUIDED: if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { -#if PARACHUTE == ENABLED + parachute_release(); //stop motors to avoid parachute tangling plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); -#endif + } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { @@ -315,9 +315,9 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) break; case Failsafe_Action_Parachute: -#if PARACHUTE == ENABLED + parachute_release(); -#endif + break; case Failsafe_Action_None: diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index a228e4fd53f278..fe716e7ab87f3d 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -162,9 +162,9 @@ void Plane::update_is_flying_5Hz(void) #if HAL_ADSB_ENABLED adsb.set_is_flying(new_is_flying); #endif -#if PARACHUTE == ENABLED + parachute.set_is_flying(new_is_flying); -#endif + AP::stats()->set_flying(new_is_flying); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 786fb4c9bf6588..4784b08efa648c 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -54,14 +54,14 @@ bool Mode::enter() plane.guided_state.last_forced_rpy_ms.zero(); plane.guided_state.last_forced_throttle_ms = 0; -#if OFFBOARD_GUIDED == ENABLED + plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. plane.guided_state.target_alt_time_ms = 0; plane.guided_state.last_target_alt = 0; -#endif + plane.camera.set_is_auto_mode(this == &plane.mode_auto); diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 85462166d1bc7f..be9d838ab923c3 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -42,7 +42,7 @@ void ModeGuided::update() plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); plane.update_load_factor(); -#if OFFBOARD_GUIDED == ENABLED + // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) // This function is used in Guided and AvoidADSB, check for guided } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { @@ -70,7 +70,7 @@ void ModeGuided::update() plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit); plane.update_load_factor(); -#endif // OFFBOARD_GUIDED == ENABLED + } else { plane.calc_nav_roll(); } @@ -128,7 +128,7 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi void ModeGuided::update_target_altitude() { -#if OFFBOARD_GUIDED == ENABLED + if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. // offboard altitude demanded uint32_t now = AP_HAL::millis(); @@ -149,7 +149,7 @@ void ModeGuided::update_target_altitude() plane.set_target_altitude_location(temp); plane.altitude_error_cm = plane.calc_altitude_error_cm(); } else -#endif // OFFBOARD_GUIDED == ENABLED + { Mode::update_target_altitude(); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index dace7daec00633..580cd649e2f353 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -187,7 +187,7 @@ void Plane::calc_airspeed_errors() target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); } -#if OFFBOARD_GUIDED == ENABLED + } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped // offboard airspeed demanded uint32_t now = AP_HAL::millis(); @@ -203,7 +203,7 @@ void Plane::calc_airspeed_errors() target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100); } -#endif // OFFBOARD_GUIDED == ENABLED + #if HAL_SOARING_ENABLED } else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { @@ -256,11 +256,11 @@ void Plane::calc_airspeed_errors() } // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely. -#if OFFBOARD_GUIDED == ENABLED + if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) { airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero } -#endif + // Bump up the target airspeed based on throttle nudging if (control_mode->allows_throttle_nudging() && airspeed_nudge_cm > 0) { diff --git a/ArduPlane/parachute.cpp b/ArduPlane/parachute.cpp index acde4fedd6036b..14ef2105ff3e9f 100644 --- a/ArduPlane/parachute.cpp +++ b/ArduPlane/parachute.cpp @@ -6,13 +6,13 @@ */ void Plane::parachute_check() { -#if PARACHUTE == ENABLED + parachute.update(); parachute.check_sink_rate(); -#endif + } -#if PARACHUTE == ENABLED + /* parachute_release - trigger the release of the parachute @@ -65,4 +65,3 @@ bool Plane::parachute_manual_release() return true; } -#endif diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 3d4da9912db010..1528b862083ef7 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -210,10 +210,10 @@ void Plane::read_radio() quadplane.tailsitter.check_input(); #endif -#if AP_TUNING_ENABLED + // check for transmitter tuning changes tuning.check_input(control_mode->mode_number()); -#endif + } int16_t Plane::rudder_input(void) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cfec6b75746f84..ba2d8b626cc840 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -78,13 +78,13 @@ bool Plane::suppress_throttle(void) return false; } -#if PARACHUTE == ENABLED + if (control_mode->does_auto_throttle() && parachute.release_initiated()) { // throttle always suppressed in auto-throttle modes after parachute release initiated throttle_suppressed = true; return true; } -#endif + if (landing.is_throttle_suppressed()) { return true; diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index be860264fd339a..741284bf14ee09 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -1,6 +1,6 @@ #include -#if AP_TUNING_ENABLED + #include "Plane.h" @@ -312,5 +312,3 @@ void AP_Tuning_Plane::reload_value(uint8_t parm) break; } } - -#endif // AP_TUNING_ENABLED diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h index 4334455a09657f..1c026e207a609c 100644 --- a/ArduPlane/tuning.h +++ b/ArduPlane/tuning.h @@ -2,7 +2,7 @@ #include -#if AP_TUNING_ENABLED + #include @@ -116,5 +116,3 @@ class AP_Tuning_Plane : public AP_Tuning // mask of what params have been set uint64_t have_set; }; - -#endif // AP_TUNING_ENABLED diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 40e8f00304af1f..efdd0b67e4ec24 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -101,7 +101,7 @@ float GCS_MAVLINK_Sub::vfr_hud_alt() const // Work around to get temperature sensor data out void GCS_MAVLINK_Sub::send_scaled_pressure3() { -#if AP_TEMPERATURE_SENSOR_ENABLED + float temperature; if (!sub.temperature_sensor.get_temperature(temperature)) { return; @@ -113,7 +113,7 @@ void GCS_MAVLINK_Sub::send_scaled_pressure3() 0, temperature * 100, 0); // TODO: use differential pressure temperature -#endif + } bool GCS_MAVLINK_Sub::send_info() diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp index 075daff9fd1f2c..7d90dbbd1fa59b 100644 --- a/ArduSub/GCS_Sub.cpp +++ b/ArduSub/GCS_Sub.cpp @@ -73,7 +73,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags() } -#if RANGEFINDER_ENABLED == ENABLED + const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (sub.rangefinder_state.enabled) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; @@ -82,7 +82,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } -#endif + } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index cc36f1f478140a..62b4993c552af4 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -609,11 +609,11 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_Motors/AP_Motors6DOF.cpp,../libraries/AP_Motors/AP_MotorsMulticopter.cpp GOBJECT(motors, "MOT_", AP_Motors6DOF), -#if RCMAP_ENABLED == ENABLED + // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp GOBJECT(rcmap, "RCMAP_", RCMapper), -#endif + #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ @@ -631,7 +631,7 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), -#if RANGEFINDER_ENABLED == ENABLED + // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), @@ -649,7 +649,7 @@ const AP_Param::Info Sub::var_info[] = { // @Units: cm // @User: Standard GSCALAR(surftrak_depth, "SURFTRAK_DEPTH", SURFTRAK_DEPTH_DEFAULT), -#endif + // @Group: TERRAIN_ diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 5795c5016843d7..33d8341bf65b24 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -243,10 +243,10 @@ class Parameters { AP_Float throttle_filt; -#if RANGEFINDER_ENABLED == ENABLED + AP_Int8 rangefinder_signal_min; // minimum signal quality for good rangefinder readings AP_Float surftrak_depth; // surftrak will try to keep sub below this depth -#endif + AP_Int8 failsafe_leak; // leak detection failsafe behavior AP_Int8 failsafe_gcs; // ground station failsafe behavior diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4358c916b3c108..629b1cd5c6089c 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -78,9 +78,9 @@ #include // Optical Flow library // libraries which are dependent on #defines in defines.h and/or config.h -#if RCMAP_ENABLED == ENABLED + #include // RC input mapping library -#endif + #include @@ -206,9 +206,9 @@ class Sub : public AP_Vehicle { Mode::Number prev_control_mode; -#if RCMAP_ENABLED == ENABLED + RCMapper rcmap; -#endif + // Failsafe struct { @@ -615,10 +615,10 @@ class Sub : public AP_Vehicle { // For Lua scripting, so index is 1..4, not 0..3 uint8_t get_and_clear_button_count(uint8_t index); -#if RANGEFINDER_ENABLED == ENABLED + float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); } bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); } -#endif // RANGEFINDER_ENABLED + }; diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 392749c6cf3017..fe50b8fd246161 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -408,11 +408,11 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED + nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif + + + } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 0cd0eefc4fabbd..18fe1f863c545d 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -190,11 +190,11 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) case JSButton::button_function_t::k_mode_poshold: set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); break; -#if RANGEFINDER_ENABLED == ENABLED + case JSButton::button_function_t::k_mode_surftrak: set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND); break; -#endif + case JSButton::button_function_t::k_mount_center: #if HAL_MOUNT_ENABLED diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index a8cb75c7fd9212..fa0594f65fa460 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -17,18 +17,18 @@ void Sub::read_barometer() void Sub::init_rangefinder() { -#if RANGEFINDER_ENABLED == ENABLED + rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); -#endif + } // return rangefinder altitude in centimeters void Sub::read_rangefinder() { -#if RANGEFINDER_ENABLED == ENABLED + rangefinder.update(); // signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a @@ -74,13 +74,13 @@ void Sub::read_rangefinder() rangefinder_state.enabled && wp_nav.rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.rangefinder_terrain_offset_cm); -#else - rangefinder_state.enabled = false; - rangefinder_state.alt_healthy = false; - rangefinder_state.alt_cm = 0; - rangefinder_state.inertial_alt_cm = 0; - rangefinder_state.rangefinder_terrain_offset_cm = 0; -#endif + + + + + + + } // return true if rangefinder_alt can be used diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 5da65f0144b354..4c7a3e29cf93f7 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -39,10 +39,10 @@ void Sub::init_ardupilot() AP_Param::set_default_by_name("BARO_EXT_BUS", 1); #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + // In order to preserve Sub's previous AP_TemperatureSensor Behavior we set the Default I2C Bus Here AP_Param::set_default_by_name("TEMP1_BUS", barometer.external_bus()); -#endif + // setup telem slots with serial ports gcs().setup_uarts(); @@ -129,9 +129,9 @@ void Sub::init_ardupilot() last_pilot_heading = ahrs.yaw_sensor; // initialise rangefinder -#if RANGEFINDER_ENABLED == ENABLED + init_rangefinder(); -#endif + // initialise AP_RPM library diff --git a/ArduSub/terrain.cpp b/ArduSub/terrain.cpp index 91df320b06ebf1..f7235e8d06cfda 100644 --- a/ArduSub/terrain.cpp +++ b/ArduSub/terrain.cpp @@ -8,12 +8,12 @@ void Sub::terrain_update() // tell the rangefinder our height, so it can go into power saving // mode if available -#if RANGEFINDER_ENABLED == ENABLED + float height; if (terrain.height_above_terrain(height, true)) { rangefinder.set_estimated_terrain_height(height); } -#endif + } diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index eba85331194459..078508a5599bed 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -215,12 +215,12 @@ bool RC_Channel_Rover::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch do_aux_function_change_mode(rover.mode_loiter, ch_flag); break; -#if MODE_FOLLOW_ENABLED == ENABLED + // Set mode to Follow case AUX_FUNC::FOLLOW: do_aux_function_change_mode(rover.mode_follow, ch_flag); break; -#endif + // set mode to Simple case AUX_FUNC::SIMPLE: diff --git a/Rover/Rover.h b/Rover/Rover.h index 0b133f96aad694..c65e8b0b330f8f 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -99,9 +99,9 @@ class Rover : public AP_Vehicle { friend class ModeManual; friend class ModeRTL; friend class ModeSmartRTL; -#if MODE_FOLLOW_ENABLED == ENABLED + friend class ModeFollow; -#endif + friend class ModeSimple; #if MODE_DOCK_ENABLED == ENABLED friend class ModeDock; @@ -251,9 +251,9 @@ class Rover : public AP_Vehicle { ModeSteering mode_steering; ModeRTL mode_rtl; ModeSmartRTL mode_smartrtl; -#if MODE_FOLLOW_ENABLED == ENABLED + ModeFollow mode_follow; -#endif + ModeSimple mode_simple; #if MODE_DOCK_ENABLED == ENABLED ModeDock mode_dock; diff --git a/Rover/mode.cpp b/Rover/mode.cpp index f0e0828ea576a4..7facd7245b2b1a 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -530,11 +530,11 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::LOITER: ret = &mode_loiter; break; -#if MODE_FOLLOW_ENABLED == ENABLED + case Mode::Number::FOLLOW: ret = &mode_follow; break; -#endif + case Mode::Number::SIMPLE: ret = &mode_simple; break; diff --git a/Rover/mode.h b/Rover/mode.h index b744727e96fa4c..7600ab11cfa5af 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -792,7 +792,7 @@ class ModeInitializing : public Mode bool _enter() override { return false; }; }; -#if MODE_FOLLOW_ENABLED == ENABLED + class ModeFollow : public Mode { public: @@ -827,7 +827,7 @@ class ModeFollow : public Mode float _desired_speed; // desired speed in m/s }; -#endif + class ModeSimple : public Mode { diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 2e767240b3854e..eb89140caf8b3c 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -784,11 +784,11 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time -#if AP_RTC_ENABLED + nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); -#else - nav_delay_time_max_ms = 0; -#endif + + + } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } diff --git a/Rover/mode_follow.cpp b/Rover/mode_follow.cpp index 964a81a102a6ba..fc2d3f32411ea9 100644 --- a/Rover/mode_follow.cpp +++ b/Rover/mode_follow.cpp @@ -1,6 +1,6 @@ #include "Rover.h" -#if MODE_FOLLOW_ENABLED + // initialize follow mode bool ModeFollow::_enter() { @@ -95,5 +95,3 @@ bool ModeFollow::set_desired_speed(float speed) _desired_speed = speed; return true; } - -#endif // MODE_FOLLOW_ENABLED diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 731821834eb769..d9b46d592f44d3 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -274,9 +274,9 @@ void AP_Periph_FW::init() } #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + temperature_sensor.init(); -#endif + #if HAL_NMEA_OUTPUT_ENABLED nmea.init(); @@ -497,9 +497,9 @@ void AP_Periph_FW::update() nmea.update(); #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + temperature_sensor.update(); -#endif + #ifdef HAL_PERIPH_ENABLE_RPM if (now - rpm_last_update_ms >= 100) { diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 276df103a173b5..4c924153157e66 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -360,14 +360,14 @@ class AP_Periph_FW { SerialOptions serial_options; #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + AP_TemperatureSensor temperature_sensor; #ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE void temperature_sensor_update(); uint32_t temperature_last_send_ms; uint8_t temperature_last_sent_index; #endif -#endif + #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) void update_rainbow(); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 6351741d0805f5..0c773c4f80c18a 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -422,11 +422,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #endif #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + // @Group: TEMP // @Path: ../libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp GOBJECT(temperature_sensor, "TEMP", AP_TemperatureSensor), -#endif + #ifdef HAL_PERIPH_ENABLE_MSP // @Param: MSP_PORT diff --git a/Tools/AP_Periph/airspeed.cpp b/Tools/AP_Periph/airspeed.cpp index 17a4da160731c5..a4272781a3a197 100644 --- a/Tools/AP_Periph/airspeed.cpp +++ b/Tools/AP_Periph/airspeed.cpp @@ -62,7 +62,7 @@ void AP_Periph_FW::can_airspeed_update(void) pkt.static_air_temperature = temp; // if a Pitot tube temperature sensor is available, use it -#if AP_TEMPERATURE_SENSOR_ENABLED + for (uint8_t i=0; ifailed_sdcard_storage() || StorageManager::storage_failed())) { check_failed(ARMING_CHECK_MISSION, report, "Failed to open %s", AP_MISSION_SDCARD_FILENAME); return false; } -#endif + #if AP_VEHICLE_ENABLED // do not allow arming if there are no mission items and we are in @@ -879,7 +879,7 @@ bool AP_Arming::mission_checks(bool report) return true; } -#endif // AP_MISSION_ENABLED + bool AP_Arming::rangefinder_checks(bool report) { @@ -1096,7 +1096,7 @@ bool AP_Arming::system_checks(bool report) bool AP_Arming::terrain_database_required() const { -#if AP_MISSION_ENABLED + AP_Mission *mission = AP::mission(); if (mission == nullptr) { // no mission support? @@ -1105,7 +1105,7 @@ bool AP_Arming::terrain_database_required() const if (mission->contains_terrain_alt_items()) { return true; } -#endif + return false; } @@ -1251,12 +1251,12 @@ bool AP_Arming::fence_checks(bool display_failure) check_failed(display_failure, "%s", fail_msg); } -#if AP_SDCARD_STORAGE_ENABLED + if (fence->failed_sdcard_storage() || StorageManager::storage_failed()) { check_failed(display_failure, "Failed to open fence storage"); return false; } -#endif + return false; } @@ -1473,7 +1473,7 @@ bool AP_Arming::generator_checks(bool display_failure) const } #endif // HAL_GENERATOR_ENABLED -#if AP_OPENDRONEID_ENABLED + // OpenDroneID Checks bool AP_Arming::opendroneid_checks(bool display_failure) { @@ -1486,7 +1486,7 @@ bool AP_Arming::opendroneid_checks(bool display_failure) } return true; } -#endif // AP_OPENDRONEID_ENABLED + //Check for multiple RC in serial protocols bool AP_Arming::serial_protocol_checks(bool display_failure) @@ -1505,7 +1505,7 @@ bool AP_Arming::estop_checks(bool display_failure) // not emergency-stopped, so no prearm failure: return true; } -#if AP_RC_CHANNEL_ENABLED + // vehicle is emergency-stopped; if this *appears* to have been done via switch then we do not fail prearms: const RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP); if (chan != nullptr) { @@ -1515,7 +1515,7 @@ bool AP_Arming::estop_checks(bool display_failure) return true; // no prearm failure } } -#endif // AP_RC_CHANNEL_ENABLED + check_failed(display_failure,"Motors Emergency Stopped"); return false; } @@ -1534,9 +1534,9 @@ bool AP_Arming::pre_arm_checks(bool report) #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) #endif -#if AP_BARO_ENABLED + & barometer_checks(report) -#endif + #if AP_INERTIALSENSOR_ENABLED & ins_checks(report) #endif @@ -1552,12 +1552,12 @@ bool AP_Arming::pre_arm_checks(bool report) #if HAL_LOGGING_ENABLED & logging_checks(report) #endif -#if AP_RC_CHANNEL_ENABLED + & manual_transmitter_checks(report) -#endif -#if AP_MISSION_ENABLED + + & mission_checks(report) -#endif + & rangefinder_checks(report) @@ -1592,15 +1592,15 @@ bool AP_Arming::pre_arm_checks(bool report) #if AP_ARMING_AUX_AUTH_ENABLED & aux_auth_checks(report) #endif -#if AP_RC_CHANNEL_ENABLED + & disarm_switch_checks(report) -#endif + & fence_checks(report) -#if AP_OPENDRONEID_ENABLED + & opendroneid_checks(report) -#endif + & serial_protocol_checks(report) & estop_checks(report); @@ -1614,13 +1614,13 @@ bool AP_Arming::pre_arm_checks(bool report) bool AP_Arming::arm_checks(AP_Arming::Method method) { -#if AP_RC_CHANNEL_ENABLED + if (check_enabled(ARMING_CHECK_RC)) { if (!rc_arm_checks(method)) { return false; } } -#endif + // ensure the GPS drivers are ready on any final changes if (check_enabled(ARMING_CHECK_GPS_CONFIG)) { @@ -1667,9 +1667,9 @@ bool AP_Arming::arm_checks(AP_Arming::Method method) bool AP_Arming::mandatory_checks(bool report) { bool ret = true; -#if AP_OPENDRONEID_ENABLED + ret &= opendroneid_checks(report); -#endif + ret &= rc_in_calibration_check(report); ret &= serial_protocol_checks(report); return ret; @@ -1806,18 +1806,18 @@ void AP_Arming::send_arm_disarm_statustext(const char *str) const AP_Arming::Required AP_Arming::arming_required() const { -#if AP_OPENDRONEID_ENABLED + // cannot be disabled if OpenDroneID is present if (AP_OpenDroneID::get_singleton() != nullptr && AP::opendroneid().enabled()) { if (require != Required::YES_MIN_PWM && require != Required::YES_ZERO_PWM) { return Required::YES_MIN_PWM; } } -#endif + return require; } -#if AP_RC_CHANNEL_ENABLED + // Copter and sub share the same RC input limits // Copter checks that min and max have been configured by default, Sub does not bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channel *channels[4]) const @@ -1846,7 +1846,7 @@ bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channe } return ret; } -#endif // AP_RC_CHANNEL_ENABLED + #if HAL_VISUALODOM_ENABLED // check visual odometry is working @@ -1869,7 +1869,7 @@ bool AP_Arming::visodom_checks(bool display_failure) const } #endif -#if AP_RC_CHANNEL_ENABLED + // check disarm switch is asserted bool AP_Arming::disarm_switch_checks(bool display_failure) const { @@ -1882,7 +1882,7 @@ bool AP_Arming::disarm_switch_checks(bool display_failure) const return true; } -#endif // AP_RC_CHANNEL_ENABLED + #if HAL_LOGGING_ENABLED void AP_Arming::Log_Write_Arm(const bool forced, const AP_Arming::Method method) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index cbafbf10dc5078..d7dca857c91a61 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -649,41 +649,41 @@ void AP_Baro::init(void) case AP_BoardConfig::PX4_BOARD_AUAV21: case AP_BoardConfig::PX4_BOARD_PH2SLIM: case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); -#endif + break; case AP_BoardConfig::PX4_BOARD_PIXHAWK2: case AP_BoardConfig::PX4_BOARD_SP01: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_EXT_NAME)))); ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); -#endif + break; case AP_BoardConfig::PX4_BOARD_MINDPXV2: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); -#endif + break; case AP_BoardConfig::PX4_BOARD_AEROFC: -#if AP_BARO_MS56XX_ENABLED + #ifdef HAL_BARO_MS5607_I2C_BUS ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(GET_I2C_DEVICE(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5607)); #endif -#endif // AP_BARO_MS56XX_ENABLED + break; case AP_BoardConfig::VRX_BOARD_BRAIN54: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); ADD_BACKEND(AP_Baro_MS56XX::probe(*this, @@ -692,7 +692,7 @@ void AP_Baro::init(void) ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_IMU_NAME)))); #endif -#endif // AP_BARO_MS56XX_ENABLED + break; case AP_BoardConfig::VRX_BOARD_BRAIN51: @@ -701,10 +701,10 @@ void AP_Baro::init(void) case AP_BoardConfig::VRX_BOARD_CORE10: case AP_BoardConfig::VRX_BOARD_UBRAIN51: case AP_BoardConfig::VRX_BOARD_UBRAIN52: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); -#endif // AP_BARO_MS56XX_ENABLED + break; case AP_BoardConfig::PX4_BOARD_PCNC1: @@ -717,10 +717,10 @@ void AP_Baro::init(void) case AP_BoardConfig::PX4_BOARD_FMUV5: case AP_BoardConfig::PX4_BOARD_FMUV6: -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); -#endif + break; default: @@ -743,19 +743,19 @@ void AP_Baro::init(void) // can optionally have baro on I2C too if (_ext_bus >= 0) { #if APM_BUILD_TYPE(APM_BUILD_ArduSub) -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(GET_I2C_DEVICE(_ext_bus, HAL_BARO_MS5837_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5837)); -#endif + #if AP_BARO_KELLERLD_ENABLED ADD_BACKEND(AP_Baro_KellerLD::probe(*this, std::move(GET_I2C_DEVICE(_ext_bus, HAL_BARO_KELLERLD_I2C_ADDR)))); #endif #else -#if AP_BARO_MS56XX_ENABLED + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(GET_I2C_DEVICE(_ext_bus, HAL_BARO_MS5611_I2C_ADDR)))); -#endif + #endif } @@ -820,46 +820,46 @@ void AP_Baro::_probe_i2c_barometers(void) AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::OwnPtr); uint8_t addr; } baroprobespec[] { -#if AP_BARO_BMP085_ENABLED + { PROBE_BMP085, AP_Baro_BMP085::probe, HAL_BARO_BMP085_I2C_ADDR }, -#endif -#if AP_BARO_BMP280_ENABLED + + { PROBE_BMP280, AP_Baro_BMP280::probe, HAL_BARO_BMP280_I2C_ADDR }, { PROBE_BMP280, AP_Baro_BMP280::probe, HAL_BARO_BMP280_I2C_ADDR2 }, -#endif -#if AP_BARO_SPL06_ENABLED + + { PROBE_SPL06, AP_Baro_SPL06::probe, HAL_BARO_SPL06_I2C_ADDR }, { PROBE_SPL06, AP_Baro_SPL06::probe, HAL_BARO_SPL06_I2C_ADDR2 }, -#endif -#if AP_BARO_BMP388_ENABLED + + { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR }, { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR2 }, -#endif -#if AP_BARO_MS56XX_ENABLED + + { PROBE_MS5611, AP_Baro_MS56XX::probe_5611, HAL_BARO_MS5611_I2C_ADDR }, { PROBE_MS5611, AP_Baro_MS56XX::probe_5611, HAL_BARO_MS5611_I2C_ADDR2 }, { PROBE_MS5607, AP_Baro_MS56XX::probe_5607, HAL_BARO_MS5607_I2C_ADDR }, { PROBE_MS5637, AP_Baro_MS56XX::probe_5637, HAL_BARO_MS5637_I2C_ADDR }, -#endif -#if AP_BARO_FBM320_ENABLED + + { PROBE_FBM320, AP_Baro_FBM320::probe, HAL_BARO_FBM320_I2C_ADDR }, { PROBE_FBM320, AP_Baro_FBM320::probe, HAL_BARO_FBM320_I2C_ADDR2 }, -#endif -#if AP_BARO_DPS280_ENABLED + + { PROBE_DPS280, AP_Baro_DPS280::probe_280, HAL_BARO_DPS280_I2C_ADDR }, { PROBE_DPS280, AP_Baro_DPS280::probe_280, HAL_BARO_DPS280_I2C_ADDR2 }, -#endif -#if AP_BARO_LPS2XH_ENABLED + + { PROBE_LPS25H, AP_Baro_LPS2XH::probe, HAL_BARO_LPS25H_I2C_ADDR }, -#endif + #if APM_BUILD_TYPE(APM_BUILD_ArduSub) #if AP_BARO_KELLERLD_ENABLED { PROBE_KELLER, AP_Baro_KellerLD::probe, HAL_BARO_KELLERLD_I2C_ADDR }, #endif -#if AP_BARO_MS56XX_ENABLED + { PROBE_MS5837, AP_Baro_MS56XX::probe_5837, HAL_BARO_MS5837_I2C_ADDR }, -#endif + #endif // APM_BUILD_TYPE(APM_BUILD_ArduSub) }; diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index d2fed889da6991..c2c28b69b0aa09 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_BMP085.h" -#if AP_BARO_BMP085_ENABLED + #include #include @@ -350,5 +350,3 @@ bool AP_Baro_BMP085::_data_ready() return AP_HAL::millis() - _last_press_read_command_time > conversion_time_msec; } - -#endif // AP_BARO_BMP085_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index 0b77e93b056da0..0bd473e0ff3c36 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_BMP085_ENABLED + #include #include @@ -64,5 +64,3 @@ class AP_Baro_BMP085 : public AP_Baro_Backend { uint8_t _vers; uint8_t _type; }; - -#endif // AP_BARO_BMP085_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index 1c89ac0a4b9557..7da8c0df656882 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_BMP280.h" -#if AP_BARO_BMP280_ENABLED + #include #include @@ -206,5 +206,3 @@ void AP_Baro_BMP280::_update_pressure(int32_t press_raw) _pressure_sum += press; _pressure_count++; } - -#endif // AP_BARO_BMP280_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_BMP280.h b/libraries/AP_Baro/AP_Baro_BMP280.h index 32969e47b5957d..16d78f32a0d9d1 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.h +++ b/libraries/AP_Baro/AP_Baro_BMP280.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_BMP280_ENABLED + #include #include @@ -44,5 +44,3 @@ class AP_Baro_BMP280 : public AP_Baro_Backend int16_t _t2, _t3, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9; uint16_t _t1, _p1; }; - -#endif // AP_BARO_BMP280_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_BMP388.cpp b/libraries/AP_Baro/AP_Baro_BMP388.cpp index 6b48d9e7831676..f68795572731f5 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP388.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_BMP388.h" -#if AP_BARO_BMP388_ENABLED + #include #include @@ -251,5 +251,3 @@ bool AP_Baro_BMP388::read_registers(uint8_t reg, uint8_t *data, uint8_t len) memcpy(data, &b[2], len); return true; } - -#endif // AP_BARO_BMP388_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_BMP388.h b/libraries/AP_Baro/AP_Baro_BMP388.h index fbae2566742eaf..ddd08952af2cd4 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.h +++ b/libraries/AP_Baro/AP_Baro_BMP388.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_BMP388_ENABLED + #include #include @@ -82,5 +82,3 @@ class AP_Baro_BMP388 : public AP_Baro_Backend void scale_calibration_data(void); bool read_registers(uint8_t reg, uint8_t *data, uint8_t len); }; - -#endif // AP_BARO_BMP388_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_DPS280.cpp b/libraries/AP_Baro/AP_Baro_DPS280.cpp index fe34ca0bd048f0..5041952493ef3a 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.cpp +++ b/libraries/AP_Baro/AP_Baro_DPS280.cpp @@ -18,7 +18,7 @@ #include "AP_Baro_DPS280.h" -#if AP_BARO_DPS280_ENABLED + #include #include @@ -311,5 +311,3 @@ void AP_Baro_DPS280::update(void) temperature_sum = 0; count=0; } - -#endif // AP_BARO_DPS280_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_DPS280.h b/libraries/AP_Baro/AP_Baro_DPS280.h index 79769261a498c3..24870e0bde9f95 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.h +++ b/libraries/AP_Baro/AP_Baro_DPS280.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_DPS280_ENABLED + #include #include @@ -71,6 +71,3 @@ class AP_Baro_DPS310 : public AP_Baro_DPS280 { using AP_Baro_DPS280::AP_Baro_DPS280; static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); }; - - -#endif // AP_BARO_DPS280_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_FBM320.cpp b/libraries/AP_Baro/AP_Baro_FBM320.cpp index 7ad9a7b0c89ae6..535f8a75a74856 100644 --- a/libraries/AP_Baro/AP_Baro_FBM320.cpp +++ b/libraries/AP_Baro/AP_Baro_FBM320.cpp @@ -18,7 +18,7 @@ #include "AP_Baro_FBM320.h" -#if AP_BARO_FBM320_ENABLED + #include #include @@ -224,5 +224,3 @@ void AP_Baro_FBM320::update(void) temperature_sum = 0; count=0; } - -#endif // AP_BARO_FBM320_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_FBM320.h b/libraries/AP_Baro/AP_Baro_FBM320.h index 546cbebabb5b0d..a9c74202f8f9c9 100644 --- a/libraries/AP_Baro/AP_Baro_FBM320.h +++ b/libraries/AP_Baro/AP_Baro_FBM320.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_FBM320_ENABLED + #include #include @@ -48,5 +48,3 @@ class AP_Baro_FBM320 : public AP_Baro_Backend { uint32_t C4, C5, C7; } calibration; }; - -#endif // AP_BARO_FBM320_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index 6db9acdc3d5447..471a1d13bd24ba 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_LPS2XH.h" -#if AP_BARO_LPS2XH_ENABLED + #include #include @@ -278,5 +278,3 @@ void AP_Baro_LPS2XH::_update_pressure(void) _pressure_sum += Pressure_mb; _pressure_count++; } - -#endif // AP_BARO_LPS2XH_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.h b/libraries/AP_Baro/AP_Baro_LPS2XH.h index 52d84abb1b7fbd..d7b3a4a67bc9ae 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.h +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_LPS2XH_ENABLED + #include #include @@ -54,5 +54,3 @@ class AP_Baro_LPS2XH : public AP_Baro_Backend enum LPS2XH_TYPE _lps2xh_type; }; - -#endif // AP_BARO_LPS2XH_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 295cd2e9e4c66f..37a6c0dc49fc93 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_MS5611.h" -#if AP_BARO_MS56XX_ENABLED + #include #include @@ -516,5 +516,3 @@ void AP_Baro_MS56XX::_calculate_5837() _copy_to_frontend(_instance, (float)pressure, temperature); } - -#endif // AP_BARO_MS56XX_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 2f124db2287dae..739fd59359e3e6 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_MS56XX_ENABLED + #include #include @@ -106,5 +106,3 @@ class AP_Baro_MS56XX : public AP_Baro_Backend enum MS56XX_TYPE _ms56xx_type; }; - -#endif // AP_BARO_MS56XX_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index baca15dee2d20f..6ef8cbb8a4f2b9 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -14,7 +14,7 @@ */ #include "AP_Baro_SPL06.h" -#if AP_BARO_SPL06_ENABLED + #include #include @@ -249,5 +249,3 @@ void AP_Baro_SPL06::_update_pressure(int32_t press_raw) _pressure_sum += press_comp; _pressure_count++; } - -#endif // AP_BARO_SPL06_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_SPL06.h b/libraries/AP_Baro/AP_Baro_SPL06.h index 7dcba34ee804c4..2e30ccc99e7879 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.h +++ b/libraries/AP_Baro/AP_Baro_SPL06.h @@ -2,7 +2,7 @@ #include "AP_Baro_Backend.h" -#if AP_BARO_SPL06_ENABLED + #include #include @@ -47,5 +47,3 @@ class AP_Baro_SPL06 : public AP_Baro_Backend int32_t _c00, _c10; int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30; }; - -#endif // AP_BARO_SPL06_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index c7d192861f3261..160ab93c34053d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -488,7 +488,7 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]); break; #endif -#if AP_BATTERY_BEBOP_ENABLED +#if AP_BATTERY_BEBOP_ENABLED && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP) case Type::BEBOP: drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]); break; @@ -498,31 +498,31 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]); break; #endif -#if AP_BATTERY_ESC_ENABLED +#if AP_BATTERY_ESC_ENABLED && !defined(HAL_LINUX) case Type::BLHeliESC: drivers[instance] = new AP_BattMonitor_ESC(*this, state[instance], _params[instance]); break; #endif -#if AP_BATTERY_SUM_ENABLED + case Type::Sum: drivers[instance] = new AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance); break; -#endif -#if AP_BATTERY_FUELFLOW_ENABLED + + case Type::FuelFlow: drivers[instance] = new AP_BattMonitor_FuelFlow(*this, state[instance], _params[instance]); break; -#endif // AP_BATTERY_FUELFLOW_ENABLED -#if AP_BATTERY_FUELLEVEL_PWM_ENABLED + + case Type::FuelLevel_PWM: drivers[instance] = new AP_BattMonitor_FuelLevel_PWM(*this, state[instance], _params[instance]); break; -#endif // AP_BATTERY_FUELLEVEL_PWM_ENABLED -#if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED + + case Type::FuelLevel_Analog: drivers[instance] = new AP_BattMonitor_FuelLevel_Analog(*this, state[instance], _params[instance]); break; -#endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED + #if HAL_GENERATOR_ENABLED case Type::GENERATOR_ELEC: drivers[instance] = new AP_BattMonitor_Generator_Elec(*this, state[instance], _params[instance]); @@ -531,12 +531,12 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_Generator_FuelLevel(*this, state[instance], _params[instance]); break; #endif // HAL_GENERATOR_ENABLED -#if AP_BATTERY_INA2XX_ENABLED + case Type::INA2XX: drivers[instance] = new AP_BattMonitor_INA2XX(*this, state[instance], _params[instance]); break; -#endif -#if AP_BATTERY_LTC2946_ENABLED + +#if AP_BATTERY_LTC2946_ENABLED && !defined(HAL_LINUX) case Type::LTC2946: drivers[instance] = new AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]); break; @@ -546,31 +546,31 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]); break; #endif -#if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED + case Type::Analog_Volt_Synthetic_Current: drivers[instance] = new AP_BattMonitor_Synthetic_Current(*this, state[instance], _params[instance]); break; -#endif -#if AP_BATTERY_INA239_ENABLED + +#if AP_BATTERY_INA239_ENABLED && !defined(HAL_LINUX) case Type::INA239_SPI: drivers[instance] = new AP_BattMonitor_INA239(*this, state[instance], _params[instance]); break; #endif -#if AP_BATTERY_EFI_ENABLED + case Type::EFI: drivers[instance] = new AP_BattMonitor_EFI(*this, state[instance], _params[instance]); break; -#endif // AP_BATTERY_EFI_ENABLED -#if AP_BATTERY_AD7091R5_ENABLED + + case Type::AD7091R5: drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]); break; -#endif// AP_BATTERY_AD7091R5_ENABLED -#if AP_BATTERY_SCRIPTING_ENABLED + + case Type::Scripting: drivers[instance] = new AP_BattMonitor_Scripting(*this, state[instance], _params[instance]); break; -#endif // AP_BATTERY_SCRIPTING_ENABLED + case Type::NONE: default: break; @@ -919,7 +919,7 @@ bool AP_BattMonitor::get_temperature(float &temperature, const uint8_t instance) return drivers[instance]->get_temperature(temperature); } -#if AP_TEMPERATURE_SENSOR_ENABLED + // return true when successfully setting a battery temperature from an external source by instance bool AP_BattMonitor::set_temperature(const float temperature, const uint8_t instance) { @@ -942,7 +942,7 @@ bool AP_BattMonitor::set_temperature_by_serial_number(const float temperature, c } return success; } -#endif // AP_TEMPERATURE_SENSOR_ENABLED + // return true if cycle count can be provided and fills in cycles argument bool AP_BattMonitor::get_cycle_count(uint8_t instance, uint16_t &cycles) const @@ -1101,7 +1101,7 @@ bool AP_BattMonitor::healthy() const return true; } -#if AP_BATTERY_SCRIPTING_ENABLED + /* handle state update from a lua script */ @@ -1112,7 +1112,7 @@ bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State } return drivers[idx]->handle_scripting(_state); } -#endif + namespace AP { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index f0c40636a33dec..5f260e05506620 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -144,10 +144,10 @@ class AP_BattMonitor uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum in milliseconds uint32_t critical_voltage_start_ms; // critical voltage failsafe start timer in milliseconds float temperature; // battery temperature in degrees Celsius -#if AP_TEMPERATURE_SENSOR_ENABLED + bool temperature_external_use; float temperature_external; // battery temperature set by an external source in degrees Celsius -#endif + uint32_t temperature_time; // timestamp of the last received temperature message float voltage_resting_estimate; // voltage with sag removed based on current and resistance estimate in Volt float resistance; // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage @@ -247,10 +247,10 @@ class AP_BattMonitor // temperature bool get_temperature(float &temperature) const { return get_temperature(temperature, AP_BATT_PRIMARY_INSTANCE); } bool get_temperature(float &temperature, const uint8_t instance) const; -#if AP_TEMPERATURE_SENSOR_ENABLED + bool set_temperature(const float temperature, const uint8_t instance); bool set_temperature_by_serial_number(const float temperature, const int32_t serial_number); -#endif + // MPPT Control (Solar panels) void MPPT_set_powered_state_to_all(const bool power_on); @@ -284,9 +284,9 @@ class AP_BattMonitor static const struct AP_Param::GroupInfo var_info[]; -#if AP_BATTERY_SCRIPTING_ENABLED + bool handle_scripting(uint8_t idx, const struct BattMonitorScript_State &state); -#endif + protected: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp index 70e37eab43c420..7fa8304275db90 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp @@ -17,7 +17,7 @@ * */ -#if AP_BATTERY_AD7091R5_ENABLED + #include #include @@ -232,5 +232,3 @@ float AP_BattMonitor_AD7091R5::_data_to_volt(uint32_t data) { return (AD7091R5_REF/AD7091R5_RESOLUTION)*data; } - -#endif // AP_BATTERY_AD7091R5_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h index e99c7e662094e8..ed7f904926a3ec 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h @@ -6,7 +6,7 @@ #define AP_BATTERY_AD7091R5_ENABLED (BOARD_FLASH_SIZE > 1024) #endif -#if AP_BATTERY_AD7091R5_ENABLED + #include #include @@ -71,5 +71,3 @@ class AP_BattMonitor_AD7091R5 : public AP_BattMonitor_Backend AP_Int8 _volt_pin; // board pin used to measure battery voltage AP_Int8 _curr_pin; // board pin used to measure battery current }; - -#endif // AP_BATTERY_AD7091R5_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index 7c65c84fa672c3..b0539b11ef4b67 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_ANALOG_ENABLED + #include #include @@ -126,5 +126,3 @@ bool AP_BattMonitor_Analog::has_current() const { return ((AP_BattMonitor::Type)_params._type.get() == AP_BattMonitor::Type::ANALOG_VOLTAGE_AND_CURRENT); } - -#endif // AP_BATTERY_ANALOG_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 2c541bd78c95dd..b7e07261a8e0f2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -292,12 +292,12 @@ void AP_BattMonitor_Backend::update_esc_telem_outbound() // returns true if battery monitor provides temperature bool AP_BattMonitor_Backend::get_temperature(float &temperature) const { -#if AP_TEMPERATURE_SENSOR_ENABLED + if (_state.temperature_external_use) { temperature = _state.temperature_external; return true; } -#endif + temperature = _state.temperature; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 12703d93d0b74b..42ce35bd149fb3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -104,9 +104,9 @@ class AP_BattMonitor_Backend return (uint16_t(_params._options.get()) & uint16_t(option)) != 0; } -#if AP_BATTERY_SCRIPTING_ENABLED + virtual bool handle_scripting(const BattMonitorScript_State &battmon_state) { return false; } -#endif + protected: AP_BattMonitor &_mon; // reference to front-end @@ -127,7 +127,7 @@ class AP_BattMonitor_Backend float _resistance_current_ref; // current used for maximum resistance calculation }; -#if AP_BATTERY_SCRIPTING_ENABLED + struct BattMonitorScript_State { float voltage; // Battery voltage in volts bool healthy; // True if communicating properly @@ -145,4 +145,4 @@ struct BattMonitorScript_State { float consumed_wh=nanf(""); // Total energy drawn since start-up in watt hours float temperature=nanf(""); // Battery temperature in degrees Celsius }; -#endif // AP_BATTERY_SCRIPTING_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp index 9907a798afcb0d..748519f5e8c8fd 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp @@ -15,7 +15,7 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_BEBOP_ENABLED +#if AP_BATTERY_BEBOP_ENABLED && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP) #include #include diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp index 5467e178c47376..90a25be4f10708 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.cpp @@ -15,7 +15,7 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_EFI_ENABLED + #include #include @@ -50,5 +50,3 @@ void AP_BattMonitor_EFI::read() _state.healthy = true; _state.last_time_micros = AP_HAL::micros(); } - -#endif // AP_BATTERY_EFI_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h index 14e12e60a2554e..7dec8c5a1e52bf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_EFI.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_Backend.h" -#if AP_BATTERY_EFI_ENABLED + class AP_BattMonitor_EFI : public AP_BattMonitor_Backend { @@ -22,4 +22,3 @@ class AP_BattMonitor_EFI : public AP_BattMonitor_Backend return true; } }; -#endif // AP_BATTERY_EFI_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.cpp index 9765a09da52f5f..29db909ffc0963 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_FUELFLOW_ENABLED + #include "AP_BattMonitor_FuelFlow.h" @@ -127,5 +127,3 @@ void AP_BattMonitor_FuelFlow::read() // map consumed_wh using fixed voltage of 1 _state.consumed_wh = _state.consumed_mah; } - -#endif // AP_BATTERY_FUELFLOW_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h b/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h index e8934307034a80..41bb99a935b7f9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelFlow.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_Analog.h" -#if AP_BATTERY_FUELFLOW_ENABLED + #include "AP_BattMonitor.h" @@ -35,5 +35,3 @@ class AP_BattMonitor_FuelFlow : public AP_BattMonitor_Analog int8_t last_pin = -1; }; - -#endif // AP_BATTERY_FUELFLOW_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp index f1dfead2bba7bf..126b09bd955eda 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp @@ -17,7 +17,7 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED + #include #include @@ -161,5 +161,3 @@ void AP_BattMonitor_FuelLevel_Analog::read() // record time _state.last_time_micros = tnow; } - -#endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h index 5f3e0530c40cb8..4ebfdc399885ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.h @@ -19,7 +19,7 @@ #include "AP_BattMonitor_Backend.h" -#if AP_BATTERY_FUELLEVEL_ANALOG_ENABLED + #include #include "AP_BattMonitor.h" @@ -60,5 +60,3 @@ class AP_BattMonitor_FuelLevel_Analog : public AP_BattMonitor_Backend LowPassFilterFloat _voltage_filter; }; - -#endif // AP_BATTERY_FUELLEVEL_ANALOG_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp index 161af7fe2b7127..89b924637eb0fb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_FUELLEVEL_PWM_ENABLED + #include @@ -66,5 +66,3 @@ void AP_BattMonitor_FuelLevel_PWM::read() // map consumed_wh using fixed voltage of 1 _state.consumed_wh = _state.consumed_mah; } - -#endif // AP_BATTERY_FUELLEVEL_PWM_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h index f5a84bb7b2004f..286b6fc3de81e1 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_PWM.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_Analog.h" -#if AP_BATTERY_FUELLEVEL_PWM_ENABLED + #include "AP_BattMonitor.h" @@ -28,5 +28,3 @@ class AP_BattMonitor_FuelLevel_PWM : public AP_BattMonitor_Analog AP_HAL::PWMSource pwm_source; }; - -#endif // AP_BATTERY_FUELLEVEL_PWM_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp index d59a5f2dbba4a8..11b04e466a7b24 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_INA239_ENABLED +#if AP_BATTERY_INA239_ENABLED && !defined(HAL_LINUX) #include #include diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h index 08757090193413..27e4b3dfbafb54 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h @@ -5,7 +5,7 @@ #include "AP_BattMonitor_Backend.h" #include -#if AP_BATTERY_INA239_ENABLED +#if AP_BATTERY_INA239_ENABLED && !defined(HAL_LINUX) class AP_BattMonitor_INA239 : public AP_BattMonitor_Backend { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index 94be30abc7b9f2..9c3afabcf53663 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_INA2XX_ENABLED + /* supports INA226, INA228 and INA238 I2C battery monitors @@ -369,5 +369,3 @@ bool AP_BattMonitor_INA2XX::get_temperature(float &temp) const temp = temperature; return has_temp; } - -#endif // AP_BATTERY_INA2XX_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h index b23b4420eacfc8..1cc2baf5442a7c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h @@ -6,7 +6,7 @@ #include #include -#if AP_BATTERY_INA2XX_ENABLED + class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend { @@ -69,5 +69,3 @@ class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend bool has_temp; }; - -#endif // AP_BATTERY_INA2XX_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp index f64b39df6dc44a..17c7fceb6f3359 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_LTC2946_ENABLED +#if AP_BATTERY_LTC2946_ENABLED && !defined(HAL_LINUX) #include #include diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h index e365c3aa11b6f2..5584a1c97df4c2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.h @@ -5,7 +5,7 @@ #include "AP_BattMonitor_Backend.h" #include -#if AP_BATTERY_LTC2946_ENABLED +#if AP_BATTERY_LTC2946_ENABLED && !defined(HAL_LINUX) class AP_BattMonitor_LTC2946 : public AP_BattMonitor_Backend { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp index e7b846258dd4fb..1c58eaaf24390f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_SCRIPTING_ENABLED + #include "AP_BattMonitor_Scripting.h" @@ -81,5 +81,3 @@ bool AP_BattMonitor_Scripting::handle_scripting(const BattMonitorScript_State &b last_update_us = now_us; return true; } - -#endif // AP_BATTERY_SCRIPTING_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h index 382437d0aa55ea..93a3f82e3b3273 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_SCRIPTING_ENABLED + #include "AP_BattMonitor_Backend.h" @@ -29,5 +29,3 @@ class AP_BattMonitor_Scripting : public AP_BattMonitor_Backend HAL_Semaphore sem; }; - -#endif // AP_BATTERY_SCRIPTING_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index 2a7401d3267a83..2b5606e7eb7efa 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_SUM_ENABLED + #include #include @@ -97,5 +97,3 @@ AP_BattMonitor_Sum::read() _state.last_time_micros = tnow_us; } } - -#endif // AP_BATTERY_SUM_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h index 1e882bb3339ffa..c032fc753a9691 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.h @@ -2,7 +2,7 @@ #include "AP_BattMonitor_Backend.h" -#if AP_BATTERY_SUM_ENABLED + #include "AP_BattMonitor.h" @@ -32,5 +32,3 @@ class AP_BattMonitor_Sum : public AP_BattMonitor_Backend uint8_t _instance; bool _has_current; }; - -#endif // AP_BATTERY_SUM_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp index acf807ff4c6545..d956a0dc109367 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp @@ -1,6 +1,6 @@ #include "AP_BattMonitor_config.h" -#if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED + #include #include "AP_BattMonitor_Synthetic_Current.h" @@ -71,5 +71,3 @@ AP_BattMonitor_Synthetic_Current::read() _state.last_time_micros = tnow; } - -#endif // AP_BATTERY_SYNTHETIC_CURRENT_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h index 0a49d0b40538cd..7dd5f270d8b4fc 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.h @@ -3,7 +3,7 @@ #include "AP_BattMonitor.h" #include "AP_BattMonitor_Analog.h" -#if AP_BATTERY_SYNTHETIC_CURRENT_ENABLED + class AP_BattMonitor_Synthetic_Current : public AP_BattMonitor_Analog { public: @@ -28,4 +28,3 @@ class AP_BattMonitor_Synthetic_Current : public AP_BattMonitor_Analog AP_Float _max_voltage; /// maximum battery voltage used in current calculation }; -#endif diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 1457d86b6235d5..6bd169d969a2d9 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -238,11 +238,11 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @User: Standard AP_GROUPINFO("SAFETYOPTION", 13, AP_BoardConfig, state.safety_option, BOARD_SAFETY_OPTION_DEFAULT), -#if AP_RTC_ENABLED + // @Group: RTC // @Path: ../AP_RTC/AP_RTC.cpp AP_SUBGROUPINFO(rtc, "RTC", 14, AP_BoardConfig, AP_RTC), -#endif + #if HAL_HAVE_BOARD_VOLTAGE // @Param: VBUS_MIN @@ -346,7 +346,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { AP_GROUPINFO("HEAT_LOWMGN", 23, AP_BoardConfig, heater.imu_arming_temperature_margin_low, HAL_IMU_TEMP_MARGIN_LOW_DEFAULT), #endif -#if AP_SDCARD_STORAGE_ENABLED + // @Param: SD_MISSION // @DisplayName: SDCard Mission size // @Description: This sets the amount of storage in kilobytes reserved on the microsd card in mission.stg for waypoint storage. Each waypoint uses 15 bytes. @@ -362,7 +362,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("SD_FENCE", 29, AP_BoardConfig, sdcard_storage.fence_kb, 0), -#endif + // index 25 used by SER5_RTSCTS // index 26 used by SER3_RTSCTS @@ -388,9 +388,9 @@ void AP_BoardConfig::init() board_setup(); -#if AP_RTC_ENABLED + AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW); -#endif + if (_boot_delay_ms > 0) { uint16_t delay_ms = uint16_t(_boot_delay_ms.get()); diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 938180a238bb95..393621409f0410 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -216,7 +216,7 @@ class AP_BoardConfig { bool get_board_heater_arming_temperature(int8_t &temperature) const; #endif -#if AP_SDCARD_STORAGE_ENABLED + // return number of kb of mission storage to use on microSD static uint16_t get_sdcard_mission_kb(void) { return _singleton? _singleton->sdcard_storage.mission_kb.get() : 0; @@ -226,7 +226,7 @@ class AP_BoardConfig { static uint16_t get_sdcard_fence_kb(void) { return _singleton? _singleton->sdcard_storage.fence_kb.get() : 0; } -#endif + private: static AP_BoardConfig *_singleton; @@ -246,12 +246,12 @@ class AP_BoardConfig { AP_Int8 io_dshot; } state; -#if AP_SDCARD_STORAGE_ENABLED + struct { AP_Int16 mission_kb; AP_Int16 fence_kb; } sdcard_storage; -#endif + #if AP_FEATURE_BOARD_DETECT static enum px4_board_type px4_configured_board; @@ -297,10 +297,10 @@ class AP_BoardConfig { AP_Radio _radio; #endif -#if AP_RTC_ENABLED + // real-time-clock; private because access is via the singleton AP_RTC rtc; -#endif + #if HAL_HAVE_BOARD_VOLTAGE AP_Float _vbus_min; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 6eb3fb497bee94..ceae81646c3544 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -434,17 +434,17 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem _have_data = true; volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[esc_index]; -#if AP_TEMPERATURE_SENSOR_ENABLED + // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) || ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) || ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); -#else - const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); - const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE); -#endif + + + + if (has_temperature) { telemdata.temperature_cdeg = new_data.temperature_cdeg; diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index b7642da19cfcf4..c3277aaf6f5baa 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -37,43 +37,43 @@ static AP_Filesystem_Backend fs_local; int errno; #endif -#if AP_FILESYSTEM_ROMFS_ENABLED + #include "AP_Filesystem_ROMFS.h" static AP_Filesystem_ROMFS fs_romfs; -#endif -#if AP_FILESYSTEM_PARAM_ENABLED + + #include "AP_Filesystem_Param.h" static AP_Filesystem_Param fs_param; -#endif -#if AP_FILESYSTEM_SYS_ENABLED + + #include "AP_Filesystem_Sys.h" static AP_Filesystem_Sys fs_sys; -#endif -#if AP_FILESYSTEM_MISSION_ENABLED + + #include "AP_Filesystem_Mission.h" static AP_Filesystem_Mission fs_mission; -#endif + /* mapping from filesystem prefix to backend */ const AP_Filesystem::Backend AP_Filesystem::backends[] = { { nullptr, fs_local }, -#if AP_FILESYSTEM_ROMFS_ENABLED + { "@ROMFS", fs_romfs }, -#endif -#if AP_FILESYSTEM_PARAM_ENABLED + + { "@PARAM", fs_param }, -#endif -#if AP_FILESYSTEM_SYS_ENABLED + + { "@SYS", fs_sys }, -#endif -#if AP_FILESYSTEM_MISSION_ENABLED + + { "@MISSION", fs_mission }, -#endif + }; extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp index dc22cbd607ab72..11b966abd9693c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp @@ -18,7 +18,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_MISSION_ENABLED + #include "AP_Filesystem.h" #include "AP_Filesystem_Mission.h" @@ -212,12 +212,12 @@ int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf) */ bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype) { -#if AP_MISSION_ENABLED + if (strcmp(name, "mission.dat") == 0) { mtype = MAV_MISSION_TYPE_MISSION; return true; } -#endif + if (strcmp(name, "fence.dat") == 0) { mtype = MAV_MISSION_TYPE_FENCE; @@ -264,7 +264,7 @@ bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype, uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const { switch (mtype) { -#if AP_MISSION_ENABLED + case MAV_MISSION_TYPE_MISSION: { auto *mission = AP::mission(); if (!mission) { @@ -272,7 +272,7 @@ uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const } return mission->num_commands(); } -#endif + case MAV_MISSION_TYPE_FENCE: { @@ -385,10 +385,10 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r) } switch (r.mtype) { -#if AP_MISSION_ENABLED + case MAV_MISSION_TYPE_MISSION: return finish_upload_mission(hdr, r, b); -#endif + #if HAL_RALLY_ENABLED case MAV_MISSION_TYPE_RALLY: return finish_upload_rally(hdr, r, b); @@ -405,7 +405,7 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r) return false; } -#if AP_MISSION_ENABLED + bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b) { auto *mission = AP::mission(); @@ -442,7 +442,7 @@ bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, cons } return true; } -#endif // AP_MISSION_ENABLED + bool AP_Filesystem_Mission::finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b) @@ -529,5 +529,3 @@ bool AP_Filesystem_Mission::finish_upload_rally(const struct header &hdr, const return success; } #endif // HAL_RALLY_ENABLED - -#endif // AP_FILESYSTEM_MISSION_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.h b/libraries/AP_Filesystem/AP_Filesystem_Mission.h index 88a97781e9c032..162a947737309c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.h @@ -17,7 +17,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_MISSION_ENABLED + #include "AP_Filesystem_backend.h" #include @@ -79,5 +79,3 @@ class AP_Filesystem_Mission : public AP_Filesystem_Backend // see if a block of memory is all zero bool all_zero(const uint8_t *b, uint8_t size) const; }; - -#endif // AP_FILESYSTEM_MISSION_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp index d29efe8f9df055..c633e0840cb177 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp @@ -18,7 +18,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_PARAM_ENABLED + #include "AP_Filesystem.h" #include "AP_Filesystem_Param.h" @@ -656,5 +656,3 @@ bool AP_Filesystem_Param::finish_upload(const rfile &r) } return true; } - -#endif // AP_FILESYSTEM_PARAM_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.h b/libraries/AP_Filesystem/AP_Filesystem_Param.h index 96f5b7d0506160..52a9140d5679e6 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.h @@ -17,7 +17,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_PARAM_ENABLED + #include "AP_Filesystem_backend.h" #include @@ -86,5 +86,3 @@ class AP_Filesystem_Param : public AP_Filesystem_Backend bool finish_upload(const rfile &r); bool param_upload_parse(const rfile &r, bool &need_retry); }; - -#endif // AP_FILESYSTEM_PARAM_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp index 930b4f62682c78..bceeea7550db49 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp @@ -18,7 +18,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_ROMFS_ENABLED + #include "AP_Filesystem.h" #include "AP_Filesystem_ROMFS.h" @@ -261,5 +261,3 @@ void AP_Filesystem_ROMFS::unload_file(FileData *fd) { AP_ROMFS::free(fd->data); } - -#endif // AP_FILESYSTEM_ROMFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h index a5176aeaf44ba2..d2dffa558cb9a1 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h @@ -17,7 +17,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_ROMFS_ENABLED + #include "AP_Filesystem_backend.h" @@ -72,5 +72,3 @@ class AP_Filesystem_ROMFS : public AP_Filesystem_Backend struct dirent de; } dir[max_open_dir]; }; - -#endif // AP_FILESYSTEM_ROMFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 9eadd7f607b407..a3be335000143d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -20,7 +20,7 @@ #include "AP_Filesystem.h" #include "AP_Filesystem_Sys.h" -#if AP_FILESYSTEM_SYS_ENABLED + #include #include @@ -294,5 +294,3 @@ int AP_Filesystem_Sys::stat(const char *pathname, struct stat *stbuf) } return 0; } - -#endif // AP_FILESYSTEM_SYS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.h b/libraries/AP_Filesystem/AP_Filesystem_Sys.h index e9d186e70fc505..7041f28fa5d942 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.h @@ -19,7 +19,7 @@ #include "AP_Filesystem_config.h" -#if AP_FILESYSTEM_SYS_ENABLED + class ExpandingString; @@ -52,5 +52,3 @@ class AP_Filesystem_Sys : public AP_Filesystem_Backend ExpandingString *str; } file[max_open_file]; }; - -#endif // AP_FILESYSTEM_SYS_ENABLED diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 99b705fdccdaf3..220045db0dc9a2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -968,14 +968,14 @@ void AP_GPS::update_instance(uint8_t instance) (void)data_should_be_logged; #endif -#if AP_RTC_ENABLED + if (state[instance].status >= GPS_OK_FIX_3D) { const uint64_t now = time_epoch_usec(instance); if (now != 0) { AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS); } } -#endif + } diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index cd44c7b3644d67..338ebc5227e729 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -40,9 +40,9 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; void RCInput::init() { -#if AP_RCPROTOCOL_ENABLED + AP::RC().init(); -#endif + #if HAL_USE_ICU == TRUE //attach timer channel on which the signal will be received @@ -155,7 +155,7 @@ void RCInput::_timer_tick(void) RCSource source = last_source; #endif -#if AP_RCPROTOCOL_ENABLED + AP_RCProtocol &rcprot = AP::RC(); #if HAL_USE_ICU == TRUE @@ -178,7 +178,7 @@ void RCInput::_timer_tick(void) } #endif -#endif // AP_RCPROTOCOL_ENABLED + #if HAL_WITH_IO_MCU uint32_t now = AP_HAL::millis(); @@ -190,7 +190,7 @@ void RCInput::_timer_tick(void) const bool have_iocmu_rc = false; #endif -#if AP_RCPROTOCOL_ENABLED + if (rcprot.new_input() && !have_iocmu_rc) { WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = AP_HAL::micros(); @@ -204,7 +204,7 @@ void RCInput::_timer_tick(void) source = rcprot.using_uart() ? RCSource::RCPROT_BYTES : RCSource::RCPROT_PULSES; #endif } -#endif // AP_RCPROTOCOL_ENABLED + #if HAL_RCINPUT_WITH_AP_RADIO if (radio && radio->last_recv_us() != last_radio_us && !have_iocmu_rc) { @@ -264,10 +264,10 @@ bool RCInput::rc_bind(int dsmMode) } #endif -#if AP_RCPROTOCOL_ENABLED + // ask AP_RCProtocol to start a bind AP::RC().start_bind(); -#endif + #if HAL_RCINPUT_WITH_AP_RADIO if (radio) { diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 8deec9fda16782..2937e7858966a6 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -543,12 +543,12 @@ bool Util::get_persistent_params(ExpandingString &str) const ins->get_persistent_params(str); } #endif -#if AP_OPENDRONEID_ENABLED + const auto *odid = AP_OpenDroneID::get_singleton(); if (odid) { odid->get_persistent_params(str); } -#endif + if (str.has_failed_allocation() || str.get_length() <= strlen(persistent_header)) { // no data return false; diff --git a/libraries/AP_HAL_ESP32/RCInput.cpp b/libraries/AP_HAL_ESP32/RCInput.cpp index 704877962d4e08..9218d5f63a25a0 100644 --- a/libraries/AP_HAL_ESP32/RCInput.cpp +++ b/libraries/AP_HAL_ESP32/RCInput.cpp @@ -25,9 +25,9 @@ void RCInput::init() if (_init) { return; } -#if AP_RCPROTOCOL_ENABLED + AP::RC().init(); -#endif + #ifdef HAL_ESP32_RCIN sig_reader.init(); @@ -89,7 +89,7 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len) void RCInput::_timer_tick(void) { -#if AP_RCPROTOCOL_ENABLED + if (!_init) { return; } @@ -126,5 +126,5 @@ void RCInput::_timer_tick(void) #endif #endif -#endif // AP_RCPROTOCOL_ENABLED + } diff --git a/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp b/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp index 9d6ab64034c590..a80dcd58120d0a 100644 --- a/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp +++ b/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp @@ -291,14 +291,14 @@ void AP_Hott_Telem::send_GPS(void) msg.home_direction = degrees(atan2f(home_vec.y, home_vec.x)) * 0.5 + 0.5; -#if AP_RTC_ENABLED + AP_RTC &rtc = AP::rtc(); { WITH_SEMAPHORE(rtc.get_semaphore()); uint16_t ms; rtc.get_system_clock_utc(msg.gps_time_h, msg.gps_time_m, msg.gps_time_s, ms); } -#endif + send_packet((const uint8_t *)&msg, sizeof(msg)); } diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 3208f8cf6e2549..15816fb34fd1b9 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -583,15 +583,15 @@ uint32_t AP_Logger_File::_get_log_time(const uint16_t log_num) // it is the file we are currently writing free(fname); write_fd_semaphore.give(); -#if AP_RTC_ENABLED + uint64_t utc_usec; if (!AP::rtc().get_utc_usec(utc_usec)) { return 0; } return utc_usec / 1000000U; -#else - return 0; -#endif + + + } write_fd_semaphore.give(); } @@ -845,10 +845,10 @@ void AP_Logger_File::start_new_log(void) #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS // remember if we had utc time when we opened the file -#if AP_RTC_ENABLED + uint64_t utc_usec; _need_rtc_update = !AP::rtc().get_utc_usec(utc_usec); -#endif + #endif // create the log directory if need be diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 405f41ac2cbd02..38d28934a863d2 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -133,7 +133,7 @@ bool AP_Logger_Backend::Write_Parameter(const AP_Param *ap, return Write_Parameter(name, ap->cast_to_float(type), default_val); } -#if AP_RC_CHANNEL_ENABLED + // Write an RCIN packet void AP_Logger::Write_RCIN(void) { @@ -177,7 +177,7 @@ void AP_Logger::Write_RCIN(void) }; WriteBlock(&pkt2, sizeof(pkt2)); } -#endif // AP_RC_CHANNEL_ENABLED + // Write an SERVO packet void AP_Logger::Write_RCOUT(void) @@ -318,13 +318,13 @@ bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission, return WriteBlock(&pkt, sizeof(pkt)); } -#if AP_MISSION_ENABLED + bool AP_Logger_Backend::Write_EntireMission() { // kick off asynchronous write: return _startup_messagewriter->writeentiremission(); } -#endif + // Write a text message to the log bool AP_Logger_Backend::Write_Message(const char *message) diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 3918fc6226c6b3..3d9cbb300d0cec 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -52,9 +52,9 @@ void LoggerMessageWriter_DFLogStart::reset() _fmt_done = false; _params_done = false; _writesysinfo.reset(); -#if AP_MISSION_ENABLED + _writeentiremission.reset(); -#endif + #if HAL_LOGGER_RALLY_ENABLED _writeallrallypoints.reset(); #endif @@ -184,14 +184,14 @@ void LoggerMessageWriter_DFLogStart::process() return; } } -#if AP_MISSION_ENABLED + if (!_writeentiremission.finished()) { _writeentiremission.process(); if (!_writeentiremission.finished()) { return; } } -#endif + #if HAL_LOGGER_RALLY_ENABLED if (!_writeallrallypoints.finished()) { _writeallrallypoints.process(); @@ -231,7 +231,7 @@ void LoggerMessageWriter_DFLogStart::process() _finished = true; } -#if AP_MISSION_ENABLED + bool LoggerMessageWriter_DFLogStart::writeentiremission() { if (stage != Stage::DONE) { @@ -242,7 +242,7 @@ bool LoggerMessageWriter_DFLogStart::writeentiremission() _writeentiremission.reset(); return true; } -#endif + #if HAL_LOGGER_RALLY_ENABLED bool LoggerMessageWriter_DFLogStart::writeallrallypoints() diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h index 2e28503676faf5..a7cb6dbc7d11fa 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.h +++ b/libraries/AP_Logger/LoggerMessageWriter.h @@ -98,9 +98,9 @@ class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter { public: LoggerMessageWriter_DFLogStart() : _writesysinfo() -#if AP_MISSION_ENABLED + , _writeentiremission() -#endif + #if HAL_RALLY_ENABLED , _writeallrallypoints() #endif @@ -113,9 +113,9 @@ class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter { void set_logger_backend(class AP_Logger_Backend *backend) override final { LoggerMessageWriter::set_logger_backend(backend); _writesysinfo.set_logger_backend(backend); -#if AP_MISSION_ENABLED + _writeentiremission.set_logger_backend(backend); -#endif + #if HAL_RALLY_ENABLED _writeallrallypoints.set_logger_backend(backend); #endif @@ -133,9 +133,9 @@ class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter { // reset some writers so we push stuff out to logs again. Will // only work if we are in state DONE! -#if AP_MISSION_ENABLED + bool writeentiremission(); -#endif + #if HAL_RALLY_ENABLED bool writeallrallypoints(); #endif @@ -177,9 +177,9 @@ class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter { LoggerMessageWriter_WriteSysInfo _writesysinfo; -#if AP_MISSION_ENABLED + LoggerMessageWriter_WriteEntireMission _writeentiremission; -#endif + #if HAL_RALLY_ENABLED LoggerMessageWriter_WriteAllRallyPoints _writeallrallypoints; #endif diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index e479b9c6233aef..1e66299018fad6 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -1024,13 +1024,13 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) { tm localtime_tm {}; // year is relative to 1900 uint64_t time_usec = 0; -#if AP_RTC_ENABLED + if (AP::rtc().get_utc_usec(time_usec)) { // may fail, leaving time_unix at 0 const time_t time_sec = time_usec / 1000000; struct tm tmd {}; localtime_tm = *gmtime_r(&time_sec, &tmd); } -#endif + const struct PACKED { uint16_t year; uint8_t mon; @@ -1053,7 +1053,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) return MSP_RESULT_ACK; } -#if AP_RC_CHANNEL_ENABLED + MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) { #if AP_RCMAPPER_ENABLED @@ -1087,7 +1087,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) return MSP_RESULT_ERROR; #endif } -#endif // AP_RC_CHANNEL_ENABLED + MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_board_info(sbuf_t *dst) { @@ -1199,14 +1199,14 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) } // flash rtc if no time available -#if AP_RTC_ENABLED + uint64_t time_usec; if (!AP::rtc().get_utc_usec(time_usec)) { BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME); } -#else - BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME); -#endif + + + // flash rssi if disabled float rssi; if (!get_rssi(rssi)) { diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index a49296d7136271..616d67c801f15e 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -3,7 +3,7 @@ #include "AP_Mission_config.h" -#if AP_MISSION_ENABLED + #include #include @@ -61,7 +61,7 @@ HAL_Semaphore AP_Mission::_rsem; /// init - initialises this library including checks the version in eeprom matches this library void AP_Mission::init() { -#if AP_SDCARD_STORAGE_ENABLED + // check for extra storage on microsd const auto *bc = AP::boardConfig(); if (bc != nullptr) { @@ -74,7 +74,7 @@ void AP_Mission::init() } } } -#endif + // work out maximum index for our storage size if (_storage.size() >= AP_MISSION_EEPROM_COMMAND_SIZE+4) { @@ -412,10 +412,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd) } switch (cmd.id) { -#if AP_RC_CHANNEL_ENABLED + case MAV_CMD_DO_AUX_FUNCTION: return start_command_do_aux_function(cmd); -#endif + case MAV_CMD_DO_GRIPPER: return start_command_do_gripper(cmd); @@ -2946,5 +2946,3 @@ AP_Mission *mission() } } - -#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 3916a63f79014c..7ea67c92347be7 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -755,11 +755,11 @@ class AP_Mission bool is_valid_index(const uint16_t index) const { return index < _cmd_total; } -#if AP_SDCARD_STORAGE_ENABLED + bool failed_sdcard_storage(void) const { return _failed_sdcard_storage; } -#endif + private: static AP_Mission *_singleton; @@ -893,9 +893,9 @@ class AP_Mission // maximum number of commands that will fit in storage uint16_t _commands_max; -#if AP_SDCARD_STORAGE_ENABLED + bool _failed_sdcard_storage; -#endif + // fast call to get command ID of a mission index uint16_t get_command_id(uint16_t index) const; diff --git a/libraries/AP_Mission/AP_Mission_ChangeDetector.cpp b/libraries/AP_Mission/AP_Mission_ChangeDetector.cpp index 6ebf7ba75e0836..dc8d6a6bbad814 100644 --- a/libraries/AP_Mission/AP_Mission_ChangeDetector.cpp +++ b/libraries/AP_Mission/AP_Mission_ChangeDetector.cpp @@ -3,7 +3,7 @@ #include "AP_Mission_ChangeDetector.h" -#if AP_MISSION_ENABLED + // detect external changes to mission bool AP_Mission_ChangeDetector::check_for_mission_change() @@ -58,5 +58,3 @@ bool AP_Mission_ChangeDetector::check_for_mission_change() // mission has changed if upcoming command contents have changed without the current command index changing return cmds_changed && !curr_cmd_idx_changed; } - -#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mission/AP_Mission_ChangeDetector.h b/libraries/AP_Mission/AP_Mission_ChangeDetector.h index 580f5c4956b932..da72f85205ff82 100644 --- a/libraries/AP_Mission/AP_Mission_ChangeDetector.h +++ b/libraries/AP_Mission/AP_Mission_ChangeDetector.h @@ -14,7 +14,7 @@ #include "AP_Mission.h" -#if AP_MISSION_ENABLED + /// @class AP_Mission_ChangeDetector /// @brief Mission command change detector @@ -37,5 +37,3 @@ class AP_Mission_ChangeDetector AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; // local copy of the next few mission commands } mis_change_detect; }; - -#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 12189cb5e45435..74d59c42bcd945 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -1,6 +1,6 @@ #include "AP_Mission_config.h" -#if AP_MISSION_ENABLED + #include "AP_Mission.h" @@ -14,7 +14,7 @@ #include #include -#if AP_RC_CHANNEL_ENABLED + bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) { const RC_Channel::AUX_FUNC function = (RC_Channel::AUX_FUNC)cmd.content.auxfunction.function; @@ -33,7 +33,7 @@ bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command rc().run_aux_function(function, pos, RC_Channel::AuxFuncTriggerSource::MISSION); return true; } -#endif // AP_RC_CHANNEL_ENABLED + bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd) @@ -346,5 +346,3 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss // if we got this far then message is not handled return false; } - -#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c8e2ae78d2f20b..610ea1cf59db7e 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -70,7 +70,7 @@ void AP_Mount_Siyi::update() request_configuration(); } -#if AP_RTC_ENABLED + // send UTC time to the camera if (sent_time_count < 5) { uint64_t utc_usec; @@ -79,7 +79,7 @@ void AP_Mount_Siyi::update() sent_time_count++; } } -#endif + } // request attitude at regular intervals diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index df163889b8f0ed..fbc8ab72e60d35 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -672,7 +672,7 @@ bool AP_Mount_Viewpro::send_m_ahrs() return false; } -#if AP_RTC_ENABLED + // get date and time uint16_t year, ms; uint8_t month, day, hour, min, sec; @@ -681,10 +681,10 @@ bool AP_Mount_Viewpro::send_m_ahrs() } uint16_t date = ((year-2000) & 0x7f) | (((month+1) & 0x0F) << 7) | ((day & 0x1F) << 11); uint64_t second_hundredths = (((hour * 60 * 60) + (min * 60) + sec) * 100) + (ms * 0.1); -#else - const uint16_t date = 0; - const uint64_t second_hundredths = 0; -#endif + + + + // get vehicle velocity in m/s in NED Frame Vector3f vel_NED; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 3d551885ce35dc..03cb8897ce566e 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -547,14 +547,14 @@ void AP_Mount_Xacti::handle_gnss_status_req(AP_DroneCAN* ap_dronecan, const Cana // get date and time uint16_t year, ms; uint8_t month, day, hour, min, sec; -#if AP_RTC_ENABLED + if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) { year = month = day = hour = min = sec = 0; } -#else - year = month = day = hour = min = sec = 0; - (void)ms; -#endif + + + + // send xacti specific gnss status message com_xacti_GnssStatus xacti_gnss_status_msg {}; @@ -988,14 +988,14 @@ bool AP_Mount_Xacti::set_datetime(uint32_t now_ms) // get date and time uint16_t year, ms; uint8_t month, day, hour, min, sec; -#if AP_RTC_ENABLED + if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) { return false; } -#else - (void)ms; - return false; -#endif + + + + // date time is of the format YYYYMMDDHHMMSS (14 bytes) // convert month from 0~11 to 1~12 range diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp index 836445abeecb65..aa09c3b78f8507 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp @@ -96,13 +96,13 @@ void AP_NMEA_Output::update() // get time and date uint64_t time_usec; -#if AP_RTC_ENABLED + if (!AP::rtc().get_utc_usec(time_usec)) { return; } -#else - time_usec = 0; -#endif + + + uint32_t space_required = 0; diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index c033ab9fbe8ebb..a16aace74f90a6 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -529,7 +529,7 @@ void AP_OSD::update_current_screen() return; } -#if AP_RC_CHANNEL_ENABLED + RC_Channel *channel = RC_Channels::rc_channel(rc_channel-1); if (channel == nullptr) { return; @@ -585,7 +585,7 @@ void AP_OSD::update_current_screen() break; } switch_debouncer = false; -#endif // AP_RC_CHANNEL_ENABLED + } //select next avaliable screen, do nothing if all screens disabled diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 8b6198c3fc342e..ee334be88c85dd 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -495,10 +495,10 @@ class AP_OSD_ParamScreen : public AP_OSD_AbstractScreen void modify_parameter(uint8_t number, Event ev); void modify_configured_parameter(uint8_t number, Event ev); -#if AP_RC_CHANNEL_ENABLED + Event map_rc_input_to_event() const; RC_Channel::AuxSwitchPos get_channel_pos(uint8_t rcmapchan) const; -#endif + uint8_t _selected_param = 1; MenuState _menu_state = MenuState::PARAM_SELECT; diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index eb8c0424427119..b8e8a37b23ab66 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -2404,7 +2404,7 @@ void AP_OSD_Screen::draw_aspd2(uint8_t x, uint8_t y) } -#if AP_RTC_ENABLED + void AP_OSD_Screen::draw_clk(uint8_t x, uint8_t y) { AP_RTC &rtc = AP::rtc(); @@ -2416,7 +2416,7 @@ void AP_OSD_Screen::draw_clk(uint8_t x, uint8_t y) backend->write(x, y, false, "%c%02u:%02u", SYMBOL(SYM_CLK), hour, min); } } -#endif + #if HAL_PLUSCODE_ENABLE void AP_OSD_Screen::draw_pluscode(uint8_t x, uint8_t y) @@ -2586,9 +2586,9 @@ void AP_OSD_Screen::draw(void) DRAW_SETTING(atemp); DRAW_SETTING(hdop); DRAW_SETTING(flightime); -#if AP_RTC_ENABLED + DRAW_SETTING(clk); -#endif + #if AP_VIDEOTX_ENABLED DRAW_SETTING(vtx_power); #endif diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp index 0e643d8d863ad9..f9e62a9e75ca44 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -29,7 +29,7 @@ #include "AP_OpenDroneID_config.h" -#if AP_OPENDRONEID_ENABLED + #include "AP_OpenDroneID.h" #include @@ -799,4 +799,3 @@ AP_OpenDroneID &opendroneid() } } -#endif //AP_OPENDRONEID_ENABLED diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h index 3ed65244acca36..0f1d156e7ea73a 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.h +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.h @@ -29,7 +29,7 @@ #include "AP_OpenDroneID_config.h" -#if AP_OPENDRONEID_ENABLED + #include #include @@ -212,5 +212,3 @@ namespace AP { AP_OpenDroneID &opendroneid(); }; - -#endif // AP_OPENDRONEID_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index b7b6bfdf945da9..5c3eb50f4776aa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -92,9 +92,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_GHST_ENABLED backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this); #endif -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MAVLinkRadio(*this); -#endif + #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this); #endif @@ -150,9 +150,9 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) uint32_t now = AP_HAL::millis(); bool searching = should_search(now); -#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); -#endif + if (_detected_protocol != AP_RCProtocol::NONE && !protocol_enabled(_detected_protocol)) { @@ -234,9 +234,9 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) uint32_t now = AP_HAL::millis(); bool searching = should_search(now); -#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); -#endif + if (_detected_protocol != AP_RCProtocol::NONE && !protocol_enabled(_detected_protocol)) { @@ -359,9 +359,9 @@ void AP_RCProtocol::check_added_uart(void) added.last_config_change_ms = AP_HAL::millis(); serial_configs[added.config_num].apply_to_uart(added.uart); } -#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); -#endif + const uint32_t current_baud = serial_configs[added.config_num].baud; process_handshake(current_baud); @@ -424,9 +424,9 @@ bool AP_RCProtocol::detect_async_protocol(rcprotocol_t protocol) return false; } -#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); -#endif + if (!protocol_enabled(protocol)) { return false; @@ -461,9 +461,9 @@ bool AP_RCProtocol::new_input() #if AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCProtocol::DRONECAN, #endif -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + AP_RCProtocol::MAVLINK_RADIO, -#endif + #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED AP_RCProtocol::JOYSTICK_SFML, #endif @@ -605,10 +605,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) case GHST: return "GHST"; #endif -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_RADIO: return "MAVRadio"; -#endif + #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED case JOYSTICK_SFML: return "SFML"; @@ -655,7 +655,7 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0; } -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { if (backend[AP_RCProtocol::MAVLINK_RADIO] == nullptr) { @@ -664,7 +664,7 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* backend[AP_RCProtocol::MAVLINK_RADIO]->update_radio_rc_channels(packet); }; -#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + namespace AP { AP_RCProtocol &RC() diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 6234e0a1a13099..394db6c65403c1 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,9 +20,9 @@ #include #include -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #include -#endif + #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 @@ -78,9 +78,9 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_GHST_ENABLED GHST = 14, #endif -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + MAVLINK_RADIO = 15, -#endif + #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED JOYSTICK_SFML = 16, #endif @@ -174,9 +174,9 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: #endif -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_RADIO: -#endif + #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED case JOYSTICK_SFML: #endif @@ -233,9 +233,9 @@ class AP_RCProtocol { } // handle mavlink radio -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); -#endif + private: void check_added_uart(void); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index e050db2dc1d974..0b42000ddbfceb 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -45,9 +45,9 @@ class AP_RCProtocol_Backend { virtual void update(void) {} // update from mavlink messages -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} -#endif + // get number of frames, ignoring failsafe uint32_t get_rc_frame_count(void) const { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index ce870cd0ac7753..0c919622ae196c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -373,10 +373,10 @@ void AP_RCProtocol_CRSF::update(void) _last_frame_time_us = now; } -#if AP_RC_CHANNEL_ENABLED + //Check if LQ is to be reported in place of RSSI _use_lq_for_rssi = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI); -#endif + } // write out a frame of any type diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index aaaf0fb2bf2d8f..01a33592cfbb10 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -53,11 +53,11 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { bool change_baud_rate(uint32_t baudrate); // bootstrap baudrate uint32_t get_bootstrap_baud_rate() const { -#if AP_RC_CHANNEL_ENABLED + return rc().option_is_enabled(RC_Channels::Option::ELRS_420KBAUD) ? ELRS_BAUDRATE : CRSF_BAUDRATE; -#else - return CRSF_BAUDRATE; -#endif + + + } // is the receiver active, used to detect power loss and baudrate changes diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp index c11fca5bc08588..690dfc4b10580e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp @@ -1,7 +1,7 @@ #include "AP_RCProtocol_config.h" -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #include "AP_RCProtocol_MAVLinkRadio.h" @@ -20,6 +20,3 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc add_input(count, rc_chan, failsafe); } - -#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h index cd80f502a3f870..b8e60d3d5b8a99 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h @@ -3,7 +3,7 @@ #include "AP_RCProtocol_config.h" -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #include "AP_RCProtocol.h" @@ -16,6 +16,3 @@ class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { // update from mavlink messages void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; }; - -#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp index de1e003ef8b1fc..dfecbea30edb60 100644 --- a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp @@ -557,15 +557,15 @@ void AP_Spektrum_Telem::calc_gps_status() _telem.gpsstat.speed = ((knots % 10000 / 1000) << 12) | ((knots % 1000 / 100) << 8) | ((knots % 100 / 10) << 4) | (knots % 10); // BCD, knots, format 3.1 uint16_t ms; uint8_t h, m, s; -#if AP_RTC_ENABLED + AP::rtc().get_system_clock_utc(h, m, s, ms); // BCD, format HH:MM:SS.S, format 6.1 // FIXME: the above call can fail! -#else - h = 0; - m = 0; - s = 0; - ms = 0; -#endif + + + + + + _telem.gpsstat.UTC = ((((h / 10) << 4) | (h % 10)) << 20) | ((((m / 10) << 4) | (m % 10)) << 12) | ((((s / 10) << 4) | (s % 10)) << 4) | (ms / 100) ; uint8_t nsats = AP::gps().num_sats(); _telem.gpsstat.numSats = ((nsats / 10) << 4) | (nsats % 10); // BCD, 0-99 diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index a57c3242852fd5..0f7cbe18d56642 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -1,6 +1,6 @@ #include "AP_RTC_config.h" -#if AP_RTC_ENABLED + #include "AP_RTC.h" @@ -311,5 +311,3 @@ AP_RTC &rtc() } } - -#endif // AP_RTC_ENABLED diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index a35887532b5473..bb30512e0210b1 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -2,7 +2,7 @@ #include "AP_RTC_config.h" -#if AP_RTC_ENABLED + #include @@ -82,5 +82,3 @@ class AP_RTC { namespace AP { AP_RTC &rtc(); }; - -#endif // AP_RTC_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index a4045b9837baf9..2958a9b56d6252 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -369,7 +369,7 @@ void AP_Scripting::thread(void) { void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd_in) { -#if AP_MISSION_ENABLED + if (!_enable) { return; } @@ -394,7 +394,7 @@ void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd AP_HAL::millis()}; mission_data->push(cmd); -#endif + } bool AP_Scripting::arming_checks(size_t buflen, char *buffer) const diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index a02d6add240d75..78aef61069ae19 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -100,7 +100,7 @@ class AP_Scripting ScriptingCANSensor *_CAN_dev2; #endif -#if AP_MISSION_ENABLED + // mission item buffer static const int mission_cmd_queue_size = 5; struct scripting_mission_cmd { @@ -111,7 +111,7 @@ class AP_Scripting uint32_t time_ms; }; ObjectBuffer * mission_data; -#endif + // PWMSource storage uint8_t num_pwm_source; diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index f47cc20bf51041..12da72a9d7e66b 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -221,7 +221,7 @@ int lua_mavlink_block_command(lua_State *L) { } #endif // HAL_GCS_ENABLED -#if AP_MISSION_ENABLED + int lua_mission_receive(lua_State *L) { binding_argcheck(L, 0); @@ -249,7 +249,7 @@ int lua_mission_receive(lua_State *L) { return 5; } -#endif // AP_MISSION_ENABLED + #if HAL_LOGGING_ENABLED int AP_Logger_Write(lua_State *L) { diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 0ca985081b8e0f..0206d8e336782c 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -542,14 +542,14 @@ void AP_SerialManager::init() AP_SERIALMANAGER_SLCAN_BUFSIZE_TX); break; -#if AP_RCPROTOCOL_ENABLED + case SerialProtocol_RCIN: if (!AP::RC().has_uart()) { AP::RC().add_uart(uart); } break; -#endif + case SerialProtocol_EFI: state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD); diff --git a/libraries/AP_Stats/AP_Stats.cpp b/libraries/AP_Stats/AP_Stats.cpp index 5a505342192f3f..fce537ad4a9b10 100644 --- a/libraries/AP_Stats/AP_Stats.cpp +++ b/libraries/AP_Stats/AP_Stats.cpp @@ -114,7 +114,7 @@ void AP_Stats::update() params.flttime.set_and_save_ifchanged(0); params.runtime.set_and_save_ifchanged(0); uint32_t system_clock = 0; // in seconds -#if AP_RTC_ENABLED + uint64_t rtc_clock_us; if (AP::rtc().get_utc_usec(rtc_clock_us)) { system_clock = rtc_clock_us / 1000000; @@ -122,7 +122,7 @@ void AP_Stats::update() // time base to Jan 1st 2016: system_clock -= 1451606400; } -#endif + params.reset.set_and_save_ifchanged(system_clock); copy_variables_from_parameters(); } diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp index 69ef84e41a6aca..2662a1620225b9 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp @@ -15,7 +15,7 @@ #include "AP_TemperatureSensor.h" -#if AP_TEMPERATURE_SENSOR_ENABLED + #include @@ -302,5 +302,3 @@ AP_TemperatureSensor &temperature_sensor() { return *AP_TemperatureSensor::get_singleton(); } }; - -#endif // AP_TEMPERATURE_SENSOR_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h index 1a9538d775b5ae..4e7db3635de17b 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h @@ -16,7 +16,7 @@ #include "AP_TemperatureSensor_config.h" -#if AP_TEMPERATURE_SENSOR_ENABLED + #include "AP_TemperatureSensor_Params.h" @@ -95,5 +95,3 @@ class AP_TemperatureSensor namespace AP { AP_TemperatureSensor &temperature_sensor(); }; - -#endif // AP_TEMPERATURE_SENSOR_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp index a32f8eb3e2b3db..7814b239a5a9c9 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp @@ -15,7 +15,7 @@ #include "AP_TemperatureSensor.h" -#if AP_TEMPERATURE_SENSOR_ENABLED + #include "AP_TemperatureSensor_Backend.h" #include @@ -101,5 +101,3 @@ void AP_TemperatureSensor_Backend::update_external_libraries(const float tempera } } - -#endif // AP_TEMPERATURE_SENSOR_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h index 10f24df9cfcea5..e09e7359c73598 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h @@ -16,7 +16,7 @@ #include "AP_TemperatureSensor.h" -#if AP_TEMPERATURE_SENSOR_ENABLED + #include #include @@ -56,5 +56,3 @@ class AP_TemperatureSensor_Backend private: HAL_Semaphore _sem; // used to copy from backend to frontend }; - -#endif // AP_TEMPERATURE_SENSOR_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp index b1e15333d40f44..ab593f0d12537d 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp @@ -16,7 +16,7 @@ #include "AP_TemperatureSensor_Params.h" #include "AP_TemperatureSensor.h" -#if AP_TEMPERATURE_SENSOR_ENABLED + #ifndef AP_TEMPERATURE_SENSOR_I2C_ADDR_DEFAULT #define AP_TEMPERATURE_SENSOR_I2C_ADDR_DEFAULT 0 @@ -73,5 +73,3 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = { AP_TemperatureSensor_Params::AP_TemperatureSensor_Params(void) { AP_Param::setup_object_defaults(this, var_info); } - -#endif // AP_TEMPERATURE_SENSOR_ENABLED diff --git a/libraries/AP_Terrain/TerrainMission.cpp b/libraries/AP_Terrain/TerrainMission.cpp index 0d496ab53f2f8f..b5c42646dacf73 100644 --- a/libraries/AP_Terrain/TerrainMission.cpp +++ b/libraries/AP_Terrain/TerrainMission.cpp @@ -34,7 +34,7 @@ extern const AP_HAL::HAL& hal; */ void AP_Terrain::update_mission_data(void) { -#if AP_MISSION_ENABLED + const AP_Mission *mission = AP::mission(); if (mission == nullptr) { return; @@ -111,7 +111,7 @@ void AP_Terrain::update_mission_data(void) next_mission_pos = 0; } } -#endif // AP_MISSION_ENABLED + } #if HAL_RALLY_ENABLED diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index 490416fdab8405..c644c95be2c67a 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -1,6 +1,6 @@ #include "AP_Tuning_config.h" -#if AP_TUNING_ENABLED + #include "AP_Tuning.h" @@ -343,5 +343,3 @@ const char *AP_Tuning::get_tuning_name(uint8_t parm) } return "UNKNOWN"; } - -#endif // AP_TUNING_ENABLED diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h index 88ffda0c4b08ab..60760d397f4a50 100644 --- a/libraries/AP_Tuning/AP_Tuning.h +++ b/libraries/AP_Tuning/AP_Tuning.h @@ -2,7 +2,7 @@ #include "AP_Tuning_config.h" -#if AP_TUNING_ENABLED + #include @@ -104,5 +104,3 @@ class AP_Tuning // parmset is in vehicle subclass var table AP_Int16 parmset; }; - -#endif // AP_TUNING_ENABLED diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 61821cd75eb07a..c04352213e1ced 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -118,17 +118,17 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(fence, "FENCE_", 14, AP_Vehicle, AC_Fence), -#if AP_OPENDRONEID_ENABLED + // @Group: DID_ // @Path: ../AP_OpenDroneID/AP_OpenDroneID.cpp AP_SUBGROUPINFO(opendroneid, "DID_", 15, AP_Vehicle, AP_OpenDroneID), -#endif -#if AP_TEMPERATURE_SENSOR_ENABLED + + // @Group: TEMP // @Path: ../AP_TemperatureSensor/AP_TemperatureSensor.cpp AP_SUBGROUPINFO(temperature_sensor, "TEMP", 16, AP_Vehicle, AP_TemperatureSensor), -#endif + #if HAL_NMEA_OUTPUT_ENABLED // @Group: NMEA_ @@ -462,18 +462,18 @@ void AP_Vehicle::setup() send_watchdog_reset_statustext(); -#if AP_OPENDRONEID_ENABLED + opendroneid.init(); -#endif + // init EFI monitoring #if HAL_EFI_ENABLED efi.init(); #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + temperature_sensor.init(); -#endif + #if AP_KDECAN_ENABLED kdecan.init(); @@ -611,18 +611,18 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if HAL_GENERATOR_ENABLED SCHED_TASK_CLASS(AP_Generator, &vehicle.generator, update, 10, 50, 235), #endif -#if AP_OPENDRONEID_ENABLED + SCHED_TASK_CLASS(AP_OpenDroneID, &vehicle.opendroneid, update, 10, 50, 236), -#endif + #if AP_NETWORKING_ENABLED SCHED_TASK_CLASS(AP_Networking, &vehicle.networking, update, 10, 50, 238), #endif #if OSD_ENABLED SCHED_TASK(publish_osd_info, 1, 10, 240), #endif -#if AP_TEMPERATURE_SENSOR_ENABLED + SCHED_TASK_CLASS(AP_TemperatureSensor, &vehicle.temperature_sensor, update, 5, 50, 242), -#endif + #if HAL_INS_ACCELCAL_ENABLED SCHED_TASK(accel_cal_update, 10, 100, 245), #endif @@ -913,7 +913,7 @@ void AP_Vehicle::reboot(bool hold_in_bootloader) #if OSD_ENABLED void AP_Vehicle::publish_osd_info() { -#if AP_MISSION_ENABLED + AP_Mission *mission = AP::mission(); if (mission == nullptr) { return; @@ -936,7 +936,7 @@ void AP_Vehicle::publish_osd_info() } nav_info.wp_number = mission->get_current_nav_index(); osd->set_nav_info(nav_info); -#endif + } #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 64b88e1ad2a498..1a91998d355f61 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -388,9 +388,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_ESC_Telem esc_telem; #endif -#if AP_OPENDRONEID_ENABLED + AP_OpenDroneID opendroneid; -#endif + #if HAL_MSP_ENABLED AP_MSP msp; @@ -447,9 +447,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AC_Fence fence; -#if AP_TEMPERATURE_SENSOR_ENABLED + AP_TemperatureSensor temperature_sensor; -#endif + AP_Scripting scripting; diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 5e1c0e15e11a53..753dc065967793 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -189,7 +189,7 @@ void GCS::update_sensor_status_flags() } #endif -#if AP_BARO_ENABLED + const AP_Baro &barometer = AP::baro(); if (barometer.num_instances() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; @@ -198,7 +198,7 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; } } -#endif + const AP_GPS &gps = AP::gps(); @@ -346,7 +346,7 @@ void GCS::update_sensor_status_flags() } #endif -#if AP_RC_CHANNEL_ENABLED + if (rc().has_ever_seen_rc_input()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; @@ -354,7 +354,7 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } } -#endif + update_vehicle_sensor_status_flags(); } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 11baabe325009a..d4e3ae369c092e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -652,10 +652,10 @@ class GCS_MAVLINK virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg); virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg); -#if AP_MISSION_ENABLED + virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_jump_tag(const mavlink_command_int_t &packet); -#endif + MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet); void handle_command_long(const mavlink_message_t &msg); MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index eab771d93d080a..6a681d707d5fa9 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -101,9 +101,9 @@ #include #include -#if AP_RCPROTOCOL_ENABLED + #include -#endif + #if HAL_WITH_IO_MCU #include @@ -893,29 +893,29 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) if (msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) { mavlink_mission_item_t mission_item; mavlink_msg_mission_item_decode(&msg, &mission_item); -#if AP_MISSION_ENABLED + MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, mission_item_int); if (ret != MAV_MISSION_ACCEPTED) { const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type; send_mission_ack(msg, type, ret); return; } -#else - // No mission library, so we can't convert from MISSION_ITEM - // to MISSION_ITEM_INT. Since we shouldn't be receiving fence - // or rally items via MISSION_ITEM, we don't need to work hard - // here, just fail: - const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item.mission_type; - send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED); - return; -#endif + + + + + + + + + send_mission_item_warning = true; } else { mavlink_msg_mission_item_int_decode(&msg, &mission_item_int); } const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type; -#if AP_MISSION_ENABLED + const uint8_t current = mission_item_int.current; if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) { @@ -943,7 +943,7 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result); return; } -#endif + // not a guided-mode reqest MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type); @@ -1999,9 +1999,9 @@ void GCS_MAVLINK::log_mavlink_stats() void GCS_MAVLINK::send_system_time() const { uint64_t time_unix = 0; -#if AP_RTC_ENABLED + AP::rtc().get_utc_usec(time_unix); // may fail, leaving time_unix at 0 -#endif + mavlink_msg_system_time_send( chan, @@ -2015,7 +2015,7 @@ bool GCS_MAVLINK::sending_mavlink1() const return ((_channel_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0); } -#if AP_RC_CHANNEL_ENABLED + /* send RC_CHANNELS messages */ @@ -2084,7 +2084,7 @@ void GCS_MAVLINK::send_rc_channels_raw() const #endif ); } -#endif // AP_RC_CHANNEL_ENABLED + void GCS_MAVLINK::send_raw_imu() { @@ -2468,12 +2468,12 @@ void GCS::update_send() if (!initialised_missionitemprotocol_objects) { initialised_missionitemprotocol_objects = true; // once-only initialisation of MissionItemProtocol objects: -#if AP_MISSION_ENABLED + AP_Mission *mission = AP::mission(); if (mission != nullptr) { missionitemprotocols[MAV_MISSION_TYPE_MISSION] = new MissionItemProtocol_Waypoints(*mission); } -#endif + #if HAL_RALLY_ENABLED AP_Rally *rally = AP::rally(); if (rally != nullptr) { @@ -2914,7 +2914,7 @@ void GCS_MAVLINK::send_heartbeat() const system_status()); } -#if AP_RC_CHANNEL_ENABLED + MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int_t &packet) { if (packet.param2 > 2) { @@ -2929,7 +2929,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int } return MAV_RESULT_ACCEPTED; } -#endif // AP_RC_CHANNEL_ENABLED + MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_int_t &packet) { @@ -3378,7 +3378,7 @@ MAV_RESULT GCS_MAVLINK::handle_flight_termination(const mavlink_command_int_t &p } -#if AP_RC_CHANNEL_ENABLED + /* handle a R/C bind request (for spektrum) */ @@ -3392,7 +3392,7 @@ MAV_RESULT GCS_MAVLINK::handle_START_RX_PAIR(const mavlink_command_int_t &packet } return MAV_RESULT_ACCEPTED; } -#endif // AP_RC_CHANNEL_ENABLED + uint64_t GCS_MAVLINK::timesync_receive_timestamp_ns() const { @@ -3541,7 +3541,7 @@ void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const #endif } -#if AP_RTC_ENABLED + void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg) { mavlink_system_time_t packet; @@ -3549,7 +3549,7 @@ void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg) AP::rtc().set_utc_usec(packet.time_unix_usec, AP_RTC::SOURCE_MAVLINK_SYSTEM_TIME); } -#endif + MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_int_t &packet) @@ -3788,7 +3788,7 @@ void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg) #endif } -#if AP_RC_CHANNEL_ENABLED + // allow override of RC channel values for complete GCS // control of switch position and RC PWM values. void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg) @@ -3841,7 +3841,7 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg) gcs().sysid_myggcs_seen(tnow); } -#endif // AP_RC_CHANNEL_ENABLED + void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) @@ -4131,11 +4131,11 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif -#if AP_RC_CHANNEL_ENABLED + case MAVLINK_MSG_ID_MANUAL_CONTROL: handle_manual_control(msg); break; -#endif + #if AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED case MAVLINK_MSG_ID_PLAY_TUNE: @@ -4192,22 +4192,22 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif // HAL_VISUALODOM_ENABLED -#if AP_RTC_ENABLED + case MAVLINK_MSG_ID_SYSTEM_TIME: handle_system_time_message(msg); break; -#endif -#if AP_RC_CHANNEL_ENABLED + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handle_rc_channels_override(msg); break; -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: handle_radio_rc_channels(msg); break; -#endif -#endif + + case MAVLINK_MSG_ID_OPTICAL_FLOW: @@ -4267,7 +4267,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif -#if AP_OPENDRONEID_ENABLED + case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: @@ -4276,7 +4276,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: AP::opendroneid().handle_msg(chan, msg); break; -#endif + #if AP_SIGNED_FIRMWARE case MAVLINK_MSG_ID_SECURE_COMMAND: @@ -4509,9 +4509,9 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm return _handle_command_preflight_calibration_baro(msg); } -#if AP_RC_CHANNEL_ENABLED + rc().calibrating(is_positive(packet.param4)); -#endif + #if HAL_INS_ACCELCAL_ENABLED if (packet.x == 1) { @@ -4583,7 +4583,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_i } #endif // AP_ARMING_ENABLED -#if AP_MISSION_ENABLED + // changes the current waypoint; at time of writing GCS // implementations use the mavlink message MISSION_SET_CURRENT to set // the current waypoint, rather than this DO command. It is hoped we @@ -4642,7 +4642,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_jump_tag(const mavlink_command_int_t & return MAV_RESULT_ACCEPTED; } -#endif + MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_int_t &packet) @@ -5210,10 +5210,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return MAV_RESULT_FAILED; #endif -#if AP_RC_CHANNEL_ENABLED + case MAV_CMD_DO_AUX_FUNCTION: return handle_command_do_aux_function(packet); -#endif + case MAV_CMD_DO_FENCE_ENABLE: @@ -5228,13 +5228,13 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_do_gripper(packet); -#if AP_MISSION_ENABLED + case MAV_CMD_DO_JUMP_TAG: return handle_command_do_jump_tag(packet); case MAV_CMD_DO_SET_MISSION_CURRENT: return handle_command_do_set_mission_current(packet); -#endif + case MAV_CMD_DO_SET_MODE: return handle_command_do_set_mode(packet); @@ -5365,10 +5365,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_set_ekf_source_set(packet); #endif -#if AP_RC_CHANNEL_ENABLED + case MAV_CMD_START_RX_PAIR: return handle_START_RX_PAIR(packet); -#endif + #if AP_FILESYSTEM_FORMAT_ENABLED case MAV_CMD_STORAGE_FORMAT: @@ -5433,7 +5433,7 @@ void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) const { bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) { switch (id) { -#if AP_MISSION_ENABLED + case MSG_CURRENT_WAYPOINT: { CHECK_PAYLOAD_SIZE(MISSION_CURRENT); @@ -5451,7 +5451,7 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(MISSION_REQUEST); gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_MISSION); break; -#endif + #if HAL_RALLY_ENABLED case MSG_NEXT_MISSION_REQUEST_RALLY: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); @@ -6116,7 +6116,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif -#if AP_RC_CHANNEL_ENABLED + case MSG_RC_CHANNELS: CHECK_PAYLOAD_SIZE(RC_CHANNELS); send_rc_channels(); @@ -6126,7 +6126,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); send_rc_channels_raw(); break; -#endif + case MSG_RAW_IMU: CHECK_PAYLOAD_SIZE(RAW_IMU); @@ -6810,7 +6810,7 @@ uint64_t GCS_MAVLINK::capabilities() const } -#if AP_RC_CHANNEL_ENABLED + void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed) { if (c == nullptr) { @@ -6849,7 +6849,7 @@ void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg) // from the ground station for failsafe purposes gcs().sysid_myggcs_seen(tnow); } -#endif // AP_RC_CHANNEL_ENABLED + #if AP_RSSI_ENABLED uint8_t GCS_MAVLINK::receiver_rssi() const @@ -6888,12 +6888,12 @@ void GCS_MAVLINK::send_high_latency2() const uint16_t current_waypoint = 0; -#if AP_MISSION_ENABLED + AP_Mission *mission = AP::mission(); if (mission != nullptr) { current_waypoint = mission->get_current_nav_index(); } -#endif + uint32_t present; uint32_t enabled; @@ -7001,7 +7001,7 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t } #endif // HAL_HIGH_LATENCY2_ENABLED -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) { mavlink_radio_rc_channels_t packet; @@ -7009,6 +7009,6 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) AP::RC().handle_radio_rc_channels(&packet); } -#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #endif // HAL_GCS_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 758d6210756da1..ec84a05ee45ece 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -19,7 +19,7 @@ #include "RC_Channel_config.h" -#if AP_RC_CHANNEL_ENABLED + #include #include @@ -1072,7 +1072,7 @@ void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag) } -#if AP_MISSION_ENABLED + void RC_Channel::do_aux_function_clear_wp(const AuxSwitchPos ch_flag) { if (ch_flag == AuxSwitchPos::HIGH) { @@ -1083,7 +1083,7 @@ void RC_Channel::do_aux_function_clear_wp(const AuxSwitchPos ch_flag) mission->clear(); } } -#endif // AP_MISSION_ENABLED + #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED void RC_Channel::do_aux_function_relay(const uint8_t relay, bool val) @@ -1183,7 +1183,7 @@ void RC_Channel::do_aux_function_rc_override_enable(const AuxSwitchPos ch_flag) } } -#if AP_MISSION_ENABLED + void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag) { if (ch_flag != AuxSwitchPos::HIGH) { @@ -1195,7 +1195,7 @@ void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag) } mission->reset(); } -#endif + void RC_Channel::do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag) { @@ -1804,5 +1804,3 @@ void RC_Channels::convert_options(const RC_Channel::AUX_FUNC old_option, const R } } } - -#endif // AP_RC_CHANNEL_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index aa156b705e7db1..c86b98f0bfab5e 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -4,7 +4,7 @@ #include "RC_Channel_config.h" -#if AP_RC_CHANNEL_ENABLED + #include #include @@ -642,5 +642,3 @@ class RC_Channels { }; RC_Channels &rc(); - -#endif // AP_RC_CHANNEL_ENABLED diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 58e8f94799bd6c..f14db41b344194 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -20,7 +20,7 @@ #include "RC_Channel_config.h" -#if AP_RC_CHANNEL_ENABLED + #include #include @@ -315,5 +315,3 @@ RC_Channels &rc() { return *RC_Channels::get_singleton(); } - -#endif // AP_RC_CHANNEL_ENABLED diff --git a/libraries/RC_Channel/RC_Channels_VarInfo.h b/libraries/RC_Channel/RC_Channels_VarInfo.h index 2419033b4087d4..18ce505bdd6815 100644 --- a/libraries/RC_Channel/RC_Channels_VarInfo.h +++ b/libraries/RC_Channel/RC_Channels_VarInfo.h @@ -2,7 +2,7 @@ #include "RC_Channel_config.h" -#if AP_RC_CHANNEL_ENABLED + #include "RC_Channel.h" @@ -114,5 +114,3 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { AP_GROUPEND }; - -#endif // AP_RC_CHANNEL_ENABLED diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index e6be2e6ef66ab1..88a82a8df7da33 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -390,7 +390,7 @@ SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t functi } } -#if AP_RC_CHANNEL_ENABLED + /* copy radio_in to radio_out for a given function */ @@ -431,7 +431,7 @@ SRV_Channels::copy_radio_in_out_mask(uint32_t mask) } } -#endif // AP_RC_CHANNEL_ENABLED + /* setup failsafe value for an auxiliary function type to a Limit @@ -482,7 +482,7 @@ SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_C if (c.function == function) { uint16_t pwm = c.get_limit_pwm(limit); c.set_output_pwm(pwm); -#if AP_RC_CHANNEL_ENABLED + if (c.function == SRV_Channel::k_manual) { RC_Channel *cin = rc().channel(c.ch_num); if (cin != nullptr) { @@ -491,7 +491,7 @@ SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_C cin->set_radio_in(pwm); } } -#endif + } } } diff --git a/libraries/StorageManager/StorageManager.cpp b/libraries/StorageManager/StorageManager.cpp index c3bb826975a5ac..ff0c4ee7e6289b 100644 --- a/libraries/StorageManager/StorageManager.cpp +++ b/libraries/StorageManager/StorageManager.cpp @@ -121,9 +121,9 @@ StorageAccess::StorageAccess(StorageManager::StorageType _type) : { // calculate available bytes total_size = 0; -#if AP_SDCARD_STORAGE_ENABLED + file = nullptr; -#endif + for (uint8_t i=0; i file->bufsize) { @@ -150,7 +150,7 @@ bool StorageAccess::read_block(void *data, uint16_t addr, size_t n) const memcpy(b, &file->buffer[addr], n2); return n == n2; } -#endif + for (uint8_t i=0; i file->bufsize) { return false; @@ -208,7 +208,7 @@ bool StorageAccess::write_block(uint16_t addr, const void *data, size_t n) const } return n == n2; } -#endif + for (uint8_t i=0; ilast_io_fail_ms = now_ms; } } -#endif // AP_SDCARD_STORAGE_ENABLED - diff --git a/libraries/StorageManager/StorageManager.h b/libraries/StorageManager/StorageManager.h index 5332ff408ede22..0a0f70f1def7f9 100644 --- a/libraries/StorageManager/StorageManager.h +++ b/libraries/StorageManager/StorageManager.h @@ -123,7 +123,7 @@ class StorageAccess { const StorageManager::StorageType type; uint16_t total_size; -#if AP_SDCARD_STORAGE_ENABLED + /* support for storage regions on microSD. Only the StorageMission for now @@ -140,5 +140,5 @@ class StorageAccess { } *file; void flush_file(void); -#endif + };