Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/ArduSub/Parameters.h
Views: 1798
#pragma once12#define AP_PARAM_VEHICLE_NAME sub34#include <AP_Common/AP_Common.h>56#include <AP_Arming/AP_Arming.h>78// Global parameter class.9//10class Parameters {11public:12// The version of the layout as described by the parameter enum.13//14// When changing the parameter enum in an incompatible fashion, this15// value should be incremented by one.16//17// The increment will prevent old parameters from being used incorrectly18// by newer code.19//20static const uint16_t k_format_version = 1;2122// Parameter identities.23//24// The enumeration defined here is used to ensure that every parameter25// or parameter group has a unique ID number. This number is used by26// AP_Param to store and locate parameters in EEPROM.27//28// Note that entries without a number are assigned the next number after29// the entry preceding them. When adding new entries, ensure that they30// don't overlap.31//32// Try to group related variables together, and assign them a set33// range in the enumeration. Place these groups in numerical order34// at the end of the enumeration.35//36// WARNING: Care should be taken when editing this enumeration as the37// AP_Param load/save code depends on the values here to identify38// variables saved in EEPROM.39//40//41enum {42// Layout version number, always key zero.43//44k_param_format_version = 0,45k_param_software_type, // unused4647k_param_g2, // 2nd block of parameters4849k_param_sitl, // Simulation5051// Telemetry52k_param_gcs0 = 10,53k_param_gcs1,54k_param_gcs2,55k_param_gcs3,56k_param_sysid_this_mav,57k_param_sysid_my_gcs,5859// Hardware/Software configuration60k_param_BoardConfig = 20, // Board configuration (Pixhawk/Linux/etc)61k_param_scheduler, // Scheduler (for debugging/perf_info)62k_param_logger, // AP_Logger Logging63k_param_serial_manager_old, // Serial ports, AP_SerialManager64k_param_notify, // Notify Library, AP_Notify65k_param_arming = 26, // Arming checks66k_param_can_mgr,6768// Sensor objects69k_param_ins = 30, // AP_InertialSensor70k_param_compass, // Compass71k_param_barometer, // Barometer/Depth Sensor72k_param_battery, // AP_BattMonitor73k_param_leak_detector, // Leak Detector74k_param_rangefinder, // Rangefinder75k_param_gps, // GPS76k_param_optflow, // Optical Flow777879// Navigation libraries80k_param_ahrs = 50, // AHRS81k_param_NavEKF, // Extended Kalman Filter Inertial Navigation // remove82k_param_NavEKF2, // EKF283k_param_attitude_control, // Attitude Control84k_param_pos_control, // Position Control85k_param_wp_nav, // Waypoint navigation86k_param_mission, // Mission library87k_param_fence_old, // only used for conversion88k_param_terrain, // Terrain database89k_param_rally, // Disabled90k_param_circle_nav, // Disabled91k_param_avoid, // Relies on proximity and fence92k_param_NavEKF3,93k_param_loiter_nav,949596// Other external hardware interfaces97k_param_motors = 65, // Motors98k_param_relay, // Relay99k_param_camera, // Camera100k_param_camera_mount, // Camera gimbal101102103// RC_Channel settings (deprecated)104k_param_rc_1_old = 75,105k_param_rc_2_old,106k_param_rc_3_old,107k_param_rc_4_old,108k_param_rc_5_old,109k_param_rc_6_old,110k_param_rc_7_old,111k_param_rc_8_old,112k_param_rc_9_old,113k_param_rc_10_old,114k_param_rc_11_old,115k_param_rc_12_old,116k_param_rc_13_old,117k_param_rc_14_old,118119// Joystick gain parameters120k_param_gain_default,121k_param_maxGain,122k_param_minGain,123k_param_numGainSettings,124k_param_cam_tilt_step, // deprecated125k_param_lights_step, // deprecated126127// Joystick button mapping parameters128k_param_jbtn_0 = 95,129k_param_jbtn_1,130k_param_jbtn_2,131k_param_jbtn_3,132k_param_jbtn_4,133k_param_jbtn_5,134k_param_jbtn_6,135k_param_jbtn_7,136k_param_jbtn_8,137k_param_jbtn_9,138k_param_jbtn_10,139k_param_jbtn_11,140k_param_jbtn_12,141k_param_jbtn_13,142k_param_jbtn_14,143k_param_jbtn_15,144145// 16 more for MANUAL_CONTROL extensions146k_param_jbtn_16,147k_param_jbtn_17,148k_param_jbtn_18,149k_param_jbtn_19,150k_param_jbtn_20,151k_param_jbtn_21,152k_param_jbtn_22,153k_param_jbtn_23,154k_param_jbtn_24,155k_param_jbtn_25,156k_param_jbtn_26,157k_param_jbtn_27,158k_param_jbtn_28,159k_param_jbtn_29,160k_param_jbtn_30,161k_param_jbtn_31,162163// PID Controllers164k_param_p_pos_xy = 126, // deprecated165k_param_p_alt_hold, // deprecated166k_param_pi_vel_xy, // deprecated167k_param_p_vel_z, // deprecated168k_param_pid_accel_z, // deprecated169170171// Failsafes172k_param_failsafe_gcs = 140,173k_param_failsafe_leak, // leak failsafe behavior174k_param_failsafe_pressure, // internal pressure failsafe behavior175k_param_failsafe_pressure_max, // maximum internal pressure in pascal before failsafe is triggered176k_param_failsafe_temperature, // internal temperature failsafe behavior177k_param_failsafe_temperature_max, // maximum internal temperature in degrees C before failsafe is triggered178k_param_failsafe_terrain, // terrain failsafe behavior179k_param_fs_ekf_thresh,180k_param_fs_ekf_action,181k_param_fs_crash_check,182k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor183k_param_fs_batt_mah, // unused - moved to AP_BattMonitor184k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor185k_param_failsafe_pilot_input,186k_param_failsafe_pilot_input_timeout,187k_param_failsafe_gcs_timeout,188189190// Misc Sub settings191k_param_log_bitmask = 165,192k_param_angle_max = 167,193k_param_rangefinder_gain, // deprecated194k_param_wp_yaw_behavior = 170,195k_param_xtrack_angle_limit, // Angle limit for crosstrack correction in Auto modes (degrees)196k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max197k_param_pilot_accel_z,198k_param_compass_enabled_deprecated,199k_param_surface_depth,200k_param_rc_speed, // Main output pwm frequency201k_param_gcs_pid_mask = 178,202k_param_throttle_filt,203k_param_throttle_deadzone, // Used in auto-throttle modes204k_param_terrain_follow = 182, // deprecated205k_param_rc_feel_rp,206k_param_throttle_gain,207k_param_cam_tilt_center, // deprecated208k_param_frame_configuration,209210// Acro Mode parameters211k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate212k_param_acro_trainer,213k_param_acro_expo,214k_param_acro_rp_p,215k_param_acro_balance_roll,216k_param_acro_balance_pitch,217218// RPM Sensor219k_param_rpm_sensor = 232, // Disabled220221// RC_Mapper Library222k_param_rcmap, // Disabled223224k_param_gcs4,225k_param_gcs5,226k_param_gcs6,227228k_param_cam_slew_limit = 237, // deprecated229k_param_lights_steps,230k_param_pilot_speed_dn,231k_param_rangefinder_signal_min,232k_param_surftrak_depth,233234k_param_vehicle = 257, // vehicle common block of parameters235};236237AP_Int16 format_version;238239// Telemetry control240//241AP_Int16 sysid_this_mav;242AP_Int16 sysid_my_gcs;243244AP_Float throttle_filt;245246#if AP_RANGEFINDER_ENABLED247AP_Int8 rangefinder_signal_min; // minimum signal quality for good rangefinder readings248AP_Float surftrak_depth; // surftrak will try to keep sub below this depth249#endif250251AP_Int8 failsafe_leak; // leak detection failsafe behavior252AP_Int8 failsafe_gcs; // ground station failsafe behavior253AP_Int8 failsafe_pressure;254AP_Int8 failsafe_temperature;255AP_Int32 failsafe_pressure_max;256AP_Int8 failsafe_temperature_max;257AP_Int8 failsafe_terrain;258AP_Int8 failsafe_pilot_input; // pilot input failsafe behavior259AP_Float failsafe_pilot_input_timeout;260AP_Float failsafe_gcs_timeout; // ground station failsafe timeout (seconds)261262AP_Int8 xtrack_angle_limit;263264AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions265AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp266267// Waypoints268//269AP_Int16 pilot_speed_up; // maximum vertical ascending velocity the pilot may request270AP_Int16 pilot_speed_dn; // maximum vertical descending velocity the pilot may request271AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request272273// Throttle274//275AP_Int16 throttle_deadzone;276277// Misc278//279AP_Int32 log_bitmask;280281AP_Int8 fs_ekf_action;282AP_Int8 fs_crash_check;283AP_Float fs_ekf_thresh;284AP_Int16 gcs_pid_mask;285286AP_Int16 rc_speed; // speed of fast RC Channels in Hz287288AP_Float gain_default;289AP_Float maxGain;290AP_Float minGain;291AP_Int8 numGainSettings;292AP_Float throttle_gain;293294AP_Int16 lights_steps;295296// Joystick button parameters297JSButton jbtn_0;298JSButton jbtn_1;299JSButton jbtn_2;300JSButton jbtn_3;301JSButton jbtn_4;302JSButton jbtn_5;303JSButton jbtn_6;304JSButton jbtn_7;305JSButton jbtn_8;306JSButton jbtn_9;307JSButton jbtn_10;308JSButton jbtn_11;309JSButton jbtn_12;310JSButton jbtn_13;311JSButton jbtn_14;312JSButton jbtn_15;313// 16 - 31 from manual_control extension314JSButton jbtn_16;315JSButton jbtn_17;316JSButton jbtn_18;317JSButton jbtn_19;318JSButton jbtn_20;319JSButton jbtn_21;320JSButton jbtn_22;321JSButton jbtn_23;322JSButton jbtn_24;323JSButton jbtn_25;324JSButton jbtn_26;325JSButton jbtn_27;326JSButton jbtn_28;327JSButton jbtn_29;328JSButton jbtn_30;329JSButton jbtn_31;330331// Acro parameters332AP_Float acro_rp_p;333AP_Float acro_yaw_p;334AP_Float acro_balance_roll;335AP_Float acro_balance_pitch;336AP_Int8 acro_trainer;337AP_Float acro_expo;338339AP_Float surface_depth;340AP_Int8 frame_configuration;341342// Note: keep initializers here in the same order as they are declared343// above.344Parameters()345{346}347};348349/*3502nd block of parameters, to avoid going past 256 top level keys351*/352class ParametersG2 {353public:354ParametersG2(void);355356// var_info for holding Parameter information357static const struct AP_Param::GroupInfo var_info[];358359#if HAL_PROXIMITY_ENABLED360// proximity (aka object avoidance) library361AP_Proximity proximity;362#endif363364// RC input channels365RC_Channels_Sub rc_channels;366367// control over servo output ranges368SRV_Channels servo_channels;369370AP_Float backup_origin_lat;371AP_Float backup_origin_lon;372AP_Float backup_origin_alt;373};374375extern const AP_Param::Info var_info[];376377// Sub-specific default parameters378static const struct AP_Param::defaults_table_struct defaults_table[] = {379{ "BRD_SAFETY_DEFLT", 0 },380{ "ARMING_CHECK", AP_Arming::ARMING_CHECK_RC |381AP_Arming::ARMING_CHECK_VOLTAGE |382AP_Arming::ARMING_CHECK_BATTERY},383{ "CIRCLE_RATE", 2.0f},384{ "ATC_ACCEL_Y_MAX", 110000.0f},385{ "ATC_RATE_Y_MAX", 180.0f},386{ "RC3_TRIM", 1100},387{ "COMPASS_OFFS_MAX", 1000},388{ "INS_GYR_CAL", 0},389#if HAL_MOUNT_ENABLED390{ "MNT1_TYPE", 1},391{ "MNT1_DEFLT_MODE", MAV_MOUNT_MODE_RC_TARGETING},392{ "MNT1_RC_RATE", 30},393#endif394{ "RC7_OPTION", 214}, // MOUNT1_YAW395{ "RC8_OPTION", 213}, // MOUNT1_PITCH396{ "MOT_PWM_MIN", 1100},397{ "MOT_PWM_MAX", 1900},398{ "PSC_JERK_Z", 50.0f},399{ "WPNAV_SPEED", 100.0f},400{ "PILOT_SPEED_UP", 100.0f},401{ "PSC_VELXY_P", 6.0f},402{ "EK3_SRC1_VELZ", 0},403#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR404#if AP_BARO_PROBE_EXT_PARAMETER_ENABLED405{ "BARO_PROBE_EXT", 0},406#endif407{ "BATT_MONITOR", 4},408{ "BATT_CAPACITY", 0},409{ "LEAK1_PIN", 27},410{ "SCHED_LOOP_RATE", 200},411{ "SERVO13_FUNCTION", 59}, // k_rcin9, lights 1412{ "SERVO14_FUNCTION", 60}, // k_rcin10, lights 2413{ "SERVO16_FUNCTION", 7}, // k_mount_tilt414{ "SERVO16_REVERSED", 1},415#else416#if AP_BARO_PROBE_EXT_PARAMETER_ENABLED417{ "BARO_PROBE_EXT", 768},418#endif419{ "SERVO9_FUNCTION", 59}, // k_rcin9, lights 1420{ "SERVO10_FUNCTION", 7}, // k_mount_tilt421#endif422};423424425