mirror of https://github.com/ArduPilot/ardupilot
610 lines
15 KiB
C
610 lines
15 KiB
C
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
//
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
|
|
//
|
|
// DO NOT EDIT this file to adjust your configuration. Create your own
|
|
// APM_Config.h and use APM_Config.h.example as a reference.
|
|
//
|
|
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
|
|
///
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Default and automatic configuration details.
|
|
//
|
|
// Notes for maintainers:
|
|
//
|
|
// - Try to keep this file organised in the same order as APM_Config.h.example
|
|
//
|
|
|
|
#include "defines.h"
|
|
|
|
///
|
|
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
|
/// change in your local copy of APM_Config.h.
|
|
///
|
|
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
|
|
///
|
|
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
|
|
/// change in your local copy of APM_Config.h.
|
|
///
|
|
|
|
// Just so that it's completely clear...
|
|
#define ENABLED 1
|
|
#define DISABLED 0
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// HARDWARE CONFIGURATION AND CONNECTIONS
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// AIRSPEED_SENSOR
|
|
// AIRSPEED_RATIO
|
|
//
|
|
#ifndef AIRSPEED_SENSOR
|
|
# define AIRSPEED_SENSOR DISABLED
|
|
#endif
|
|
#ifndef AIRSPEED_RATIO
|
|
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// HIL_PROTOCOL OPTIONAL
|
|
// HIL_MODE OPTIONAL
|
|
// HIL_PORT OPTIONAL
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
|
|
|
# undef GPS_PROTOCOL
|
|
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
|
|
|
|
#ifndef HIL_PROTOCOL
|
|
# error Must define HIL_PROTOCOL if HIL_MODE is not disabled
|
|
#endif
|
|
|
|
#ifndef HIL_PORT
|
|
# error Must define HIL_PORT if HIL_PROTOCOL is set
|
|
#endif
|
|
|
|
#endif
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
|
|
|
// check xplane settings
|
|
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
|
|
// MAGNETOMETER not supported by XPLANE
|
|
# undef MAGNETOMETER
|
|
# define MAGNETOMETER DISABLED
|
|
# if HIL_MODE != HIL_MODE_ATTITUDE
|
|
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
|
|
# endif
|
|
#endif
|
|
|
|
#endif // HIL_MODE != HIL_MODE_DISABLED
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// GPS_PROTOCOL
|
|
//
|
|
// Note that this test must follow the HIL_PROTOCOL block as the HIL
|
|
// setup may override the GPS configuration.
|
|
//
|
|
#ifndef GPS_PROTOCOL
|
|
# error XXX
|
|
# error XXX You must define GPS_PROTOCOL in APM_Config.h, or select a HIL configuration.
|
|
# error XXX
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// GCS_PROTOCOL
|
|
// GCS_PORT
|
|
//
|
|
#ifndef GCS_PROTOCOL
|
|
# define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
|
#endif
|
|
|
|
#ifndef DEBUGTERMINAL_VERBOSE
|
|
# define DEBUGTERMINAL_VERBOSE 1 //Set this to 1 to get more detail in DEBUGTERMINAL messages, or 0 to save flash space
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Serial port speeds.
|
|
//
|
|
#ifndef SERIAL0_BAUD
|
|
# define SERIAL0_BAUD 115200
|
|
#endif
|
|
#ifndef SERIAL3_BAUD
|
|
# define SERIAL3_BAUD 57600
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Battery monitoring
|
|
//
|
|
#ifndef BATTERY_EVENT
|
|
# define BATTERY_EVENT DISABLED
|
|
#endif
|
|
#ifndef BATTERY_TYPE
|
|
# define BATTERY_TYPE 0
|
|
#endif
|
|
#ifndef LOW_VOLTAGE
|
|
# define LOW_VOLTAGE 11.4
|
|
#endif
|
|
#ifndef VOLT_DIV_RATIO
|
|
# define VOLT_DIV_RATIO 3.0
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// INPUT_VOLTAGE
|
|
//
|
|
#ifndef INPUT_VOLTAGE
|
|
# define INPUT_VOLTAGE 5.0
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// MAGNETOMETER
|
|
// NOTE - There is no support for using the magnetometer in v1.0
|
|
#ifndef MAGNETOMETER
|
|
# define MAGNETOMETER DISABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// RADIO CONFIGURATION
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Radio channel limits
|
|
//
|
|
// Note that these are not called out in APM_Config.h.example as there
|
|
// is currently no configured behaviour for these channels.
|
|
//
|
|
#ifndef CH5_MIN
|
|
# define CH5_MIN 1000
|
|
#endif
|
|
#ifndef CH5_MAX
|
|
# define CH5_MAX 2000
|
|
#endif
|
|
#ifndef CH6_MIN
|
|
# define CH6_MIN 1000
|
|
#endif
|
|
#ifndef CH6_MAX
|
|
# define CH6_MAX 2000
|
|
#endif
|
|
#ifndef CH7_MIN
|
|
# define CH7_MIN 1000
|
|
#endif
|
|
#ifndef CH7_MAX
|
|
# define CH7_MAX 2000
|
|
#endif
|
|
#ifndef CH8_MIN
|
|
# define CH8_MIN 1000
|
|
#endif
|
|
#ifndef CH8_MAX
|
|
# define CH8_MAX 2000
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// FLIGHT_MODE
|
|
// FLIGHT_MODE_CHANNEL
|
|
//
|
|
#ifndef FLIGHT_MODE_CHANNEL
|
|
# define FLIGHT_MODE_CHANNEL 8
|
|
#endif
|
|
#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8)
|
|
# error XXX
|
|
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
|
|
# error XXX
|
|
#endif
|
|
|
|
#if !defined(FLIGHT_MODE_1)
|
|
# define FLIGHT_MODE_1 FLY_BY_WIRE_A
|
|
#endif
|
|
#if !defined(FLIGHT_MODE_2)
|
|
# define FLIGHT_MODE_2 FLY_BY_WIRE_A
|
|
#endif
|
|
#if !defined(FLIGHT_MODE_3)
|
|
# define FLIGHT_MODE_3 STABILIZE
|
|
#endif
|
|
#if !defined(FLIGHT_MODE_4)
|
|
# define FLIGHT_MODE_4 STABILIZE
|
|
#endif
|
|
#if !defined(FLIGHT_MODE_5)
|
|
# define FLIGHT_MODE_5 MANUAL
|
|
#endif
|
|
#if !defined(FLIGHT_MODE_6)
|
|
# define FLIGHT_MODE_6 MANUAL
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// THROTTLE_FAILSAFE
|
|
// THROTTLE_FS_VALUE
|
|
// THROTTLE_FAILSAFE_ACTION
|
|
//
|
|
#ifndef THROTTLE_FAILSAFE
|
|
# define THROTTLE_FAILSAFE DISABLED
|
|
#endif
|
|
#ifndef THROTTE_FS_VALUE
|
|
# define THROTTLE_FS_VALUE 975
|
|
#endif
|
|
#ifndef THROTTLE_FAILSAFE_ACTION
|
|
# define THROTTLE_FAILSAFE_ACTION 2
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// AUTO_TRIM
|
|
//
|
|
#ifndef AUTO_TRIM
|
|
# define AUTO_TRIM ENABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// THROTTLE_REVERSE
|
|
//
|
|
#ifndef THROTTLE_REVERSE
|
|
# define THROTTLE_REVERSE DISABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// ENABLE_STICK_MIXING
|
|
//
|
|
#ifndef ENABLE_STICK_MIXING
|
|
# define ENABLE_STICK_MIXING ENABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// THROTTLE_OUT
|
|
//
|
|
#ifndef THROTTE_OUT
|
|
# define THROTTLE_OUT ENABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// STARTUP BEHAVIOUR
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// GROUND_START_DELAY
|
|
//
|
|
#ifndef GROUND_START_DELAY
|
|
# define GROUND_START_DELAY 0
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// ENABLE_AIR_START
|
|
//
|
|
#ifndef ENABLE_AIR_START
|
|
# define ENABLE_AIR_START DISABLED
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// ENABLE REVERSE_SWITCH
|
|
//
|
|
#ifndef REVERSE_SWITCH
|
|
# define REVERSE_SWITCH ENABLED
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// FLIGHT AND NAVIGATION CONTROL
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Altitude measurement and control.
|
|
//
|
|
#ifndef AOA
|
|
# define AOA 100 // XXX still 100ths of a degree
|
|
#endif
|
|
#ifndef ALT_EST_GAIN
|
|
# define ALT_EST_GAIN 0.01
|
|
#endif
|
|
#ifndef ALTITUDE_MIX
|
|
# define ALTITUDE_MIX 0
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// AIRSPEED_CRUISE
|
|
//
|
|
#ifndef AIRSPEED_CRUISE
|
|
# define AIRSPEED_CRUISE 1000 // 10 m/s * 100
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// FLY_BY_WIRE_B airspeed control
|
|
//
|
|
#ifndef AIRSPEED_FBW_MIN
|
|
# define AIRSPEED_FBW_MIN 6
|
|
#endif
|
|
#ifndef AIRSPEED_FBW_MAX
|
|
# define AIRSPEED_FBW_MAX 30
|
|
#endif
|
|
#ifndef THROTTLE_ALT_P
|
|
# define THROTTLE_ALT_P 0.32
|
|
#endif
|
|
#ifndef THROTTLE_ALT_I
|
|
# define THROTTLE_ALT_I 0.0
|
|
#endif
|
|
#ifndef THROTTLE_ALT_D
|
|
# define THROTTLE_ALT_D 0.0
|
|
#endif
|
|
#ifndef THROTTLE_ALT_INT_MAX
|
|
# define THROTTLE_ALT_INT_MAX 20
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Throttle control
|
|
//
|
|
#ifndef THROTTLE_MIN
|
|
# define THROTTLE_MIN 0
|
|
#endif
|
|
#ifndef THROTTLE_CRUISE
|
|
# define THROTTLE_CRUISE 45
|
|
#endif
|
|
#ifndef THROTTLE_MAX
|
|
# define THROTTLE_MAX 75
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Autopilot control limits
|
|
//
|
|
#ifndef HEAD_MAX
|
|
# define HEAD_MAX 4500 // deg * 100
|
|
#endif
|
|
#ifndef PITCH_MAX
|
|
# define PITCH_MAX 1500 // deg * 100
|
|
#endif
|
|
#ifndef PITCH_MIN
|
|
# define PITCH_MIN -2500 // deg * 100
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Attitude control gains
|
|
//
|
|
#ifndef SERVO_ROLL_P
|
|
# define SERVO_ROLL_P 0.4
|
|
#endif
|
|
#ifndef SERVO_ROLL_I
|
|
# define SERVO_ROLL_I 0.0
|
|
#endif
|
|
#ifndef SERVO_ROLL_D
|
|
# define SERVO_ROLL_D 0.0
|
|
#endif
|
|
#ifndef SERVO_ROLL_INT_MAX
|
|
# define SERVO_ROLL_INT_MAX 5
|
|
#endif
|
|
#ifndef ROLL_SLEW_LIMIT
|
|
# define ROLL_SLEW_LIMIT 0
|
|
#endif
|
|
#ifndef SERVO_PITCH_P
|
|
# define SERVO_PITCH_P 0.6
|
|
#endif
|
|
#ifndef SERVO_PITCH_I
|
|
# define SERVO_PITCH_I 0.0
|
|
#endif
|
|
#ifndef SERVO_PITCH_D
|
|
# define SERVO_PITCH_D 0.0
|
|
#endif
|
|
#ifndef SERVO_PITCH_INT_MAX
|
|
# define SERVO_PITCH_INT_MAX 5
|
|
#endif
|
|
#ifndef PITCH_COMP
|
|
# define PITCH_COMP 0.2
|
|
#endif
|
|
#ifndef SERVO_YAW_P
|
|
# define SERVO_YAW_P 0.0
|
|
#endif
|
|
#ifndef SERVO_YAW_I
|
|
# define SERVO_YAW_I 0.0
|
|
#endif
|
|
#ifndef SERVO_YAW_D
|
|
# define SERVO_YAW_D 0.0
|
|
#endif
|
|
#ifndef SERVO_YAW_INT_MAX
|
|
# define SERVO_YAW_INT_MAX 5
|
|
#endif
|
|
#ifndef RUDDER_MIX
|
|
# define RUDDER_MIX 0.5
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Navigation control gains
|
|
//
|
|
#ifndef NAV_ROLL_P
|
|
# define NAV_ROLL_P 0.7
|
|
#endif
|
|
#ifndef NAV_ROLL_I
|
|
# define NAV_ROLL_I 0.0
|
|
#endif
|
|
#ifndef NAV_ROLL_D
|
|
# define NAV_ROLL_D 0.02
|
|
#endif
|
|
#ifndef NAV_ROLL_INT_MAX
|
|
# define NAV_ROLL_INT_MAX 5
|
|
#endif
|
|
#ifndef NAV_PITCH_ASP_P
|
|
# define NAV_PITCH_ASP_P 0.65
|
|
#endif
|
|
#ifndef NAV_PITCH_ASP_I
|
|
# define NAV_PITCH_ASP_I 0.0
|
|
#endif
|
|
#ifndef NAV_PITCH_ASP_D
|
|
# define NAV_PITCH_ASP_D 0.0
|
|
#endif
|
|
#ifndef NAV_PITCH_ASP_INT_MAX
|
|
# define NAV_PITCH_ASP_INT_MAX 5
|
|
#endif
|
|
#ifndef NAV_PITCH_ALT_P
|
|
# define NAV_PITCH_ALT_P 0.65
|
|
#endif
|
|
#ifndef NAV_PITCH_ALT_I
|
|
# define NAV_PITCH_ALT_I 0.0
|
|
#endif
|
|
#ifndef NAV_PITCH_ALT_D
|
|
# define NAV_PITCH_ALT_D 0.0
|
|
#endif
|
|
#ifndef NAV_PITCH_ALT_INT_MAX
|
|
# define NAV_PITCH_ALT_INT_MAX 5
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Energy/Altitude control gains
|
|
//
|
|
#ifndef THROTTLE_TE_P
|
|
# define THROTTLE_TE_P 0.50
|
|
#endif
|
|
#ifndef THROTTLE_TE_I
|
|
# define THROTTLE_TE_I 0.0
|
|
#endif
|
|
#ifndef THROTTLE_TE_D
|
|
# define THROTTLE_TE_D 0.0
|
|
#endif
|
|
#ifndef THROTTLE_TE_INT_MAX
|
|
# define THROTTLE_TE_INT_MAX 20
|
|
#endif
|
|
#ifndef THROTTLE_SLEW_LIMIT
|
|
# define THROTTLE_SLEW_LIMIT 0
|
|
#endif
|
|
#ifndef P_TO_T
|
|
# define P_TO_T 2.5
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Crosstrack compensation
|
|
//
|
|
#ifndef XTRACK_GAIN
|
|
# define XTRACK_GAIN 10 // deg/m * 100
|
|
#endif
|
|
#ifndef XTRACK_ENTRY_ANGLE
|
|
# define XTRACK_ENTRY_ANGLE 3000 // deg * 100
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// DEBUGGING
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// DEBUG_SUBSYSTEM
|
|
//
|
|
#ifndef DEBUG_SUBSYSTEM
|
|
# define DEBUG_SUBSYSTEM 0
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// DEBUG_LEVEL
|
|
//
|
|
#ifndef DEBUG_LEVEL
|
|
# define DEBUG_LEVEL SEVERITY_LOW
|
|
#endif
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Dataflash logging control
|
|
//
|
|
#ifndef LOG_ATTITUDE_FAST
|
|
# define LOG_ATTITUDE_FAST DISABLED
|
|
#endif
|
|
#ifndef LOG_ATTITUDE_MED
|
|
# define LOG_ATTITUDE_MED ENABLED
|
|
#endif
|
|
#ifndef LOG_GPS
|
|
# define LOG_GPS ENABLED
|
|
#endif
|
|
#ifndef LOG_PM
|
|
# define LOG_PM ENABLED
|
|
#endif
|
|
#ifndef LOG_CTUN
|
|
# define LOG_CTUN DISABLED
|
|
#endif
|
|
#ifndef LOG_NTUN
|
|
# define LOG_NTUN DISABLED
|
|
#endif
|
|
#ifndef LOG_MODE
|
|
# define LOG_MODE ENABLED
|
|
#endif
|
|
#ifndef LOG_RAW
|
|
# define LOG_RAW DISABLED
|
|
#endif
|
|
#ifndef LOG_CMD
|
|
# define LOG_CMD ENABLED
|
|
#endif
|
|
|
|
#ifndef DEBUG_PORT
|
|
# define DEBUG_PORT 0
|
|
#endif
|
|
|
|
#if DEBUG_PORT == 0
|
|
# define SendDebug_P(a) Serial.print_P(PSTR(a))
|
|
# define SendDebugln_P(a) Serial.println_P(PSTR(a))
|
|
# define SendDebug Serial.print
|
|
# define SendDebugln Serial.println
|
|
#elif DEBUG_PORT == 1
|
|
# define SendDebug_P(a) Serial1.print_P(PSTR(a))
|
|
# define SendDebugln_P(a) Serial1.println_P(PSTR(a))
|
|
# define SendDebug Serial1.print
|
|
# define SendDebugln Serial1.println
|
|
#elif DEBUG_PORT == 2
|
|
# define SendDebug_P(a) Serial2.print_P(PSTR(a))
|
|
# define SendDebugln_P(a) Serial2.println_P(PSTR(a))
|
|
# define SendDebug Serial2.print
|
|
# define SendDebugln Serial2.println
|
|
#elif DEBUG_PORT == 3
|
|
# define SendDebug_P(a) Serial3.print_P(PSTR(a))
|
|
# define SendDebugln_P(a) Serial3.println_P(PSTR(a))
|
|
# define SendDebug Serial3.print
|
|
# define SendDebugln Serial3.println
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Navigation defaults
|
|
//
|
|
#ifndef WP_RADIUS_DEFAULT
|
|
# define WP_RADIUS_DEFAULT 20
|
|
#endif
|
|
#ifndef LOITER_RADIUS_DEFAULT
|
|
# define LOITER_RADIUS_DEFAULT 60
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////
|
|
// Developer Items
|
|
//
|
|
|
|
#ifndef STANDARD_SPEED
|
|
# define STANDARD_SPEED 15.0
|
|
#endif
|
|
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
|
|
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
|
|
#define THROTTLE2SPEED_SCALER
|