mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
82e51aec82
moved ground start to first arming added ground start flag moved throttle_integrator to 50hz loop CAMERA_STABILIZER deprecated - now always on renamed current logging bit mask to match APM added MA filter to PID - D term Adjusted PIDs based on continued testing and new PID filter added MASK_LOG_SET_DEFAULTS to match APM moved some stuff out of ground start into system start where it belonged Added slower Yaw gains for DCM when the copter is in the air changed camera output to be none scaled PWM fixed bug where ground_temperature was unfiltered shortened Baro startup time fixed issue with Nav_WP integrator not being reset RTL no longer yaws towards home Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
379 lines
13 KiB
C++
379 lines
13 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#ifndef PARAMETERS_H
|
|
#define PARAMETERS_H
|
|
|
|
#include <AP_Common.h>
|
|
|
|
// Global parameter class.
|
|
//
|
|
class Parameters {
|
|
public:
|
|
// The version of the layout as described by the parameter enum.
|
|
//
|
|
// When changing the parameter enum in an incompatible fashion, this
|
|
// value should be incremented by one.
|
|
//
|
|
// The increment will prevent old parameters from being used incorrectly
|
|
// by newer code.
|
|
//
|
|
static const uint16_t k_format_version = 105;
|
|
|
|
// The parameter software_type is set up solely for ground station use
|
|
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
|
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
|
|
// values within that range to identify different branches.
|
|
//
|
|
static const uint16_t k_software_type = 10; // 0 for APM trunk
|
|
|
|
//
|
|
// Parameter identities.
|
|
//
|
|
// The enumeration defined here is used to ensure that every parameter
|
|
// or parameter group has a unique ID number. This number is used by
|
|
// AP_Var to store and locate parameters in EEPROM.
|
|
//
|
|
// Note that entries without a number are assigned the next number after
|
|
// the entry preceding them. When adding new entries, ensure that they
|
|
// don't overlap.
|
|
//
|
|
// Try to group related variables together, and assign them a set
|
|
// range in the enumeration. Place these groups in numerical order
|
|
// at the end of the enumeration.
|
|
//
|
|
// WARNING: Care should be taken when editing this enumeration as the
|
|
// AP_Var load/save code depends on the values here to identify
|
|
// variables saved in EEPROM.
|
|
//
|
|
//
|
|
enum {
|
|
// Layout version number, always key zero.
|
|
//
|
|
k_param_format_version = 0,
|
|
k_param_software_type,
|
|
|
|
|
|
// Misc
|
|
//
|
|
k_param_log_bitmask,
|
|
|
|
// 110: Telemetry control
|
|
//
|
|
k_param_streamrates_port0 = 110,
|
|
k_param_streamrates_port3,
|
|
k_param_sysid_this_mav,
|
|
k_param_sysid_my_gcs,
|
|
|
|
//
|
|
// 140: Sensor parameters
|
|
//
|
|
k_param_IMU_calibration = 140,
|
|
k_param_battery_monitoring,
|
|
k_param_pack_capacity,
|
|
k_param_compass_enabled,
|
|
k_param_compass,
|
|
k_param_sonar,
|
|
k_param_frame_orientation,
|
|
k_param_top_bottom_ratio,
|
|
k_param_optflow_enabled,
|
|
|
|
//
|
|
// 160: Navigation parameters
|
|
//
|
|
k_param_crosstrack_entry_angle = 160,
|
|
k_param_pitch_max,
|
|
k_param_RTL_altitude,
|
|
|
|
//
|
|
// 180: Radio settings
|
|
//
|
|
k_param_rc_1 = 180,
|
|
k_param_rc_2,
|
|
k_param_rc_3,
|
|
k_param_rc_4,
|
|
k_param_rc_5,
|
|
k_param_rc_6,
|
|
k_param_rc_7,
|
|
k_param_rc_8,
|
|
k_param_rc_9,
|
|
k_param_rc_10,
|
|
k_param_throttle_min,
|
|
k_param_throttle_max,
|
|
k_param_throttle_fs_enabled,
|
|
k_param_throttle_fs_action,
|
|
k_param_throttle_fs_value,
|
|
k_param_throttle_cruise,
|
|
k_param_esc_calibrate,
|
|
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
//
|
|
// 200: Heli
|
|
//
|
|
k_param_heli_servo_1 = 200,
|
|
k_param_heli_servo_2,
|
|
k_param_heli_servo_3,
|
|
k_param_heli_servo_4,
|
|
k_param_heli_servo1_pos ,
|
|
k_param_heli_servo2_pos,
|
|
k_param_heli_servo3_pos,
|
|
k_param_heli_roll_max,
|
|
k_param_heli_pitch_max,
|
|
k_param_heli_collective_min,
|
|
k_param_heli_collective_max,
|
|
k_param_heli_collective_mid,
|
|
k_param_heli_ext_gyro_enabled,
|
|
k_param_heli_ext_gyro_gain, // 213
|
|
#endif
|
|
|
|
|
|
//
|
|
// 210: flight modes
|
|
//
|
|
k_param_flight_mode1,
|
|
k_param_flight_mode2,
|
|
k_param_flight_mode3,
|
|
k_param_flight_mode4,
|
|
k_param_flight_mode5,
|
|
k_param_flight_mode6,
|
|
|
|
|
|
//
|
|
// 220: Waypoint data
|
|
//
|
|
k_param_waypoint_mode = 220,
|
|
k_param_waypoint_total,
|
|
k_param_waypoint_index,
|
|
k_param_command_must_index,
|
|
k_param_waypoint_radius,
|
|
k_param_loiter_radius,
|
|
k_param_waypoint_speed_max,
|
|
|
|
//
|
|
// 240: PID Controllers
|
|
//
|
|
// Heading-to-roll PID:
|
|
// heading error from commnd to roll command deviation from trim
|
|
// (bank to turn strategy)
|
|
//
|
|
k_param_pid_rate_roll = 240,
|
|
k_param_pid_rate_pitch,
|
|
k_param_pid_rate_yaw,
|
|
k_param_pid_stabilize_roll,
|
|
k_param_pid_stabilize_pitch,
|
|
k_param_pid_stabilize_yaw,
|
|
k_param_pid_nav_lat,
|
|
k_param_pid_nav_lon,
|
|
k_param_pid_nav_wp,
|
|
k_param_pid_throttle,
|
|
k_param_pid_crosstrack,
|
|
|
|
|
|
// 255: reserved
|
|
};
|
|
|
|
AP_Int16 format_version;
|
|
AP_Int8 software_type;
|
|
|
|
// Telemetry control
|
|
//
|
|
AP_Int16 sysid_this_mav;
|
|
AP_Int16 sysid_my_gcs;
|
|
|
|
|
|
// Crosstrack navigation
|
|
//
|
|
AP_Int16 crosstrack_entry_angle;
|
|
|
|
// Waypoints
|
|
//
|
|
AP_Int8 waypoint_mode;
|
|
AP_Int8 waypoint_total;
|
|
AP_Int8 waypoint_index;
|
|
AP_Int8 command_must_index;
|
|
AP_Int8 waypoint_radius;
|
|
AP_Int8 loiter_radius;
|
|
AP_Int16 waypoint_speed_max;
|
|
|
|
// Throttle
|
|
//
|
|
AP_Int16 throttle_min;
|
|
AP_Int16 throttle_max;
|
|
AP_Int8 throttle_fs_enabled;
|
|
AP_Int8 throttle_fs_action;
|
|
AP_Int16 throttle_fs_value;
|
|
AP_Int16 throttle_cruise;
|
|
|
|
// Flight modes
|
|
//
|
|
AP_Int8 flight_mode1;
|
|
AP_Int8 flight_mode2;
|
|
AP_Int8 flight_mode3;
|
|
AP_Int8 flight_mode4;
|
|
AP_Int8 flight_mode5;
|
|
AP_Int8 flight_mode6;
|
|
|
|
// Radio settings
|
|
//
|
|
//AP_Var_group pwm_roll;
|
|
//AP_Var_group pwm_pitch;
|
|
//AP_Var_group pwm_throttle;
|
|
//AP_Var_group pwm_yaw;
|
|
|
|
AP_Int16 pitch_max;
|
|
|
|
// Misc
|
|
//
|
|
AP_Int16 log_bitmask;
|
|
AP_Int16 RTL_altitude;
|
|
|
|
AP_Int8 sonar_enabled;
|
|
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
|
|
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
|
AP_Int8 compass_enabled;
|
|
AP_Int8 esc_calibrate;
|
|
AP_Int8 frame_orientation;
|
|
AP_Float top_bottom_ratio;
|
|
AP_Int8 optflow_enabled;
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
// Heli
|
|
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
|
|
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
|
|
AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate
|
|
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch
|
|
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
|
|
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
|
|
#endif
|
|
|
|
// RC channels
|
|
RC_Channel rc_1;
|
|
RC_Channel rc_2;
|
|
RC_Channel rc_3;
|
|
RC_Channel rc_4;
|
|
RC_Channel rc_5;
|
|
RC_Channel rc_6;
|
|
RC_Channel rc_7;
|
|
RC_Channel rc_8;
|
|
RC_Channel rc_camera_pitch;
|
|
RC_Channel rc_camera_roll;
|
|
|
|
// PID controllers
|
|
PID pid_rate_roll;
|
|
PID pid_rate_pitch;
|
|
PID pid_rate_yaw;
|
|
PID pid_stabilize_roll;
|
|
PID pid_stabilize_pitch;
|
|
PID pid_stabilize_yaw;
|
|
PID pid_nav_lat;
|
|
PID pid_nav_lon;
|
|
PID pid_nav_wp;
|
|
PID pid_throttle;
|
|
PID pid_crosstrack;
|
|
|
|
uint8_t junk;
|
|
|
|
// Note: keep initializers here in the same order as they are declared above.
|
|
Parameters() :
|
|
// variable default key name
|
|
//-------------------------------------------------------------------------------------------------------------------
|
|
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
|
|
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
|
|
|
|
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
|
|
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
|
|
|
|
//crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
|
|
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
|
|
|
|
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
|
|
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
|
|
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
|
|
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
|
|
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
|
|
|
|
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
|
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
|
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
|
|
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
|
|
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
|
|
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
|
|
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
|
|
|
|
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
|
|
throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")),
|
|
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
|
|
throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")),
|
|
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
|
|
throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
|
|
|
|
flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")),
|
|
flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")),
|
|
flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")),
|
|
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
|
|
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
|
|
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
|
|
|
|
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
|
|
|
|
log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
|
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
|
|
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
|
|
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
|
|
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
|
|
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
|
|
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
|
|
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
|
|
heli_servo1_pos (-60, k_param_heli_servo1_pos, PSTR("SV1_POS_")),
|
|
heli_servo2_pos (60, k_param_heli_servo2_pos, PSTR("SV2_POS_")),
|
|
heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
|
|
heli_roll_max (4500, k_param_heli_roll_max, PSTR("ROL_MAX_")),
|
|
heli_pitch_max (4500, k_param_heli_pitch_max, PSTR("PIT_MAX_")),
|
|
heli_coll_min (1000, k_param_heli_collective_min, PSTR("COL_MIN_")),
|
|
heli_coll_max (2000, k_param_heli_collective_max, PSTR("COL_MAX_")),
|
|
heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")),
|
|
heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")),
|
|
heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
|
|
#endif
|
|
|
|
// RC channel group key name
|
|
//----------------------------------------------------------------------
|
|
rc_1 (k_param_rc_1, PSTR("RC1_")),
|
|
rc_2 (k_param_rc_2, PSTR("RC2_")),
|
|
rc_3 (k_param_rc_3, PSTR("RC3_")),
|
|
rc_4 (k_param_rc_4, PSTR("RC4_")),
|
|
rc_5 (k_param_rc_5, PSTR("RC5_")),
|
|
rc_6 (k_param_rc_6, PSTR("RC6_")),
|
|
rc_7 (k_param_rc_7, PSTR("RC7_")),
|
|
rc_8 (k_param_rc_8, PSTR("RC8_")),
|
|
rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")),
|
|
rc_camera_roll (k_param_rc_10, PSTR("RC_CR_")),
|
|
|
|
// PID controller group key name initial P initial I initial D initial imax
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, 0, RATE_ROLL_IMAX * 100),
|
|
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, 0, RATE_PITCH_IMAX * 100),
|
|
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, 0, RATE_YAW_IMAX * 100),
|
|
|
|
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100),
|
|
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100),
|
|
pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, STABILIZE_YAW_IMAX * 100),
|
|
|
|
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
|
|
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
|
|
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
|
|
|
|
pid_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
|
|
pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, XTRACK_IMAX),
|
|
|
|
|
|
junk(0) // XXX just so that we can add things without worrying about the trailing comma
|
|
{
|
|
}
|
|
};
|
|
|
|
#endif // PARAMETERS_H
|