mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
AP_AHRS: move parameters back into AP_AHRS.h
metadata collection kind of dies otherwise
This commit is contained in:
parent
d351b7c7dc
commit
4f9201a160
libraries/AP_AHRS
@ -34,6 +34,146 @@
|
||||
#define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
|
||||
#define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
|
||||
|
||||
#ifndef HAL_AHRS_EKF_TYPE_DEFAULT
|
||||
#define HAL_AHRS_EKF_TYPE_DEFAULT 3
|
||||
#endif
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AP_AHRS::var_info[] = {
|
||||
// index 0 and 1 are for old parameters that are no longer not used
|
||||
|
||||
// @Param: GPS_GAIN
|
||||
// @DisplayName: AHRS GPS gain
|
||||
// @Description: This controls how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0.
|
||||
// @Range: 0.0 1.0
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
|
||||
|
||||
// @Param: GPS_USE
|
||||
// @DisplayName: AHRS DCM use GPS for navigation
|
||||
// @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. A value of 2 means to use GPS for height as well as position in DCM.
|
||||
// @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
|
||||
|
||||
// @Param: YAW_P
|
||||
// @DisplayName: Yaw P
|
||||
// @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
|
||||
// @Range: 0.1 0.4
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.2f),
|
||||
|
||||
// @Param: RP_P
|
||||
// @DisplayName: AHRS RP_P
|
||||
// @Description: This controls how fast the accelerometers correct the attitude
|
||||
// @Range: 0.1 0.4
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.2f),
|
||||
|
||||
// @Param: WIND_MAX
|
||||
// @DisplayName: Maximum wind
|
||||
// @Description: This sets the maximum allowable difference between ground speed and airspeed. This allows the plane to cope with a failing airspeed sensor. A value of zero means to use the airspeed as is. See ARSPD_OPTIONS and ARSPD_MAX_WIND to disable airspeed sensors.
|
||||
// @Range: 0 127
|
||||
// @Units: m/s
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f),
|
||||
|
||||
// NOTE: 7 was BARO_USE
|
||||
|
||||
// @Param: TRIM_X
|
||||
// @DisplayName: AHRS Trim Roll
|
||||
// @Description: Compensates for the roll angle difference between the control board and the frame. Positive values make the vehicle roll right.
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: TRIM_Y
|
||||
// @DisplayName: AHRS Trim Pitch
|
||||
// @Description: Compensates for the pitch angle difference between the control board and the frame. Positive values make the vehicle pitch up/back.
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: TRIM_Z
|
||||
// @DisplayName: AHRS Trim Yaw
|
||||
// @Description: Not Used
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TRIM", 8, AP_AHRS, _trim, 0),
|
||||
|
||||
// @Param: ORIENTATION
|
||||
// @DisplayName: Board Orientation
|
||||
// @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. This option takes affect on next boot. After changing you will need to re-level your vehicle.
|
||||
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0),
|
||||
|
||||
// @Param: COMP_BETA
|
||||
// @DisplayName: AHRS Velocity Complementary Filter Beta Coefficient
|
||||
// @Description: This controls the time constant for the cross-over frequency used to fuse AHRS (airspeed and heading) and GPS data to estimate ground velocity. Time constant is 0.1/beta. A larger time constant will use GPS data less and a small time constant will use air data less.
|
||||
// @Range: 0.001 0.5
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("COMP_BETA", 10, AP_AHRS, beta, 0.1f),
|
||||
|
||||
// @Param: GPS_MINSATS
|
||||
// @DisplayName: AHRS GPS Minimum satellites
|
||||
// @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
|
||||
|
||||
// NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
|
||||
// of 1 was found to be the best choice
|
||||
|
||||
// 13 was the old EKF_USE
|
||||
|
||||
// @Param: EKF_TYPE
|
||||
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
|
||||
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
|
||||
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3,11:ExternalAHRS
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, HAL_AHRS_EKF_TYPE_DEFAULT),
|
||||
|
||||
// @Param: CUSTOM_ROLL
|
||||
// @DisplayName: Board orientation roll offset
|
||||
// @Description: Autopilot mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_ROLL", 15, AP_AHRS, _custom_roll, 0),
|
||||
|
||||
// @Param: CUSTOM_PIT
|
||||
// @DisplayName: Board orientation pitch offset
|
||||
// @Description: Autopilot mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_PIT", 16, AP_AHRS, _custom_pitch, 0),
|
||||
|
||||
// @Param: CUSTOM_YAW
|
||||
// @DisplayName: Board orientation yaw offset
|
||||
// @Description: Autopilot mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_YAW", 17, AP_AHRS, _custom_yaw, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// constructor
|
||||
|
@ -24,146 +24,6 @@
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#ifndef HAL_AHRS_EKF_TYPE_DEFAULT
|
||||
#define HAL_AHRS_EKF_TYPE_DEFAULT 3
|
||||
#endif
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AP_AHRS::var_info[] = {
|
||||
// index 0 and 1 are for old parameters that are no longer not used
|
||||
|
||||
// @Param: GPS_GAIN
|
||||
// @DisplayName: AHRS GPS gain
|
||||
// @Description: This controls how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0.
|
||||
// @Range: 0.0 1.0
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
|
||||
|
||||
// @Param: GPS_USE
|
||||
// @DisplayName: AHRS DCM use GPS for navigation
|
||||
// @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. A value of 2 means to use GPS for height as well as position in DCM.
|
||||
// @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
|
||||
|
||||
// @Param: YAW_P
|
||||
// @DisplayName: Yaw P
|
||||
// @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
|
||||
// @Range: 0.1 0.4
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.2f),
|
||||
|
||||
// @Param: RP_P
|
||||
// @DisplayName: AHRS RP_P
|
||||
// @Description: This controls how fast the accelerometers correct the attitude
|
||||
// @Range: 0.1 0.4
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.2f),
|
||||
|
||||
// @Param: WIND_MAX
|
||||
// @DisplayName: Maximum wind
|
||||
// @Description: This sets the maximum allowable difference between ground speed and airspeed. This allows the plane to cope with a failing airspeed sensor. A value of zero means to use the airspeed as is. See ARSPD_OPTIONS and ARSPD_MAX_WIND to disable airspeed sensors.
|
||||
// @Range: 0 127
|
||||
// @Units: m/s
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f),
|
||||
|
||||
// NOTE: 7 was BARO_USE
|
||||
|
||||
// @Param: TRIM_X
|
||||
// @DisplayName: AHRS Trim Roll
|
||||
// @Description: Compensates for the roll angle difference between the control board and the frame. Positive values make the vehicle roll right.
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: TRIM_Y
|
||||
// @DisplayName: AHRS Trim Pitch
|
||||
// @Description: Compensates for the pitch angle difference between the control board and the frame. Positive values make the vehicle pitch up/back.
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
|
||||
// @Param: TRIM_Z
|
||||
// @DisplayName: AHRS Trim Yaw
|
||||
// @Description: Not Used
|
||||
// @Units: rad
|
||||
// @Range: -0.1745 +0.1745
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TRIM", 8, AP_AHRS, _trim, 0),
|
||||
|
||||
// @Param: ORIENTATION
|
||||
// @DisplayName: Board Orientation
|
||||
// @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. This option takes affect on next boot. After changing you will need to re-level your vehicle.
|
||||
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0),
|
||||
|
||||
// @Param: COMP_BETA
|
||||
// @DisplayName: AHRS Velocity Complementary Filter Beta Coefficient
|
||||
// @Description: This controls the time constant for the cross-over frequency used to fuse AHRS (airspeed and heading) and GPS data to estimate ground velocity. Time constant is 0.1/beta. A larger time constant will use GPS data less and a small time constant will use air data less.
|
||||
// @Range: 0.001 0.5
|
||||
// @Increment: .01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("COMP_BETA", 10, AP_AHRS, beta, 0.1f),
|
||||
|
||||
// @Param: GPS_MINSATS
|
||||
// @DisplayName: AHRS GPS Minimum satellites
|
||||
// @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
|
||||
|
||||
// NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
|
||||
// of 1 was found to be the best choice
|
||||
|
||||
// 13 was the old EKF_USE
|
||||
|
||||
// @Param: EKF_TYPE
|
||||
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
|
||||
// @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
|
||||
// @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3,11:ExternalAHRS
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, HAL_AHRS_EKF_TYPE_DEFAULT),
|
||||
|
||||
// @Param: CUSTOM_ROLL
|
||||
// @DisplayName: Board orientation roll offset
|
||||
// @Description: Autopilot mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_ROLL", 15, AP_AHRS, _custom_roll, 0),
|
||||
|
||||
// @Param: CUSTOM_PIT
|
||||
// @DisplayName: Board orientation pitch offset
|
||||
// @Description: Autopilot mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_PIT", 16, AP_AHRS, _custom_pitch, 0),
|
||||
|
||||
// @Param: CUSTOM_YAW
|
||||
// @DisplayName: Board orientation yaw offset
|
||||
// @Description: Autopilot mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when AHRS_ORIENTATION is set to CUSTOM.
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("CUSTOM_YAW", 17, AP_AHRS, _custom_yaw, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
// init sets up INS board orientation
|
||||
void AP_AHRS_Backend::init()
|
||||
{
|
||||
|
Loading…
Reference in New Issue
Block a user