#include "Tracker.h"12/*3* AntennaTracker parameter definitions4*5*/67const AP_Param::Info Tracker::var_info[] = {8// @Param: FORMAT_VERSION9// @DisplayName: Eeprom format version number10// @Description: This value is incremented when changes are made to the eeprom format11// @User: Advanced12GSCALAR(format_version, "FORMAT_VERSION", 0),1314// SYSID_THISMAV was here1516// SYSID_MYGCS was here1718// @Param: SYSID_TARGET19// @DisplayName: Target vehicle's MAVLink system ID20// @Description: The identifier of the vehicle being tracked. This should be zero (to auto detect) or be the same as the MAV_SYSID parameter of the vehicle being tracked.21// @Range: 1 25522// @User: Advanced23GSCALAR(sysid_target, "SYSID_TARGET", 0),2425// @Param: YAW_SLEW_TIME26// @DisplayName: Time for yaw to slew through its full range27// @Description: This controls how rapidly the tracker will change the servo output for yaw. It is set as the number of seconds to do a full rotation. You can use this parameter to slow the trackers movements, which may help with some types of trackers. A value of zero will allow for unlimited servo movement per update.28// @Units: s29// @Increment: 0.130// @Range: 0 2031// @User: Standard32GSCALAR(yaw_slew_time, "YAW_SLEW_TIME", 2),3334// @Param: PITCH_SLEW_TIME35// @DisplayName: Time for pitch to slew through its full range36// @Description: This controls how rapidly the tracker will change the servo output for pitch. It is set as the number of seconds to do a full range of pitch movement. You can use this parameter to slow the trackers movements, which may help with some types of trackers. A value of zero will allow for unlimited servo movement per update.37// @Units: s38// @Increment: 0.139// @Range: 0 2040// @User: Standard41GSCALAR(pitch_slew_time, "PITCH_SLEW_TIME", 2),4243// @Param: MIN_REVERSE_TIME44// @DisplayName: Minimum time to apply a yaw reversal45// @Description: When the tracker detects it has reached the limit of servo movement in yaw it will reverse and try moving to the other extreme of yaw. This parameter controls the minimum time it should reverse for. It is used to cope with trackers that have a significant lag in movement to ensure they do move all the way around.46// @Units: s47// @Increment: 148// @Range: 0 2049// @User: Standard50GSCALAR(min_reverse_time, "MIN_REVERSE_TIME", 1),5152// @Param: START_LATITUDE53// @DisplayName: Initial Latitude before GPS lock54// @Description: Combined with START_LONGITUDE this parameter allows for an initial position of the tracker to be set. This position will be used until the GPS gets lock. It can also be used to run a stationary tracker with no GPS attached.55// @Units: deg56// @Increment: 0.00000157// @Range: -90 9058// @User: Standard59GSCALAR(start_latitude, "START_LATITUDE", 0),6061// @Param: START_LONGITUDE62// @DisplayName: Initial Longitude before GPS lock63// @Description: Combined with START_LATITUDE this parameter allows for an initial position of the tracker to be set. This position will be used until the GPS gets lock. It can also be used to run a stationary tracker with no GPS attached.64// @Units: deg65// @Increment: 0.00000166// @Range: -180 18067// @User: Standard68GSCALAR(start_longitude, "START_LONGITUDE", 0),6970// @Param: STARTUP_DELAY71// @DisplayName: Delay before first servo movement from trim72// @Description: This parameter can be used to force the servos to their trim value for a time on startup. This can help with some servo types73// @Units: s74// @Increment: 0.175// @Range: 0 1076// @User: Standard77GSCALAR(startup_delay, "STARTUP_DELAY", 0),7879// @Param: SERVO_PITCH_TYPE80// @DisplayName: Type of servo system being used for pitch81// @Description: This allows selection of position servos or on/off servos for pitch82// @Values: 0:Position,1:OnOff,2:ContinuousRotation83// @User: Standard84GSCALAR(servo_pitch_type, "SERVO_PITCH_TYPE", SERVO_TYPE_POSITION),8586// @Param: SERVO_YAW_TYPE87// @DisplayName: Type of servo system being used for yaw88// @Description: This allows selection of position servos or on/off servos for yaw89// @Values: 0:Position,1:OnOff,2:ContinuousRotation90// @User: Standard91GSCALAR(servo_yaw_type, "SERVO_YAW_TYPE", SERVO_TYPE_POSITION),9293// @Param: ONOFF_YAW_RATE94// @DisplayName: Yaw rate for on/off servos95// @Description: Rate of change of yaw in degrees/second for on/off servos96// @Units: deg/s97// @Increment: 0.198// @Range: 0 5099// @User: Standard100GSCALAR(onoff_yaw_rate, "ONOFF_YAW_RATE", 9.0f),101102// @Param: ONOFF_PITCH_RATE103// @DisplayName: Pitch rate for on/off servos104// @Description: Rate of change of pitch in degrees/second for on/off servos105// @Units: deg/s106// @Increment: 0.1107// @Range: 0 50108// @User: Standard109GSCALAR(onoff_pitch_rate, "ONOFF_PITCH_RATE", 1.0f),110111// @Param: ONOFF_YAW_MINT112// @DisplayName: Yaw minimum movement time113// @Description: Minimum amount of time in seconds to move in yaw114// @Units: s115// @Increment: 0.01116// @Range: 0 2117// @User: Standard118GSCALAR(onoff_yaw_mintime, "ONOFF_YAW_MINT", 0.1f),119120// @Param: ONOFF_PITCH_MINT121// @DisplayName: Pitch minimum movement time122// @Description: Minimum amount of time in seconds to move in pitch123// @Units: s124// @Increment: 0.01125// @Range: 0 2126// @User: Standard127GSCALAR(onoff_pitch_mintime, "ONOFF_PITCH_MINT", 0.1f),128129// @Param: YAW_TRIM130// @DisplayName: Yaw trim131// @Description: Amount of extra yaw to add when tracking. This allows for small adjustments for an out of trim compass.132// @Units: deg133// @Increment: 0.1134// @Range: -10 10135// @User: Standard136GSCALAR(yaw_trim, "YAW_TRIM", 0),137138// @Param: PITCH_TRIM139// @DisplayName: Pitch trim140// @Description: Amount of extra pitch to add when tracking. This allows for small adjustments for a badly calibrated barometer.141// @Units: deg142// @Increment: 0.1143// @Range: -10 10144// @User: Standard145GSCALAR(pitch_trim, "PITCH_TRIM", 0),146147// @Param: YAW_RANGE148// @DisplayName: Yaw Angle Range149// @Description: Yaw axis total range of motion in degrees150// @Units: deg151// @Increment: 0.1152// @Range: 0 360153// @User: Standard154GSCALAR(yaw_range, "YAW_RANGE", YAW_RANGE_DEFAULT),155156// @Param: DISTANCE_MIN157// @DisplayName: Distance minimum to target158// @Description: Tracker will track targets at least this distance away159// @Units: m160// @Increment: 1161// @Range: 0 100162// @User: Standard163GSCALAR(distance_min, "DISTANCE_MIN", DISTANCE_MIN_DEFAULT),164165// @Param: ALT_SOURCE166// @DisplayName: Altitude Source167// @Description: What provides altitude information for vehicle. Vehicle only assumes tracker has same altitude as vehicle's home168// @Values: 0:Barometer,1:GPS,2:GPS vehicle only169// @User: Standard170GSCALAR(alt_source, "ALT_SOURCE", 0),171172// @Param: MAV_UPDATE_RATE173// @DisplayName: Mavlink Update Rate174// @Description: The rate at which Mavlink updates position and baro data175// @Units: Hz176// @Increment: 1177// @Range: 1 10178// @User: Standard179GSCALAR(mavlink_update_rate, "MAV_UPDATE_RATE", 1),180181// @Param: PITCH_MIN182// @DisplayName: Minimum Pitch Angle183// @Description: The lowest angle the pitch can reach184// @Units: deg185// @Increment: 1186// @Range: -90 0187// @User: Standard188GSCALAR(pitch_min, "PITCH_MIN", PITCH_MIN_DEFAULT),189190// @Param: PITCH_MAX191// @DisplayName: Maximum Pitch Angle192// @Description: The highest angle the pitch can reach193// @Units: deg194// @Increment: 1195// @Range: 0 90196// @User: Standard197GSCALAR(pitch_max, "PITCH_MAX", PITCH_MAX_DEFAULT),198199// barometer library200// @Group: BARO201// @Path: ../libraries/AP_Baro/AP_Baro.cpp202GOBJECT(barometer, "BARO", AP_Baro),203204// @Group: COMPASS_205// @Path: ../libraries/AP_Compass/AP_Compass.cpp206GOBJECT(compass, "COMPASS_", Compass),207208// @Group: SCHED_209// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp210GOBJECT(scheduler, "SCHED_", AP_Scheduler),211212// @Param: LOG_BITMASK213// @DisplayName: Log bitmask214// @Description: 4 byte bitmap of log types to enable215// @Bitmask: 0:ATTITUDE,1:GPS,2:RCIN,3:IMU,4:RCOUT,5:COMPASS,6:Battery216// @User: Standard217GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),218219// @Group: INS220// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp221GOBJECT(ins, "INS", AP_InertialSensor),222223// @Group: AHRS_224// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp225GOBJECT(ahrs, "AHRS_", AP_AHRS),226227#if AP_SIM_ENABLED228// @Group: SIM_229// @Path: ../libraries/SITL/SITL.cpp230GOBJECT(sitl, "SIM_", SITL::SIM),231#endif232233// @Group: BRD_234// @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp235GOBJECT(BoardConfig, "BRD_", AP_BoardConfig),236237#if HAL_MAX_CAN_PROTOCOL_DRIVERS238// @Group: CAN_239// @Path: ../libraries/AP_CANManager/AP_CANManager.cpp240GOBJECT(can_mgr, "CAN_", AP_CANManager),241#endif242243// GPS driver244// @Group: GPS245// @Path: ../libraries/AP_GPS/AP_GPS.cpp246GOBJECT(gps, "GPS", AP_GPS),247248// @Group: NTF_249// @Path: ../libraries/AP_Notify/AP_Notify.cpp250GOBJECT(notify, "NTF_", AP_Notify),251252// @Group: RC253// @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h254GOBJECT(rc_channels, "RC", RC_Channels_Tracker),255256// @Group: SERVO257// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp258GOBJECT(servo_channels, "SERVO", SRV_Channels),259260// AP_SerialManager was here261262// @Param: PITCH2SRV_P263// @DisplayName: Pitch axis controller P gain264// @Description: Pitch axis controller P gain. Converts the difference between desired pitch angle and actual pitch angle into a pitch servo pwm change265// @Range: 0.0 3.0266// @Increment: 0.01267// @User: Standard268269// @Param: PITCH2SRV_I270// @DisplayName: Pitch axis controller I gain271// @Description: Pitch axis controller I gain. Corrects long-term difference in desired pitch angle vs actual pitch angle272// @Range: 0.0 3.0273// @Increment: 0.01274// @User: Standard275276// @Param: PITCH2SRV_IMAX277// @DisplayName: Pitch axis controller I gain maximum278// @Description: Pitch axis controller I gain maximum. Constrains the maximum pwm change that the I gain will output279// @Range: 0 4000280// @Increment: 10281// @Units: d%282// @User: Standard283284// @Param: PITCH2SRV_D285// @DisplayName: Pitch axis controller D gain286// @Description: Pitch axis controller D gain. Compensates for short-term change in desired pitch angle vs actual pitch angle287// @Range: 0.001 0.1288// @Increment: 0.001289// @User: Standard290291// @Param: PITCH2SRV_FF292// @DisplayName: Pitch axis controller feed forward293// @Description: Pitch axis controller feed forward294// @Range: 0 0.5295// @Increment: 0.001296// @User: Standard297298// @Param: PITCH2SRV_FLTT299// @DisplayName: Pitch axis controller target frequency in Hz300// @Description: Pitch axis controller target frequency in Hz301// @Range: 1 50302// @Increment: 1303// @Units: Hz304// @User: Standard305306// @Param: PITCH2SRV_FLTE307// @DisplayName: Pitch axis controller error frequency in Hz308// @Description: Pitch axis controller error frequency in Hz309// @Range: 1 100310// @Increment: 1311// @Units: Hz312// @User: Standard313314// @Param: PITCH2SRV_FLTD315// @DisplayName: Pitch axis controller derivative frequency in Hz316// @Description: Pitch axis controller derivative frequency in Hz317// @Range: 1 100318// @Increment: 1319// @Units: Hz320// @User: Standard321322// @Param: PITCH2SRV_SMAX323// @DisplayName: Pitch slew rate limit324// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.325// @Range: 0 200326// @Increment: 0.5327// @User: Advanced328329// @Param: PITCH2SRV_PDMX330// @DisplayName: Pitch axis controller PD sum maximum331// @Description: Pitch axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output332// @Range: 0 4000333// @Increment: 10334// @Units: d%335// @User: Advanced336337// @Param: PITCH2SRV_D_FF338// @DisplayName: Pitch Derivative FeedForward Gain339// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target340// @Range: 0 0.1341// @Increment: 0.001342// @User: Advanced343344// @Param: PITCH2SRV_NTF345// @DisplayName: Pitch Target notch filter index346// @Description: Pitch Target notch filter index347// @Range: 1 8348// @User: Advanced349350// @Param: PITCH2SRV_NEF351// @DisplayName: Pitch Error notch filter index352// @Description: Pitch Error notch filter index353// @Range: 1 8354// @User: Advanced355356GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID),357358// @Param: YAW2SRV_P359// @DisplayName: Yaw axis controller P gain360// @Description: Yaw axis controller P gain. Converts the difference between desired yaw angle (heading) and actual yaw angle into a yaw servo pwm change361// @Range: 0.0 3.0362// @Increment: 0.01363// @User: Standard364365// @Param: YAW2SRV_I366// @DisplayName: Yaw axis controller I gain367// @Description: Yaw axis controller I gain. Corrects long-term difference in desired yaw angle (heading) vs actual yaw angle368// @Range: 0.0 3.0369// @Increment: 0.01370// @User: Standard371372// @Param: YAW2SRV_IMAX373// @DisplayName: Yaw axis controller I gain maximum374// @Description: Yaw axis controller I gain maximum. Constrains the maximum pwm change that the I gain will output375// @Range: 0 4000376// @Increment: 10377// @Units: d%378// @User: Standard379380// @Param: YAW2SRV_D381// @DisplayName: Yaw axis controller D gain382// @Description: Yaw axis controller D gain. Compensates for short-term change in desired yaw angle (heading) vs actual yaw angle383// @Range: 0.001 0.1384// @Increment: 0.001385// @User: Standard386387// @Param: YAW2SRV_FF388// @DisplayName: Yaw axis controller feed forward389// @Description: Yaw axis controller feed forward390// @Range: 0 0.5391// @Increment: 0.001392// @User: Standard393394// @Param: YAW2SRV_FLTT395// @DisplayName: Yaw axis controller target frequency in Hz396// @Description: Yaw axis controller target frequency in Hz397// @Range: 1 50398// @Increment: 1399// @Units: Hz400// @User: Standard401402// @Param: YAW2SRV_FLTE403// @DisplayName: Yaw axis controller error frequency in Hz404// @Description: Yaw axis controller error frequency in Hz405// @Range: 1 100406// @Increment: 1407// @Units: Hz408// @User: Standard409410// @Param: YAW2SRV_FLTD411// @DisplayName: Yaw axis controller derivative frequency in Hz412// @Description: Yaw axis controller derivative frequency in Hz413// @Range: 1 100414// @Increment: 1415// @Units: Hz416// @User: Standard417418// @Param: YAW2SRV_SMAX419// @DisplayName: Yaw slew rate limit420// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.421// @Range: 0 200422// @Increment: 0.5423// @User: Advanced424425// @Param: YAW2SRV_PDMX426// @DisplayName: Yaw axis controller PD sum maximum427// @Description: Yaw axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output428// @Range: 0 4000429// @Increment: 10430// @Units: d%431// @User: Advanced432433// @Param: YAW2SRV_D_FF434// @DisplayName: Yaw Derivative FeedForward Gain435// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target436// @Range: 0 0.1437// @Increment: 0.001438// @User: Advanced439440// @Param: YAW2SRV_NTF441// @DisplayName: Yaw Target notch filter index442// @Description: Yaw Target notch filter index443// @Range: 1 8444// @User: Advanced445446// @Param: YAW2SRV_NEF447// @DisplayName: Yaw Error notch filter index448// @Description: Yaw Error notch filter index449// @Range: 1 8450// @User: Advanced451452GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID),453454// @Param: CMD_TOTAL455// @DisplayName: Number of loaded mission items456// @Description: Set to 1 if HOME location has been loaded by the ground station. Do not change this manually.457// @Range: 1 255458// @User: Advanced459GSCALAR(command_total, "CMD_TOTAL", 0),460461// @Group: BATT462// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp463GOBJECT(battery, "BATT", AP_BattMonitor),464465// @Param: GCS_PID_MASK466// @DisplayName: GCS PID tuning mask467// @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for468// @User: Advanced469// @Bitmask: 0:Pitch,1:Yaw470GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),471472// @Param: SCAN_SPEED_YAW473// @DisplayName: Speed at which to rotate the yaw axis in scan mode474// @Description: This controls how rapidly the tracker will move the servos in SCAN mode475// @Units: deg/s476// @Increment: 1477// @Range: 0 100478// @User: Standard479GSCALAR(scan_speed_yaw, "SCAN_SPEED_YAW", 2),480481// @Param: SCAN_SPEED_PIT482// @DisplayName: Speed at which to rotate pitch axis in scan mode483// @Description: This controls how rapidly the tracker will move the servos in SCAN mode484// @Units: deg/s485// @Increment: 1486// @Range: 0 100487// @User: Standard488GSCALAR(scan_speed_pitch, "SCAN_SPEED_PIT", 5),489490// @Param: INITIAL_MODE491// @DisplayName: Mode tracker will switch into after initialization492// @Description: 0:MANUAL, 1:STOP, 2:SCAN, 10:AUTO493// @User: Standard494GSCALAR(initial_mode, "INITIAL_MODE", 10),495496// @Param: SAFE_DISARM_PWM497// @DisplayName: PWM that will be output when disarmed or in stop mode498// @Description: 0:zero pwm, 1:trim pwm499// @User: Standard500GSCALAR(disarm_pwm, "SAFE_DISARM_PWM", 0),501502// @Param: AUTO_OPTIONS503// @DisplayName: Auto mode options504// @Description: 1: Scan for unknown target505// @User: Standard506// @Bitmask: 0:Scan for unknown target507GSCALAR(auto_opts, "AUTO_OPTIONS", 0),508509// @Group:510// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp511PARAM_VEHICLE_INFO,512513#if HAL_NAVEKF2_AVAILABLE514// @Group: EK2_515// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp516GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),517#endif518519#if HAL_NAVEKF3_AVAILABLE520// @Group: EK3_521// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp522GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),523#endif524525#if HAL_GCS_ENABLED526// @Group: MAV527// @Path: ../libraries/GCS_MAVLink/GCS.cpp528GOBJECT(_gcs, "MAV", GCS),529#endif530531AP_VAREND532};533534535void Tracker::load_parameters(void)536{537AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version);538539#if AP_STATS_ENABLED540// PARAMETER_CONVERSION - Added: Jan-2024541AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, true);542#endif543544#if AP_SCRIPTING_ENABLED545// PARAMETER_CONVERSION - Added: Jan-2024546AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, true);547#endif548549// PARAMETER_CONVERSION - Added: Feb-2024 for Tracker-4.6550#if HAL_LOGGING_ENABLED551AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true);552#endif553554static const AP_Param::TopLevelObjectConversion toplevel_conversions[] {555#if AP_SERIALMANAGER_ENABLED556// PARAMETER_CONVERSION - Added: Feb-2024 for Tracker-4.6557{ &serial_manager, serial_manager.var_info, Parameters::k_param_serial_manager_old },558#endif559};560561AP_Param::convert_toplevel_objects(toplevel_conversions, ARRAY_SIZE(toplevel_conversions));562563#if HAL_HAVE_SAFETY_SWITCH564// configure safety switch to allow stopping the motors while armed565AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|566AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON|567AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED);568#endif569570#if HAL_GCS_ENABLED571// Move parameters into new MAV_ parameter namespace572// PARAMETER_CONVERSION - Added: Mar-2025573{574static const AP_Param::ConversionInfo gcs_conversion_info[] {575{ Parameters::k_param_sysid_this_mav_old, 0, AP_PARAM_INT16, "MAV_SYSID" },576{ Parameters::k_param_sysid_my_gcs_old, 0, AP_PARAM_INT16, "MAV_GCS_SYSID" },577};578AP_Param::convert_old_parameters(&gcs_conversion_info[0], ARRAY_SIZE(gcs_conversion_info));579}580#endif // HAL_GCS_ENABLED581582}583584585