mirror of https://github.com/ArduPilot/ardupilot
Copter: remove ENABLE/ENABLED/DISABLE/DISABLED defines
This commit is contained in:
parent
0e33a0f8f7
commit
784760342d
|
@ -1,33 +1,33 @@
|
|||
// User specific config file. Any items listed in config.h can be overridden here.
|
||||
|
||||
// uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards)
|
||||
//#define LOGGING_ENABLED DISABLED // disable logging to save 11K of flash space
|
||||
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
|
||||
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
|
||||
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
|
||||
//#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support
|
||||
//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support
|
||||
//#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support
|
||||
//#define MODE_CIRCLE_ENABLED DISABLED // disable circle mode support
|
||||
//#define MODE_DRIFT_ENABLED DISABLED // disable drift mode support
|
||||
//#define MODE_FLIP_ENABLED DISABLED // disable flip mode support
|
||||
//#define MODE_FOLLOW_ENABLED DISABLED // disable follow mode support
|
||||
//#define MODE_GUIDED_ENABLED DISABLED // disable guided mode support
|
||||
//#define MODE_GUIDED_NOGPS_ENABLED DISABLED // disable guided/nogps mode support
|
||||
//#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support
|
||||
//#define MODE_POSHOLD_ENABLED DISABLED // disable poshold mode support
|
||||
//#define MODE_RTL_ENABLED DISABLED // disable rtl mode support
|
||||
//#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support
|
||||
//#define MODE_SPORT_ENABLED DISABLED // disable sport mode support
|
||||
//#define MODE_SYSTEMID_ENABLED DISABLED // disable system ID mode support
|
||||
//#define MODE_THROW_ENABLED DISABLED // disable throw mode support
|
||||
//#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support
|
||||
//#define OSD_ENABLED DISABLED // disable on-screen-display support
|
||||
//#define LOGGING_ENABLED 0 // disable logging to save 11K of flash space
|
||||
//#define MOUNT 0 // disable the camera gimbal to save 8K of flash space
|
||||
//#define AUTOTUNE_ENABLED 0 // disable the auto tune functionality to save 7k of flash
|
||||
//#define NAV_GUIDED 0 // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
|
||||
//#define MODE_ACRO_ENABLED 0 // disable acrobatic mode support
|
||||
//#define MODE_AUTO_ENABLED 0 // disable auto mode support
|
||||
//#define MODE_BRAKE_ENABLED 0 // disable brake mode support
|
||||
//#define MODE_CIRCLE_ENABLED 0 // disable circle mode support
|
||||
//#define MODE_DRIFT_ENABLED 0 // disable drift mode support
|
||||
//#define MODE_FLIP_ENABLED 0 // disable flip mode support
|
||||
//#define MODE_FOLLOW_ENABLED 0 // disable follow mode support
|
||||
//#define MODE_GUIDED_ENABLED 0 // disable guided mode support
|
||||
//#define MODE_GUIDED_NOGPS_ENABLED 0 // disable guided/nogps mode support
|
||||
//#define MODE_LOITER_ENABLED 0 // disable loiter mode support
|
||||
//#define MODE_POSHOLD_ENABLED 0 // disable poshold mode support
|
||||
//#define MODE_RTL_ENABLED 0 // disable rtl mode support
|
||||
//#define MODE_SMARTRTL_ENABLED 0 // disable smartrtl mode support
|
||||
//#define MODE_SPORT_ENABLED 0 // disable sport mode support
|
||||
//#define MODE_SYSTEMID_ENABLED 0 // disable system ID mode support
|
||||
//#define MODE_THROW_ENABLED 0 // disable throw mode support
|
||||
//#define MODE_ZIGZAG_ENABLED 0 // disable zigzag mode support
|
||||
//#define OSD_ENABLED 0 // disable on-screen-display support
|
||||
|
||||
// features below are disabled by default on all boards
|
||||
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes
|
||||
//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link
|
||||
//#define ADVANCED_FAILSAFE ENABLED // enabled advanced failsafe which allows running a portion of the mission in failsafe events
|
||||
//#define ADVANCED_FAILSAFE 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events
|
||||
|
||||
// other settings
|
||||
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)
|
||||
|
@ -42,5 +42,5 @@
|
|||
//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); // for code to be run at 10hz
|
||||
//#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz
|
||||
//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz
|
||||
//#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches
|
||||
//#define USER_PARAMS_ENABLED ENABLED // to enable user parameters
|
||||
//#define USERHOOK_AUXSWITCH 1 // for code to handle user aux switches
|
||||
//#define USER_PARAMS_ENABLED 1 // to enable user parameters
|
||||
|
|
|
@ -210,7 +210,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
|
|||
}
|
||||
|
||||
// acro balance parameter check
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
|
||||
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ACRO_BAL_ROLL/PITCH");
|
||||
return false;
|
||||
|
@ -251,7 +251,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 MODE_RTL_ENABLED
|
||||
if (copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::TERRAIN) {
|
||||
// get terrain source from wpnav
|
||||
const char *failure_template = "RTL_ALT_TYPE is above-terrain but %s";
|
||||
|
@ -728,7 +728,7 @@ 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
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
copter.g2.smart_rtl.set_home(copter.position_ok());
|
||||
#endif
|
||||
|
||||
|
@ -815,7 +815,7 @@ 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
|
||||
#if MODE_AUTO_ENABLED
|
||||
// reset the mission
|
||||
copter.mode_auto.mission.reset();
|
||||
#endif
|
||||
|
@ -828,7 +828,7 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che
|
|||
|
||||
copter.ap.in_arming_delay = false;
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
// Possibly save auto tuned parameters
|
||||
copter.mode_autotune.autotune.disarmed(copter.flightmode == &copter.mode_autotune);
|
||||
#endif
|
||||
|
|
|
@ -64,7 +64,7 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
|
|||
return 0.0f;
|
||||
}
|
||||
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
if (g2.toy_mode.enabled()) {
|
||||
// allow throttle to be reduced after throttle arming and for
|
||||
// slower descent close to the ground
|
||||
|
|
|
@ -114,7 +114,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
|
||||
// run low level rate controllers that only require IMU data
|
||||
FAST_TASK(run_rate_controller),
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
FAST_TASK(run_custom_controller),
|
||||
#endif
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -159,7 +159,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
SCHED_TASK(update_batt_compass, 10, 120, 15),
|
||||
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50, 18),
|
||||
SCHED_TASK(arm_motors_check, 10, 50, 21),
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
|
||||
#endif
|
||||
SCHED_TASK(auto_disarm_check, 10, 50, 27),
|
||||
|
@ -176,7 +176,7 @@ 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
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
|
||||
#endif
|
||||
#if HAL_SPRAYER_ENABLED
|
||||
|
@ -232,7 +232,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
|
|||
#if HAL_ADSB_ENABLED
|
||||
SCHED_TASK(avoidance_adsb_update, 10, 100, 138),
|
||||
#endif
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
SCHED_TASK(afs_fs_check, 10, 100, 141),
|
||||
#endif
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
|
@ -274,7 +274,7 @@ constexpr int8_t Copter::_failsafe_priorities[7];
|
|||
|
||||
|
||||
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
// set target location (for use by external control and scripting)
|
||||
bool Copter::set_target_location(const Location& target_loc)
|
||||
{
|
||||
|
@ -285,11 +285,11 @@ bool Copter::set_target_location(const Location& target_loc)
|
|||
|
||||
return mode_guided.set_destination(target_loc);
|
||||
}
|
||||
#endif //MODE_GUIDED_ENABLED == ENABLED
|
||||
#endif //MODE_GUIDED_ENABLED
|
||||
#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
|
||||
|
||||
#if AP_SCRIPTING_ENABLED
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
// start takeoff to given altitude (for use by scripting)
|
||||
bool Copter::start_takeoff(float alt)
|
||||
{
|
||||
|
@ -391,7 +391,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
|
|||
}
|
||||
#endif
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
// circle mode controls
|
||||
bool Copter::get_circle_radius(float &radius_m)
|
||||
{
|
||||
|
@ -412,7 +412,7 @@ bool Copter::set_desired_speed(float speed)
|
|||
return flightmode->set_speed_xy(speed * 100.0f);
|
||||
}
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
// returns true if mode supports NAV_SCRIPT_TIME mission commands
|
||||
bool Copter::nav_scripting_enable(uint8_t mode)
|
||||
{
|
||||
|
@ -488,7 +488,7 @@ bool Copter::is_taking_off() const
|
|||
|
||||
bool Copter::current_mode_requires_mission() const
|
||||
{
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
return flightmode == &mode_auto;
|
||||
#else
|
||||
return false;
|
||||
|
@ -631,7 +631,7 @@ void Copter::twentyfive_hz_logging()
|
|||
AP::ins().Write_IMU();
|
||||
}
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
//update autorotation log
|
||||
g2.arot.Log_Write_Autorotation();
|
||||
|
@ -702,7 +702,7 @@ void Copter::one_hz_loop()
|
|||
// slowly update the PID notches with the average loop rate
|
||||
attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
|
||||
pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -83,7 +83,7 @@
|
|||
#define MOTOR_CLASS AP_MotorsMulticopter
|
||||
#endif
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
#include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
|
||||
#endif
|
||||
|
||||
|
@ -115,7 +115,7 @@
|
|||
# include <AC_PrecLand/AC_PrecLand.h>
|
||||
# include <AC_PrecLand/AC_PrecLand_StateMachine.h>
|
||||
#endif
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
# include <AP_Follow/AP_Follow.h>
|
||||
#endif
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
|
@ -137,10 +137,10 @@
|
|||
#include <AP_OSD/AP_OSD.h>
|
||||
#endif
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
# include "afs_copter.h"
|
||||
#endif
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
# include "toy_mode.h"
|
||||
#endif
|
||||
#if AP_WINCH_ENABLED
|
||||
|
@ -152,7 +152,7 @@
|
|||
#include <AP_Scripting/AP_Scripting.h>
|
||||
#endif
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
#include <AC_CustomControl/AC_CustomControl.h> // Custom control library
|
||||
#endif
|
||||
|
||||
|
@ -183,7 +183,7 @@ public:
|
|||
friend class ParametersG2;
|
||||
friend class AP_Avoidance_Copter;
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
friend class AP_AdvancedFailsafe_Copter;
|
||||
#endif
|
||||
friend class AP_Arming_Copter;
|
||||
|
@ -479,11 +479,11 @@ private:
|
|||
AC_WPNav *wp_nav;
|
||||
AC_Loiter *loiter_nav;
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()};
|
||||
#endif
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
AC_Circle *circle_nav;
|
||||
#endif
|
||||
|
||||
|
@ -665,13 +665,13 @@ private:
|
|||
uint8_t &task_count,
|
||||
uint32_t &log_bit) override;
|
||||
#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
bool set_target_location(const Location& target_loc) override;
|
||||
#endif // MODE_GUIDED_ENABLED == ENABLED
|
||||
#endif // MODE_GUIDED_ENABLED
|
||||
#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
|
||||
|
||||
#if AP_SCRIPTING_ENABLED
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
bool start_takeoff(float alt) override;
|
||||
bool get_target_location(Location& target_loc) override;
|
||||
bool update_target_location(const Location &old_loc, const Location &new_loc) override;
|
||||
|
@ -682,12 +682,12 @@ private:
|
|||
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
|
||||
#if MODE_CIRCLE_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
|
||||
#if MODE_AUTO_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;
|
||||
|
@ -725,7 +725,7 @@ private:
|
|||
uint16_t get_pilot_speed_dn() const;
|
||||
void run_rate_controller();
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
void run_custom_controller() { custom_control.update(); }
|
||||
#endif
|
||||
|
||||
|
@ -801,7 +801,7 @@ private:
|
|||
// failsafe.cpp
|
||||
void failsafe_enable();
|
||||
void failsafe_disable();
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
void afs_fs_check(void);
|
||||
#endif
|
||||
|
||||
|
@ -995,7 +995,7 @@ private:
|
|||
void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag);
|
||||
void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag);
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
ModeAcro_Heli mode_acro;
|
||||
#else
|
||||
|
@ -1003,38 +1003,38 @@ private:
|
|||
#endif
|
||||
#endif
|
||||
ModeAltHold mode_althold;
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
ModeAuto mode_auto;
|
||||
#endif
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
ModeAutoTune mode_autotune;
|
||||
#endif
|
||||
#if MODE_BRAKE_ENABLED == ENABLED
|
||||
#if MODE_BRAKE_ENABLED
|
||||
ModeBrake mode_brake;
|
||||
#endif
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
ModeCircle mode_circle;
|
||||
#endif
|
||||
#if MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_DRIFT_ENABLED
|
||||
ModeDrift mode_drift;
|
||||
#endif
|
||||
#if MODE_FLIP_ENABLED == ENABLED
|
||||
#if MODE_FLIP_ENABLED
|
||||
ModeFlip mode_flip;
|
||||
#endif
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
ModeFollow mode_follow;
|
||||
#endif
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
ModeGuided mode_guided;
|
||||
#endif
|
||||
ModeLand mode_land;
|
||||
#if MODE_LOITER_ENABLED == ENABLED
|
||||
#if MODE_LOITER_ENABLED
|
||||
ModeLoiter mode_loiter;
|
||||
#endif
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_ENABLED
|
||||
ModePosHold mode_poshold;
|
||||
#endif
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
ModeRTL mode_rtl;
|
||||
#endif
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -1042,34 +1042,34 @@ private:
|
|||
#else
|
||||
ModeStabilize mode_stabilize;
|
||||
#endif
|
||||
#if MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_SPORT_ENABLED
|
||||
ModeSport mode_sport;
|
||||
#endif
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
ModeSystemId mode_systemid;
|
||||
#endif
|
||||
#if HAL_ADSB_ENABLED
|
||||
ModeAvoidADSB mode_avoid_adsb;
|
||||
#endif
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
ModeThrow mode_throw;
|
||||
#endif
|
||||
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_NOGPS_ENABLED
|
||||
ModeGuidedNoGPS mode_guided_nogps;
|
||||
#endif
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
ModeSmartRTL mode_smartrtl;
|
||||
#endif
|
||||
#if MODE_FLOWHOLD_ENABLED == ENABLED
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
ModeFlowHold mode_flowhold;
|
||||
#endif
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
ModeZigZag mode_zigzag;
|
||||
#endif
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
ModeAutorotate mode_autorotate;
|
||||
#endif
|
||||
#if MODE_TURTLE_ENABLED == ENABLED
|
||||
#if MODE_TURTLE_ENABLED
|
||||
ModeTurtle mode_turtle;
|
||||
#endif
|
||||
|
||||
|
|
|
@ -155,7 +155,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 MODE_GUIDED_ENABLED
|
||||
if (!copter.flightmode->in_guided_mode()) {
|
||||
return;
|
||||
}
|
||||
|
@ -621,7 +621,7 @@ 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
|
||||
#if MODE_AUTO_ENABLED
|
||||
return copter.mode_auto.do_guided(cmd);
|
||||
#else
|
||||
return false;
|
||||
|
@ -638,7 +638,7 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
|
|||
copter.avoidance_adsb.handle_msg(msg);
|
||||
}
|
||||
#endif
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
// pass message to follow library
|
||||
copter.g2.follow.handle_msg(msg);
|
||||
#endif
|
||||
|
@ -719,7 +719,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int
|
|||
|
||||
MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
|
||||
{
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_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;
|
||||
|
@ -771,7 +771,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
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
case MAV_CMD_DO_FOLLOW:
|
||||
// param1: sysid of target to follow
|
||||
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
|
||||
|
@ -812,7 +812,7 @@ 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
|
||||
#if MODE_AUTO_ENABLED
|
||||
case MAV_CMD_MISSION_START:
|
||||
return handle_MAV_CMD_MISSION_START(packet);
|
||||
#endif
|
||||
|
@ -841,7 +841,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
|
|||
}
|
||||
return MAV_RESULT_ACCEPTED;
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
case MAV_CMD_DO_RETURN_PATH_START:
|
||||
if (copter.mode_auto.return_path_start_auto_RTL(ModeReason::GCS_COMMAND)) {
|
||||
return MAV_RESULT_ACCEPTED;
|
||||
|
@ -957,7 +957,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_comm
|
|||
return MAV_RESULT_FAILED;
|
||||
}
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet)
|
||||
{
|
||||
if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
|
||||
|
@ -1090,7 +1090,7 @@ 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 MODE_BRAKE_ENABLED
|
||||
if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) {
|
||||
copter.mode_brake.timeout_to_loiter_ms(2500);
|
||||
} else {
|
||||
|
@ -1178,7 +1178,7 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
|
|||
return true;
|
||||
}
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
// for mavlink SET_POSITION_TARGET messages
|
||||
constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
|
||||
POSITION_TARGET_TYPEMASK_X_IGNORE |
|
||||
|
@ -1203,7 +1203,7 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
|
|||
POSITION_TARGET_TYPEMASK_FORCE_SET;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_message_t &msg)
|
||||
{
|
||||
// decode packet
|
||||
|
@ -1486,13 +1486,13 @@ void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mav
|
|||
copter.mode_guided.init(true);
|
||||
}
|
||||
}
|
||||
#endif // MODE_GUIDED_ENABLED == ENABLED
|
||||
#endif // MODE_GUIDED_ENABLED
|
||||
|
||||
void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
|
||||
{
|
||||
|
||||
switch (msg.msgid) {
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
|
||||
handle_message_set_attitude_target(msg);
|
||||
break;
|
||||
|
@ -1509,7 +1509,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
|
|||
copter.terrain.handle_data(chan, msg);
|
||||
break;
|
||||
#endif
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
|
||||
copter.g2.toy_mode.handle_message(msg);
|
||||
break;
|
||||
|
@ -1521,7 +1521,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
|
|||
}
|
||||
|
||||
MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) {
|
||||
return MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
|
|
|
@ -258,7 +258,7 @@ struct PACKED log_SysIdD {
|
|||
// Write an rate packet
|
||||
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z)
|
||||
{
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
struct log_SysIdD pkt_sidd = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
|
@ -292,7 +292,7 @@ struct PACKED log_SysIdS {
|
|||
// Write an rate packet
|
||||
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out)
|
||||
{
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
struct log_SysIdS pkt_sids = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
|
|
|
@ -94,7 +94,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
|
||||
#if MODE_RTL_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.
|
||||
|
@ -354,7 +354,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
// @User: Advanced
|
||||
ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
|
||||
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_ENABLED
|
||||
// @Param: PHLD_BRAKE_RATE
|
||||
// @DisplayName: PosHold braking rate
|
||||
// @Description: PosHold flight mode's rotation rate during braking in deg/sec
|
||||
|
@ -410,7 +410,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
// @User: Advanced
|
||||
GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED),
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
// @Param: ACRO_BAL_ROLL
|
||||
// @DisplayName: Acro Balance Roll
|
||||
// @Description: rate at which roll angle returns to level in acro and sport mode. A higher value causes the vehicle to return to level faster. For helicopter sets the decay rate of the virtual flybar in the roll axis. A higher value causes faster decay of desired to actual attitude.
|
||||
|
@ -430,7 +430,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
|
||||
// ACRO_RP_EXPO moved to Command Model class
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
// @Param: ACRO_TRAINER
|
||||
// @DisplayName: Acro Trainer
|
||||
// @Description: Type of trainer used in acro mode
|
||||
|
@ -487,7 +487,7 @@ 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
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
// @Group: CIRCLE_
|
||||
// @Path: ../libraries/AC_WPNav/AC_Circle.cpp
|
||||
GOBJECTPTR(circle_nav, "CIRCLE_", AC_Circle),
|
||||
|
@ -628,7 +628,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
|
||||
#endif
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
// @Group: MIS_
|
||||
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
|
||||
GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
|
||||
|
@ -684,7 +684,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
|
||||
#if MODE_THROW_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.
|
||||
|
@ -713,7 +713,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
GOBJECT(osd, "OSD", AP_OSD),
|
||||
#endif
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
// @Group: CC
|
||||
// @Path: ../libraries/AC_CustomControl/AC_CustomControl.cpp
|
||||
GOBJECT(custom_control, "CC", AC_CustomControl),
|
||||
|
@ -748,7 +748,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_SUBGROUPPTR(button_ptr, "BTN_", 2, ParametersG2, AP_Button),
|
||||
#endif
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_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)
|
||||
|
@ -771,7 +771,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1),
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
// @Group: AFS_
|
||||
// @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
|
||||
AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe),
|
||||
|
@ -798,7 +798,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
|
||||
// ACRO_Y_EXPO (9) moved to Command Model Class
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
// @Param: ACRO_THR_MID
|
||||
// @DisplayName: Acro Thr Mid
|
||||
// @Description: Acro Throttle Mid
|
||||
|
@ -842,13 +842,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration),
|
||||
#endif
|
||||
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
// @Group: TMODE
|
||||
// @Path: toy_mode.cpp
|
||||
AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode),
|
||||
#endif
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
// @Group: SRTL_
|
||||
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
|
||||
AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL),
|
||||
|
@ -880,23 +880,23 @@ 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
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
// @Group: FHLD
|
||||
// @Path: mode_flowhold.cpp
|
||||
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold),
|
||||
#endif
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
// @Group: FOLL
|
||||
// @Path: ../libraries/AP_Follow/AP_Follow.cpp
|
||||
AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow),
|
||||
#endif
|
||||
|
||||
#if USER_PARAMS_ENABLED == ENABLED
|
||||
#if USER_PARAMS_ENABLED
|
||||
AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters),
|
||||
#endif
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_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),
|
||||
|
@ -922,7 +922,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner),
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
// @Group: SID
|
||||
// @Path: mode_systemid.cpp
|
||||
AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
|
||||
|
@ -942,19 +942,19 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Copter::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL),
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
// @Group: AROT_
|
||||
// @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp
|
||||
AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation),
|
||||
#endif
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
// @Group: ZIGZ_
|
||||
// @Path: mode_zigzag.cpp
|
||||
AP_SUBGROUPPTR(mode_zigzag_ptr, "ZIGZ_", 38, ParametersG2, ModeZigZag),
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_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.
|
||||
|
@ -963,7 +963,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_GROUPINFO("ACRO_OPTIONS", 39, ParametersG2, acro_options, 0),
|
||||
#endif
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_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.
|
||||
|
@ -972,7 +972,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_GROUPINFO("AUTO_OPTIONS", 40, ParametersG2, auto_options, 0),
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
// @Param: GUID_OPTIONS
|
||||
// @DisplayName: Guided mode options
|
||||
// @Description: Options that can be applied to change guided mode behaviour
|
||||
|
@ -990,7 +990,7 @@ 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
|
||||
#if MODE_RTL_ENABLED
|
||||
// @Param: RTL_OPTIONS
|
||||
// @DisplayName: RTL mode options
|
||||
// @Description: Options that can be applied to change RTL mode behaviour
|
||||
|
@ -1018,7 +1018,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT),
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_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
|
||||
|
@ -1054,7 +1054,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30),
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
// @Param: ACRO_RP_RATE
|
||||
// @DisplayName: Acro Roll and Pitch Rate
|
||||
// @Description: Acro mode maximum roll and pitch rate. Higher values mean faster rate of rotation
|
||||
|
@ -1080,7 +1080,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_SUBGROUPINFO(command_model_acro_rp, "ACRO_RP_", 54, ParametersG2, AC_CommandModel),
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
|
||||
// @Param: ACRO_Y_RATE
|
||||
// @DisplayName: Acro Yaw Rate
|
||||
// @Description: Acro mode maximum yaw rate. Higher value means faster rate of rotation
|
||||
|
@ -1147,7 +1147,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_GROUPINFO("TKOFF_RPM_MIN", 58, ParametersG2, takeoff_rpm_min, 0),
|
||||
#endif
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
// @Group: WVANE_
|
||||
// @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp
|
||||
AP_SUBGROUPINFO(weathervane, "WVANE_", 59, ParametersG2, AC_WeatherVane),
|
||||
|
@ -1251,46 +1251,46 @@ ParametersG2::ParametersG2(void)
|
|||
#if HAL_PROXIMITY_ENABLED
|
||||
, proximity()
|
||||
#endif
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
,afs()
|
||||
#endif
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
,smart_rtl()
|
||||
#endif
|
||||
#if MODE_FLOWHOLD_ENABLED == ENABLED
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
,mode_flowhold_ptr(&copter.mode_flowhold)
|
||||
#endif
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
,follow()
|
||||
#endif
|
||||
#if USER_PARAMS_ENABLED == ENABLED
|
||||
#if USER_PARAMS_ENABLED
|
||||
,user_parameters()
|
||||
#endif
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
,autotune_ptr(&copter.mode_autotune.autotune)
|
||||
#endif
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
,mode_systemid_ptr(&copter.mode_systemid)
|
||||
#endif
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
,arot()
|
||||
#endif
|
||||
#if HAL_BUTTON_ENABLED
|
||||
,button_ptr(&copter.button)
|
||||
#endif
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
,mode_zigzag_ptr(&copter.mode_zigzag)
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f)
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
|
||||
,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f)
|
||||
#endif
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
,weathervane()
|
||||
#endif
|
||||
{
|
||||
|
@ -1345,7 +1345,7 @@ void Copter::load_parameters(void)
|
|||
convert_lgr_parameters();
|
||||
#endif
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
// PARAMETER_CONVERSION - Added: Sep-2021
|
||||
g.rtl_altitude.convert_parameter_width(AP_PARAM_INT16);
|
||||
#endif
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
#include "RC_Channel.h"
|
||||
#include <AP_Proximity/AP_Proximity.h>
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
# include <AP_Follow/AP_Follow.h>
|
||||
#endif
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
#include <AC_AttitudeControl/AC_WeatherVane.h>
|
||||
#endif
|
||||
|
||||
|
@ -398,7 +398,7 @@ public:
|
|||
AP_Int16 throttle_behavior;
|
||||
AP_Float pilot_takeoff_alt;
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
AP_Int32 rtl_altitude;
|
||||
AP_Int16 rtl_speed_cms;
|
||||
AP_Float rtl_cone_slope;
|
||||
|
@ -415,7 +415,7 @@ public:
|
|||
|
||||
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
|
||||
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_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
|
||||
|
@ -459,7 +459,7 @@ public:
|
|||
AP_Float fs_ekf_thresh;
|
||||
AP_Int16 gcs_pid_mask;
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
AP_Enum<ModeThrow::PreThrowMotorState> 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
|
||||
|
@ -467,13 +467,13 @@ public:
|
|||
|
||||
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
// Acro parameters
|
||||
AP_Float acro_balance_roll;
|
||||
AP_Float acro_balance_pitch;
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
// Acro parameters
|
||||
AP_Int8 acro_trainer;
|
||||
#endif
|
||||
|
@ -504,7 +504,7 @@ public:
|
|||
AP_Button *button_ptr;
|
||||
#endif
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
// Throw mode parameters
|
||||
AP_Int8 throw_nextmode;
|
||||
AP_Enum<ModeThrow::ThrowType> throw_type;
|
||||
|
@ -531,7 +531,7 @@ public:
|
|||
// whether to enforce acceptance of packets only from sysid_my_gcs
|
||||
AP_Int8 sysid_enforce;
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
// advanced failsafe library
|
||||
AP_AdvancedFailsafe_Copter afs;
|
||||
#endif
|
||||
|
@ -539,7 +539,7 @@ public:
|
|||
// developer options
|
||||
AP_Int32 dev_options;
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
AP_Float acro_thr_mid;
|
||||
#endif
|
||||
|
||||
|
@ -552,7 +552,7 @@ public:
|
|||
// control over servo output ranges
|
||||
SRV_Channels servo_channels;
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
// Safe RTL library
|
||||
AP_SmartRTL smart_rtl;
|
||||
#endif
|
||||
|
@ -568,7 +568,7 @@ public:
|
|||
// Land alt final stage
|
||||
AP_Int16 land_alt_low;
|
||||
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
ToyMode toy_mode;
|
||||
#endif
|
||||
|
||||
|
@ -577,17 +577,17 @@ public:
|
|||
void *mode_flowhold_ptr;
|
||||
#endif
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
// follow
|
||||
AP_Follow follow;
|
||||
#endif
|
||||
|
||||
#if USER_PARAMS_ENABLED == ENABLED
|
||||
#if USER_PARAMS_ENABLED
|
||||
// User custom parameters
|
||||
UserParameters user_parameters;
|
||||
#endif
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
// we need a pointer to autotune for the G2 table
|
||||
void *autotune_ptr;
|
||||
#endif
|
||||
|
@ -600,7 +600,7 @@ public:
|
|||
AP_OAPathPlanner oa;
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
// we need a pointer to the mode for the G2 table
|
||||
void *mode_systemid_ptr;
|
||||
#endif
|
||||
|
@ -611,42 +611,42 @@ public:
|
|||
// Failsafe options bitmask #36
|
||||
AP_Int32 fs_options;
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
// Autonmous autorotation
|
||||
AC_Autorotation arot;
|
||||
#endif
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_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
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
AC_CommandModel command_model_acro_rp;
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
|
||||
AC_CommandModel command_model_acro_y;
|
||||
#endif
|
||||
|
||||
AC_CommandModel command_model_pilot;
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
AP_Int8 acro_options;
|
||||
#endif
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
AP_Int32 auto_options;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
AP_Int32 guided_options;
|
||||
#endif
|
||||
|
||||
AP_Float fs_gcs_timeout;
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
AP_Int32 rtl_options;
|
||||
#endif
|
||||
|
||||
|
@ -656,7 +656,7 @@ public:
|
|||
AP_Float rangefinder_filt;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
AP_Float guided_timeout;
|
||||
#endif
|
||||
|
||||
|
@ -676,7 +676,7 @@ public:
|
|||
// EKF variance filter cutoff
|
||||
AP_Float fs_ekf_filt_hz;
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
AC_WeatherVane weathervane;
|
||||
#endif
|
||||
|
||||
|
|
|
@ -190,7 +190,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
}
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
case AUX_FUNC::RTL:
|
||||
do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
|
||||
break;
|
||||
|
@ -204,7 +204,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
}
|
||||
break;
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
case AUX_FUNC::SAVE_WP:
|
||||
// save waypoint when switch is brought high
|
||||
if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
|
||||
|
@ -272,7 +272,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
case AUX_FUNC::ACRO_TRAINER:
|
||||
switch(ch_flag) {
|
||||
case AuxSwitchPos::LOW:
|
||||
|
@ -291,7 +291,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
case AUX_FUNC::AUTOTUNE_MODE:
|
||||
do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag);
|
||||
break;
|
||||
|
@ -383,19 +383,19 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_BRAKE_ENABLED == ENABLED
|
||||
#if MODE_BRAKE_ENABLED
|
||||
case AUX_FUNC::BRAKE:
|
||||
do_aux_function_change_mode(Mode::Number::BRAKE, ch_flag);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
case AUX_FUNC::THROW:
|
||||
do_aux_function_change_mode(Mode::Number::THROW, ch_flag);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED == ENABLED
|
||||
#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED
|
||||
case AUX_FUNC::PRECISION_LOITER:
|
||||
switch (ch_flag) {
|
||||
case AuxSwitchPos::HIGH:
|
||||
|
@ -411,7 +411,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
case AUX_FUNC::SMART_RTL:
|
||||
do_aux_function_change_mode(Mode::Number::SMART_RTL, ch_flag);
|
||||
break;
|
||||
|
@ -471,7 +471,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
case AUX_FUNC::ZIGZAG:
|
||||
do_aux_function_change_mode(Mode::Number::ZIGZAG, ch_flag);
|
||||
break;
|
||||
|
@ -499,7 +499,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
|
||||
break;
|
||||
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_ENABLED
|
||||
case AUX_FUNC::POSHOLD:
|
||||
do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
|
||||
break;
|
||||
|
@ -509,7 +509,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
do_aux_function_change_mode(Mode::Number::ALT_HOLD, ch_flag);
|
||||
break;
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
case AUX_FUNC::ACRO:
|
||||
do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
|
||||
break;
|
||||
|
@ -521,13 +521,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
case AUX_FUNC::CIRCLE:
|
||||
do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_DRIFT_ENABLED
|
||||
case AUX_FUNC::DRIFT:
|
||||
do_aux_function_change_mode(Mode::Number::DRIFT, ch_flag);
|
||||
break;
|
||||
|
@ -580,7 +580,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
}
|
||||
break;
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
case AUX_FUNC::ZIGZAG_Auto:
|
||||
if (copter.flightmode == &copter.mode_zigzag) {
|
||||
switch (ch_flag) {
|
||||
|
@ -597,7 +597,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
|
||||
case AUX_FUNC::AIRMODE:
|
||||
do_aux_function_change_air_mode(ch_flag);
|
||||
#if MODE_ACRO_ENABLED == ENABLED && FRAME_CONFIG != HELI_FRAME
|
||||
#if MODE_ACRO_ENABLED && FRAME_CONFIG != HELI_FRAME
|
||||
copter.mode_acro.air_mode_aux_changed();
|
||||
#endif
|
||||
break;
|
||||
|
@ -606,13 +606,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
do_aux_function_change_force_flying(ch_flag);
|
||||
break;
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
case AUX_FUNC::AUTO_RTL:
|
||||
do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_TURTLE_ENABLED == ENABLED
|
||||
#if MODE_TURTLE_ENABLED
|
||||
case AUX_FUNC::TURTLE:
|
||||
do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag);
|
||||
break;
|
||||
|
@ -632,13 +632,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
|
|||
}
|
||||
break;
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
case AUX_FUNC::CUSTOM_CONTROLLER:
|
||||
copter.custom_control.set_custom_controller(ch_flag == AuxSwitchPos::HIGH);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
case AUX_FUNC::WEATHER_VANE_ENABLE: {
|
||||
switch (ch_flag) {
|
||||
case AuxSwitchPos::HIGH:
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include "UserParameters.h"
|
||||
#include "config.h"
|
||||
|
||||
#if USER_PARAMS_ENABLED == ENABLED
|
||||
#if USER_PARAMS_ENABLED
|
||||
// "USR" + 13 chars remaining for param name
|
||||
const AP_Param::GroupInfo UserParameters::var_info[] = {
|
||||
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
|
||||
#include "Copter.h"
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
|
||||
/*
|
||||
setup radio_out values for all channels to termination values
|
||||
|
|
|
@ -18,7 +18,7 @@
|
|||
advanced failsafe support for copter
|
||||
*/
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
|
||||
|
||||
/*
|
||||
|
|
|
@ -323,7 +323,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
|
|||
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
|
||||
}
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
update_weathervane(_pilot_yaw_rate_cds);
|
||||
#endif
|
||||
|
||||
|
@ -354,7 +354,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
|
||||
#if WEATHERVANE_ENABLED
|
||||
void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds)
|
||||
{
|
||||
if (!copter.flightmode->allows_weathervaning()) {
|
||||
|
@ -382,4 +382,4 @@ void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif // WEATHERVANE_ENABLED == ENABLED
|
||||
#endif // WEATHERVANE_ENABLED
|
||||
|
|
|
@ -25,7 +25,7 @@ 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
|
||||
#if MODE_THROW_ENABLED
|
||||
copter.flightmode->mode_number() == Mode::Number::THROW ||
|
||||
#endif
|
||||
copter.flightmode->mode_number() == Mode::Number::FLIP) {
|
||||
|
@ -148,7 +148,7 @@ 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
|
||||
#if MODE_RTL_ENABLED
|
||||
// do not descend if below RTL alt
|
||||
return copter.g.rtl_altitude;
|
||||
#else
|
||||
|
|
|
@ -30,7 +30,7 @@ void Copter::set_home_to_current_location_inflight() {
|
|||
return;
|
||||
}
|
||||
// we have successfully set AHRS home, set it for SmartRTL
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
g2.smart_rtl.set_home(true);
|
||||
#endif
|
||||
}
|
||||
|
@ -45,7 +45,7 @@ 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
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
g2.smart_rtl.set_home(true);
|
||||
#endif
|
||||
return true;
|
||||
|
|
|
@ -117,101 +117,101 @@
|
|||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Auto Tuning
|
||||
#ifndef AUTOTUNE_ENABLED
|
||||
# define AUTOTUNE_ENABLED ENABLED
|
||||
# define AUTOTUNE_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Nav-Guided - allows external nav computer to control vehicle
|
||||
#ifndef AC_NAV_GUIDED
|
||||
# define AC_NAV_GUIDED ENABLED
|
||||
# define AC_NAV_GUIDED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Acro - fly vehicle in acrobatic mode
|
||||
#ifndef MODE_ACRO_ENABLED
|
||||
# define MODE_ACRO_ENABLED ENABLED
|
||||
# define MODE_ACRO_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Auto mode - allows vehicle to trace waypoints and perform automated actions
|
||||
#ifndef MODE_AUTO_ENABLED
|
||||
# define MODE_AUTO_ENABLED ENABLED
|
||||
# define MODE_AUTO_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Brake mode - bring vehicle to stop
|
||||
#ifndef MODE_BRAKE_ENABLED
|
||||
# define MODE_BRAKE_ENABLED ENABLED
|
||||
# define MODE_BRAKE_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Circle - fly vehicle around a central point
|
||||
#ifndef MODE_CIRCLE_ENABLED
|
||||
# define MODE_CIRCLE_ENABLED ENABLED
|
||||
# define MODE_CIRCLE_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Drift - fly vehicle in altitude-held, coordinated-turn mode
|
||||
#ifndef MODE_DRIFT_ENABLED
|
||||
# define MODE_DRIFT_ENABLED ENABLED
|
||||
# define MODE_DRIFT_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// flip - fly vehicle in flip in pitch and roll direction mode
|
||||
#ifndef MODE_FLIP_ENABLED
|
||||
# define MODE_FLIP_ENABLED ENABLED
|
||||
# define MODE_FLIP_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Follow - follow another vehicle or GCS
|
||||
#ifndef MODE_FOLLOW_ENABLED
|
||||
#if AP_FOLLOW_ENABLED && AP_AVOIDANCE_ENABLED
|
||||
#define MODE_FOLLOW_ENABLED ENABLED
|
||||
#define MODE_FOLLOW_ENABLED 1
|
||||
#else
|
||||
#define MODE_FOLLOW_ENABLED DISABLED
|
||||
#define MODE_FOLLOW_ENABLED 0
|
||||
#endif
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Guided mode - control vehicle's position or angles from GCS
|
||||
#ifndef MODE_GUIDED_ENABLED
|
||||
# define MODE_GUIDED_ENABLED ENABLED
|
||||
# define MODE_GUIDED_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// GuidedNoGPS mode - control vehicle's angles from GCS
|
||||
#ifndef MODE_GUIDED_NOGPS_ENABLED
|
||||
# define MODE_GUIDED_NOGPS_ENABLED ENABLED
|
||||
# define MODE_GUIDED_NOGPS_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Loiter mode - allows vehicle to hold global position
|
||||
#ifndef MODE_LOITER_ENABLED
|
||||
# define MODE_LOITER_ENABLED ENABLED
|
||||
# define MODE_LOITER_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Position Hold - enable holding of global position
|
||||
#ifndef MODE_POSHOLD_ENABLED
|
||||
# define MODE_POSHOLD_ENABLED ENABLED
|
||||
# define MODE_POSHOLD_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// RTL - Return To Launch
|
||||
#ifndef MODE_RTL_ENABLED
|
||||
# define MODE_RTL_ENABLED ENABLED
|
||||
# define MODE_RTL_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home
|
||||
#ifndef MODE_SMARTRTL_ENABLED
|
||||
# define MODE_SMARTRTL_ENABLED ENABLED
|
||||
# define MODE_SMARTRTL_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Sport - fly vehicle in rate-controlled (earth-frame) mode
|
||||
#ifndef MODE_SPORT_ENABLED
|
||||
# define MODE_SPORT_ENABLED DISABLED
|
||||
# define MODE_SPORT_ENABLED 0
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -223,13 +223,13 @@
|
|||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Throw - fly vehicle after throwing it in the air
|
||||
#ifndef MODE_THROW_ENABLED
|
||||
# define MODE_THROW_ENABLED ENABLED
|
||||
# define MODE_THROW_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B
|
||||
#ifndef MODE_ZIGZAG_ENABLED
|
||||
# define MODE_ZIGZAG_ENABLED ENABLED
|
||||
# define MODE_ZIGZAG_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -249,7 +249,7 @@
|
|||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Weathervane - allow vehicle to yaw into wind
|
||||
#ifndef WEATHERVANE_ENABLED
|
||||
# define WEATHERVANE_ENABLED ENABLED
|
||||
# define WEATHERVANE_ENABLED 1
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -260,13 +260,13 @@
|
|||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
#ifndef MODE_AUTOROTATE_ENABLED
|
||||
# define MODE_AUTOROTATE_ENABLED ENABLED
|
||||
# define MODE_AUTOROTATE_ENABLED 1
|
||||
#endif
|
||||
#else
|
||||
# define MODE_AUTOROTATE_ENABLED DISABLED
|
||||
# define MODE_AUTOROTATE_ENABLED 0
|
||||
#endif
|
||||
#else
|
||||
# define MODE_AUTOROTATE_ENABLED DISABLED
|
||||
# define MODE_AUTOROTATE_ENABLED 0
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
@ -575,7 +575,7 @@
|
|||
//
|
||||
|
||||
#ifndef ADVANCED_FAILSAFE
|
||||
# define ADVANCED_FAILSAFE DISABLED
|
||||
# define ADVANCED_FAILSAFE 0
|
||||
#endif
|
||||
|
||||
#ifndef CH_MODE_DEFAULT
|
||||
|
@ -583,7 +583,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef TOY_MODE_ENABLED
|
||||
#define TOY_MODE_ENABLED DISABLED
|
||||
#define TOY_MODE_ENABLED 0
|
||||
#endif
|
||||
|
||||
#if TOY_MODE_ENABLED && FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -603,5 +603,5 @@
|
|||
#endif
|
||||
|
||||
#ifndef USER_PARAMS_ENABLED
|
||||
#define USER_PARAMS_ENABLED DISABLED
|
||||
#define USER_PARAMS_ENABLED 0
|
||||
#endif
|
||||
|
|
|
@ -47,7 +47,7 @@ void Copter::crash_check()
|
|||
return;
|
||||
}
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
//return immediately if in autorotation mode
|
||||
if (flightmode->mode_number() == Mode::Number::AUTOROTATE) {
|
||||
crash_counter = 0;
|
||||
|
|
|
@ -2,14 +2,6 @@
|
|||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
// Just so that it's completely clear...
|
||||
#define ENABLED 1
|
||||
#define DISABLED 0
|
||||
|
||||
// this avoids a very common config error
|
||||
#define ENABLE ENABLED
|
||||
#define DISABLE DISABLED
|
||||
|
||||
// Frame types
|
||||
#define UNDEFINED_FRAME 0
|
||||
#define MULTICOPTER_FRAME 1
|
||||
|
|
|
@ -286,7 +286,7 @@ void Copter::failsafe_terrain_on_event()
|
|||
|
||||
if (should_disarm_on_failsafe()) {
|
||||
arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
} else if (flightmode->mode_number() == Mode::Number::RTL) {
|
||||
mode_rtl.restart_without_terrain();
|
||||
#endif
|
||||
|
@ -425,7 +425,7 @@ 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 MODE_AUTO_ENABLED
|
||||
if (set_mode(Mode::Number::AUTO_RTL, reason)) {
|
||||
AP_Notify::events.failsafe_mode_change = 1;
|
||||
return;
|
||||
|
@ -440,7 +440,7 @@ 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 MODE_BRAKE_ENABLED
|
||||
if (set_mode(Mode::Number::BRAKE, reason)) {
|
||||
AP_Notify::events.failsafe_mode_change = 1;
|
||||
return;
|
||||
|
@ -492,7 +492,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
|
|||
set_mode_SmartRTL_or_land_with_pause(reason);
|
||||
break;
|
||||
case FailsafeAction::TERMINATE: {
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
g2.afs.gcs_terminate(true, "Failsafe");
|
||||
#else
|
||||
arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
|
||||
|
|
|
@ -72,7 +72,7 @@ void Copter::failsafe_check()
|
|||
}
|
||||
|
||||
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
/*
|
||||
check for AFS failsafe check
|
||||
*/
|
||||
|
|
|
@ -191,7 +191,7 @@ void Copter::heli_update_autorotation()
|
|||
{
|
||||
// check if flying and interlock disengaged
|
||||
if (!ap.land_complete && !motors->get_interlock()) {
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
if (g2.arot.is_enable()) {
|
||||
if (!flightmode->has_manual_throttle()) {
|
||||
// set autonomous autorotation flight mode
|
||||
|
|
|
@ -83,7 +83,7 @@ void Copter::update_land_detector()
|
|||
const bool landing = flightmode->is_landing();
|
||||
SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
|
||||
bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll()) < M_PI/2.0f)
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
|| (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
|
||||
#endif
|
||||
|| ((!get_force_flying() || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f);
|
||||
|
|
|
@ -35,7 +35,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
|
|||
Mode *ret = nullptr;
|
||||
|
||||
switch (mode) {
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
case Mode::Number::ACRO:
|
||||
ret = &mode_acro;
|
||||
break;
|
||||
|
@ -49,25 +49,25 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
|
|||
ret = &mode_althold;
|
||||
break;
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
case Mode::Number::AUTO:
|
||||
ret = &mode_auto;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
case Mode::Number::CIRCLE:
|
||||
ret = &mode_circle;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_LOITER_ENABLED == ENABLED
|
||||
#if MODE_LOITER_ENABLED
|
||||
case Mode::Number::LOITER:
|
||||
ret = &mode_loiter;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
case Mode::Number::GUIDED:
|
||||
ret = &mode_guided;
|
||||
break;
|
||||
|
@ -77,49 +77,49 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
|
|||
ret = &mode_land;
|
||||
break;
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
case Mode::Number::RTL:
|
||||
ret = &mode_rtl;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_DRIFT_ENABLED
|
||||
case Mode::Number::DRIFT:
|
||||
ret = &mode_drift;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_SPORT_ENABLED
|
||||
case Mode::Number::SPORT:
|
||||
ret = &mode_sport;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_FLIP_ENABLED == ENABLED
|
||||
#if MODE_FLIP_ENABLED
|
||||
case Mode::Number::FLIP:
|
||||
ret = &mode_flip;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
case Mode::Number::AUTOTUNE:
|
||||
ret = &mode_autotune;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_ENABLED
|
||||
case Mode::Number::POSHOLD:
|
||||
ret = &mode_poshold;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_BRAKE_ENABLED == ENABLED
|
||||
#if MODE_BRAKE_ENABLED
|
||||
case Mode::Number::BRAKE:
|
||||
ret = &mode_brake;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
case Mode::Number::THROW:
|
||||
ret = &mode_throw;
|
||||
break;
|
||||
|
@ -131,49 +131,49 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_NOGPS_ENABLED
|
||||
case Mode::Number::GUIDED_NOGPS:
|
||||
ret = &mode_guided_nogps;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
case Mode::Number::SMART_RTL:
|
||||
ret = &mode_smartrtl;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_FLOWHOLD_ENABLED == ENABLED
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
case Mode::Number::FLOWHOLD:
|
||||
ret = (Mode *)g2.mode_flowhold_ptr;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
case Mode::Number::FOLLOW:
|
||||
ret = &mode_follow;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
case Mode::Number::ZIGZAG:
|
||||
ret = &mode_zigzag;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
case Mode::Number::SYSTEMID:
|
||||
ret = (Mode *)g2.mode_systemid_ptr;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
case Mode::Number::AUTOROTATE:
|
||||
ret = &mode_autorotate;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_TURTLE_ENABLED == ENABLED
|
||||
#if MODE_TURTLE_ENABLED
|
||||
case Mode::Number::TURTLE:
|
||||
ret = &mode_turtle;
|
||||
break;
|
||||
|
@ -273,7 +273,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
|
|||
return false;
|
||||
}
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
if (mode == Mode::Number::AUTO_RTL) {
|
||||
// Special case for AUTO RTL, not a true mode, just AUTO in disguise
|
||||
// Attempt to join return path, fallback to do-land-start
|
||||
|
@ -294,7 +294,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
|
|||
// rotor runup is not complete
|
||||
if (!ignore_checks && !new_flightmode->has_manual_throttle() &&
|
||||
(motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
//if the mode being exited is the autorotation mode allow mode change despite rotor not being at
|
||||
//full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes
|
||||
bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
|
||||
|
@ -315,7 +315,7 @@ 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 MODE_DRIFT_ENABLED
|
||||
if (new_flightmode == &mode_drift) {
|
||||
user_throttle = true;
|
||||
}
|
||||
|
@ -398,11 +398,11 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
|
|||
#endif
|
||||
|
||||
// set rate shaping time constants
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
|
||||
#endif
|
||||
attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
|
||||
if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
|
||||
attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
|
||||
}
|
||||
|
|
|
@ -190,7 +190,7 @@ public:
|
|||
void make_safe_ground_handling(bool force_throttle_unlimited = false);
|
||||
|
||||
// true if weathervaning is allowed in the current mode
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
virtual bool allows_weathervaning() const { return false; }
|
||||
#endif
|
||||
|
||||
|
@ -331,7 +331,7 @@ public:
|
|||
|
||||
bool reached_fixed_yaw_target();
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
void update_weathervane(const int16_t pilot_yaw_cds);
|
||||
#endif
|
||||
|
||||
|
@ -390,7 +390,7 @@ public:
|
|||
};
|
||||
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
class ModeAcro : public Mode {
|
||||
|
||||
public:
|
||||
|
@ -582,7 +582,7 @@ public:
|
|||
AP_Mission_ChangeDetector mis_change_detector;
|
||||
|
||||
// true if weathervaning is allowed in auto
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
bool allows_weathervaning(void) const override;
|
||||
#endif
|
||||
|
||||
|
@ -641,7 +641,7 @@ private:
|
|||
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
|
||||
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline);
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
|
@ -679,7 +679,7 @@ private:
|
|||
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||
|
@ -752,7 +752,7 @@ private:
|
|||
} desired_speed_override;
|
||||
};
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
/*
|
||||
wrapper class for AC_AutoTune
|
||||
*/
|
||||
|
@ -935,7 +935,7 @@ private:
|
|||
};
|
||||
|
||||
|
||||
#if MODE_FLOWHOLD_ENABLED == ENABLED
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
/*
|
||||
class to support FLOWHOLD mode, which is a position hold mode using
|
||||
optical flow directly, avoiding the need for a rangefinder
|
||||
|
@ -1116,7 +1116,7 @@ public:
|
|||
bool resume() override;
|
||||
|
||||
// true if weathervaning is allowed in guided
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
bool allows_weathervaning(void) const override;
|
||||
#endif
|
||||
|
||||
|
@ -1753,7 +1753,7 @@ private:
|
|||
float free_fall_start_velz; // vertical velocity when free fall was detected
|
||||
};
|
||||
|
||||
#if MODE_TURTLE_ENABLED == ENABLED
|
||||
#if MODE_TURTLE_ENABLED
|
||||
class ModeTurtle : public Mode {
|
||||
|
||||
public:
|
||||
|
@ -1814,7 +1814,7 @@ private:
|
|||
|
||||
};
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
class ModeFollow : public ModeGuided {
|
||||
|
||||
public:
|
||||
|
@ -1943,7 +1943,7 @@ private:
|
|||
bool is_suspended; // true if zigzag auto is suspended
|
||||
};
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
class ModeAutorotate : public Mode {
|
||||
|
||||
public:
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include "mode.h"
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for acro flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_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
|
||||
#endif //MODE_ACRO_ENABLED
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for auto flight mode
|
||||
|
@ -139,7 +139,7 @@ void ModeAuto::run()
|
|||
|
||||
case SubMode::NAVGUIDED:
|
||||
case SubMode::NAV_SCRIPT_TIME:
|
||||
#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
|
||||
#if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
|
||||
nav_guided_run();
|
||||
#endif
|
||||
break;
|
||||
|
@ -215,7 +215,7 @@ bool ModeAuto::allows_arming(AP_Arming::Method method) const
|
|||
return option_is_enabled(Option::AllowArming);
|
||||
}
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
bool ModeAuto::allows_weathervaning() const
|
||||
{
|
||||
return option_is_enabled(Option::AllowWeatherVaning);
|
||||
|
@ -569,7 +569,7 @@ void ModeAuto::circle_start()
|
|||
set_submode(SubMode::CIRCLE);
|
||||
}
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
|
||||
void ModeAuto::nav_guided_start()
|
||||
{
|
||||
|
@ -721,7 +721,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
do_spline_wp(cmd);
|
||||
break;
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
|
||||
do_nav_guided_enable(cmd);
|
||||
break;
|
||||
|
@ -783,7 +783,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
do_mount_control(cmd);
|
||||
break;
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
|
||||
do_guided_limits(cmd);
|
||||
break;
|
||||
|
@ -959,7 +959,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
|
|||
cmd_complete = verify_spline_wp(cmd);
|
||||
break;
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
case MAV_CMD_NAV_GUIDED_ENABLE:
|
||||
cmd_complete = verify_nav_guided_enable(cmd);
|
||||
break;
|
||||
|
@ -1101,7 +1101,7 @@ void ModeAuto::circle_run()
|
|||
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
|
||||
}
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
|
||||
#if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
|
||||
// auto_nav_guided_run - allows control by external navigation controller
|
||||
// called by auto_run at 100hz or more
|
||||
void ModeAuto::nav_guided_run()
|
||||
|
@ -1282,7 +1282,7 @@ void PayloadPlace::run()
|
|||
}
|
||||
}
|
||||
|
||||
#if AP_GRIPPER_ENABLED == ENABLED
|
||||
#if AP_GRIPPER_ENABLED
|
||||
// if pilot releases load manually:
|
||||
if (AP::gripper().valid() && AP::gripper().released()) {
|
||||
switch (state) {
|
||||
|
@ -1377,7 +1377,7 @@ void PayloadPlace::run()
|
|||
case State::Release:
|
||||
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload
|
||||
pos_control->init_z_controller_no_descent();
|
||||
#if AP_GRIPPER_ENABLED == ENABLED
|
||||
#if AP_GRIPPER_ENABLED
|
||||
if (AP::gripper().valid()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str);
|
||||
AP::gripper().release();
|
||||
|
@ -1811,7 +1811,7 @@ void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const
|
|||
}
|
||||
}
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
// do_nav_guided_enable - initiate accepting commands from external nav computer
|
||||
void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
|
@ -2265,7 +2265,7 @@ bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
|
|||
return false;
|
||||
}
|
||||
|
||||
#if AC_NAV_GUIDED == ENABLED
|
||||
#if AC_NAV_GUIDED
|
||||
// verify_nav_guided - check if we have breached any limits
|
||||
bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
|
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
#include <utility>
|
||||
|
||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||
#if MODE_AUTOROTATE_ENABLED
|
||||
|
||||
#define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for
|
||||
#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
autotune mode is a wrapper around the AC_AutoTune library
|
||||
*/
|
||||
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
#if AUTOTUNE_ENABLED
|
||||
|
||||
bool AutoTune::init()
|
||||
{
|
||||
|
@ -123,4 +123,4 @@ void ModeAutoTune::exit()
|
|||
autotune.stop();
|
||||
}
|
||||
|
||||
#endif // AUTOTUNE_ENABLED == ENABLED
|
||||
#endif // AUTOTUNE_ENABLED
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_BRAKE_ENABLED == ENABLED
|
||||
#if MODE_BRAKE_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for brake flight mode
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include "Copter.h"
|
||||
#include <AP_Mount/AP_Mount.h>
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for circle flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_DRIFT_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for drift flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_FLIP_ENABLED == ENABLED
|
||||
#if MODE_FLIP_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for flip flight mode
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include "Copter.h"
|
||||
#include <utility>
|
||||
|
||||
#if MODE_FLOWHOLD_ENABLED == ENABLED
|
||||
#if MODE_FLOWHOLD_ENABLED
|
||||
|
||||
/*
|
||||
implement FLOWHOLD mode, for position hold using optical flow
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_FOLLOW_ENABLED == ENABLED
|
||||
#if MODE_FOLLOW_ENABLED
|
||||
|
||||
/*
|
||||
* mode_follow.cpp - follow another mavlink-enabled vehicle by system id
|
||||
|
@ -172,4 +172,4 @@ bool ModeFollow::get_wp(Location &loc) const
|
|||
return true;
|
||||
}
|
||||
|
||||
#endif // MODE_FOLLOW_ENABLED == ENABLED
|
||||
#endif // MODE_FOLLOW_ENABLED
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for guided flight mode
|
||||
|
@ -114,7 +114,7 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
|
|||
return option_is_enabled(Option::AllowArmingFromTX);
|
||||
};
|
||||
|
||||
#if WEATHERVANE_ENABLED == ENABLED
|
||||
#if WEATHERVANE_ENABLED
|
||||
bool ModeGuided::allows_weathervaning() const
|
||||
{
|
||||
return option_is_enabled(Option::AllowWeatherVaning);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
|
||||
#if MODE_GUIDED_NOGPS_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for guided_nogps flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_LOITER_ENABLED == ENABLED
|
||||
#if MODE_LOITER_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for loiter flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_POSHOLD_ENABLED == ENABLED
|
||||
#if MODE_POSHOLD_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for PosHold flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_RTL_ENABLED == ENABLED
|
||||
#if MODE_RTL_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for RTL flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for Smart_RTL flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_SPORT_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for sport flight mode
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#include "Copter.h"
|
||||
#include <AP_Math/control.h>
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for systemId, flight mode
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
|
||||
// throw_init - initialise throw controller
|
||||
bool ModeThrow::init(bool ignore_checks)
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_TURTLE_ENABLED == ENABLED
|
||||
#if MODE_TURTLE_ENABLED
|
||||
|
||||
#define CRASH_FLIP_EXPO 35.0f
|
||||
#define CRASH_FLIP_STICK_MINF 0.15f
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#if MODE_ZIGZAG_ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for zigzag flight mode
|
||||
|
@ -604,4 +604,4 @@ float ModeZigZag::crosstrack_error() const
|
|||
return is_auto ? wp_nav->crosstrack_error() : 0;
|
||||
}
|
||||
|
||||
#endif // MODE_ZIGZAG_ENABLED == ENABLED
|
||||
#endif // MODE_ZIGZAG_ENABLED
|
||||
|
|
|
@ -20,7 +20,7 @@ void Copter::arm_motors_check()
|
|||
return;
|
||||
}
|
||||
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
if (g2.toy_mode.enabled()) {
|
||||
// not armed with sticks in toy mode
|
||||
return;
|
||||
|
@ -135,7 +135,7 @@ void Copter::auto_disarm_check()
|
|||
// motors_output - send output to motors library which will adjust and send to ESCs and servos
|
||||
void Copter::motors_output()
|
||||
{
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
#if ADVANCED_FAILSAFE
|
||||
// this is to allow the failsafe module to deliberately crash
|
||||
// the vehicle. Only used in extreme circumstances to meet the
|
||||
// OBC rules
|
||||
|
|
|
@ -197,7 +197,7 @@ void Copter::radio_passthrough_to_motors()
|
|||
*/
|
||||
int16_t Copter::get_throttle_mid(void)
|
||||
{
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
if (g2.toy_mode.enabled()) {
|
||||
return g2.toy_mode.get_throttle_mid();
|
||||
}
|
||||
|
|
|
@ -37,7 +37,7 @@ void Copter::init_ardupilot()
|
|||
// setup telem slots with serial ports
|
||||
gcs().setup_uarts();
|
||||
|
||||
#if OSD_ENABLED == ENABLED
|
||||
#if OSD_ENABLED
|
||||
osd.init();
|
||||
#endif
|
||||
|
||||
|
@ -157,12 +157,12 @@ void Copter::init_ardupilot()
|
|||
rpm_sensor.init();
|
||||
#endif
|
||||
|
||||
#if MODE_AUTO_ENABLED == ENABLED
|
||||
#if MODE_AUTO_ENABLED
|
||||
// initialise mission library
|
||||
mode_auto.mission.init();
|
||||
#endif
|
||||
|
||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||
#if MODE_SMARTRTL_ENABLED
|
||||
// initialize SmartRTL
|
||||
g2.smart_rtl.init();
|
||||
#endif
|
||||
|
@ -174,7 +174,7 @@ void Copter::init_ardupilot()
|
|||
|
||||
startup_INS_ground();
|
||||
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
|
||||
#if AC_CUSTOMCONTROL_MULTI_ENABLED
|
||||
custom_control.init();
|
||||
#endif
|
||||
|
||||
|
@ -469,7 +469,7 @@ void Copter::allocate_motors(void)
|
|||
}
|
||||
AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
|
||||
|
||||
#if MODE_CIRCLE_ENABLED == ENABLED
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
circle_nav = NEW_NOTHROW AC_Circle(inertial_nav, *ahrs_view, *pos_control);
|
||||
if (circle_nav == nullptr) {
|
||||
AP_BoardConfig::allocation_error("CircleNav");
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if TOY_MODE_ENABLED == ENABLED
|
||||
#if TOY_MODE_ENABLED
|
||||
|
||||
// times in 0.1s units
|
||||
#define TOY_COMMAND_DELAY 15
|
||||
|
@ -490,7 +490,7 @@ void ToyMode::update()
|
|||
break;
|
||||
|
||||
case ACTION_MODE_ACRO:
|
||||
#if MODE_ACRO_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED
|
||||
new_mode = Mode::Number::ACRO;
|
||||
#else
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ACRO is disabled");
|
||||
|
@ -542,7 +542,7 @@ void ToyMode::update()
|
|||
break;
|
||||
|
||||
case ACTION_MODE_THROW:
|
||||
#if MODE_THROW_ENABLED == ENABLED
|
||||
#if MODE_THROW_ENABLED
|
||||
new_mode = Mode::Number::THROW;
|
||||
#else
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: THROW is disabled");
|
||||
|
|
|
@ -107,14 +107,14 @@ void Copter::tuning()
|
|||
wp_nav->set_speed_xy(tuning_value);
|
||||
break;
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
|
||||
// Acro roll pitch rates
|
||||
case TUNING_ACRO_RP_RATE:
|
||||
g2.command_model_acro_rp.set_rate(tuning_value);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
|
||||
#if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
|
||||
// Acro yaw rate
|
||||
case TUNING_ACRO_YAW_RATE:
|
||||
g2.command_model_acro_y.set_rate(tuning_value);
|
||||
|
@ -143,7 +143,7 @@ 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
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
case TUNING_CIRCLE_RATE:
|
||||
circle_nav->set_rate(tuning_value);
|
||||
break;
|
||||
|
@ -188,7 +188,7 @@ void Copter::tuning()
|
|||
break;
|
||||
|
||||
case TUNING_SYSTEM_ID_MAGNITUDE:
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
#if MODE_SYSTEMID_ENABLED
|
||||
copter.mode_systemid.set_magnitude(tuning_value);
|
||||
#endif
|
||||
break;
|
||||
|
|
|
@ -122,7 +122,7 @@ STORAGE_FLASH_PAGE 1
|
|||
define HAL_STORAGE_SIZE 15360
|
||||
|
||||
# setup defines for ArduCopter config
|
||||
define TOY_MODE_ENABLED ENABLED
|
||||
define TOY_MODE_ENABLED 1
|
||||
define ARMING_DELAY_SEC 0
|
||||
define LAND_START_ALT 700
|
||||
define LAND_DETECTOR_ACCEL_MAX 2.0f
|
||||
|
|
|
@ -113,7 +113,7 @@ STORAGE_FLASH_PAGE 1
|
|||
define HAL_STORAGE_SIZE 15360
|
||||
|
||||
# setup defines for ArduCopter config
|
||||
define TOY_MODE_ENABLED ENABLED
|
||||
define TOY_MODE_ENABLED 1
|
||||
define ARMING_DELAY_SEC 0
|
||||
define LAND_START_ALT 700
|
||||
define LAND_DETECTOR_ACCEL_MAX 2.0f
|
||||
|
|
|
@ -44,7 +44,7 @@ PD15 MPU_DRDY INPUT GPIO(100)
|
|||
define HAL_GPIO_RADIO_IRQ 100
|
||||
|
||||
# setup defines for ArduCopter config
|
||||
define TOY_MODE_ENABLED ENABLED
|
||||
define TOY_MODE_ENABLED 1
|
||||
define ARMING_DELAY_SEC 0
|
||||
define LAND_START_ALT 700
|
||||
define LAND_DETECTOR_ACCEL_MAX 2.0f
|
||||
|
|
Loading…
Reference in New Issue