Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Servo mount's STAB params replaced by BrushlessPWM type

This commit is contained in:
Randy Mackay 2022-08-30 17:20:19 +09:00
parent 0d9526f1c5
commit ac769baa3c
5 changed files with 42 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_Mount/AP_Mount.cpp
View File

@ -19,7 +19,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
// @Param: _TYPE
// @DisplayName: Mount Type
// @Description: Mount Type (None, Servo or MAVLink)
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO_FLAGS("_TYPE", 19, AP_Mount, state[0]._type, 0, AP_PARAM_FLAG_ENABLE),
@ -83,26 +83,9 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
// 3 was used for control_angles
// @Param: _STAB_ROLL
// @DisplayName: Stabilize mount's roll angle
// @Description: enable roll stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("_STAB_ROLL", 4, AP_Mount, state[0]._stab_roll, 0),
// @Param: _STAB_TILT
// @DisplayName: Stabilize mount's pitch/tilt angle
// @Description: enable tilt/pitch stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("_STAB_TILT", 5, AP_Mount, state[0]._stab_tilt, 0),
// @Param: _STAB_PAN
// @DisplayName: Stabilize mount pan/yaw angle
// @Description: enable pan/yaw stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("_STAB_PAN", 6, AP_Mount, state[0]._stab_pan, 0),
// 4 was _STAB_ROLL
// 5 was _STAB_TILT
// 6 was _STAB_PAN
// 7 was _RC_IN_ROLL
@ -263,26 +246,9 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
// 3 was used for control_angles
// @Param: 2_STAB_ROLL
// @DisplayName: Stabilize Mount2's roll angle
// @Description: enable roll stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("2_STAB_ROLL", 28, AP_Mount, state[1]._stab_roll, 0),
// @Param: 2_STAB_TILT
// @DisplayName: Stabilize Mount2's pitch/tilt angle
// @Description: enable tilt/pitch stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("2_STAB_TILT", 29, AP_Mount, state[1]._stab_tilt, 0),
// @Param: 2_STAB_PAN
// @DisplayName: Stabilize mount2 pan/yaw angle
// @Description: enable pan/yaw stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("2_STAB_PAN", 30, AP_Mount, state[1]._stab_pan, 0),
// 28 was 2_STAB_ROLL
// 29 was 2_STAB_TILT
// 30 was 2_STAB_PAN
// 31 was 2_RC_IN_ROLL
@ -365,7 +331,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
// @Param: 2_TYPE
// @DisplayName: Mount2 Type
// @Description: Mount Type (None, Servo or MAVLink)
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM
// @User: Standard
AP_GROUPINFO("2_TYPE", 42, AP_Mount, state[1]._type, 0),
#endif // AP_MOUNT_MAX_INSTANCES > 1
@ -416,7 +382,7 @@ void AP_Mount::init()
// check for servo mounts
if (mount_type == Mount_Type_Servo) {
#if HAL_MOUNT_SERVO_ENABLED
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_backends[instance] = new AP_Mount_Servo(*this, state[instance], true, instance);
_num_instances++;
#endif
@ -454,6 +420,13 @@ void AP_Mount::init()
_backends[instance] = new AP_Mount_Gremsy(*this, state[instance], instance);
_num_instances++;
#endif // HAL_MOUNT_GREMSY_ENABLED
#if HAL_MOUNT_SERVO_ENABLED
// check for BrushlessPWM mounts (uses Servo backend)
} else if (mount_type == Mount_Type_BrushlessPWM) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], false, instance);
_num_instances++;
#endif
}
// init new instance
@ -591,9 +564,6 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_lon
return MAV_RESULT_FAILED;
}
_backends[_primary]->set_mode((MAV_MOUNT_MODE)packet.param1);
state[_primary]._stab_roll.set(packet.param2);
state[_primary]._stab_tilt.set(packet.param3);
state[_primary]._stab_pan.set(packet.param4);
return MAV_RESULT_ACCEPTED;
}

6
libraries/AP_Mount/AP_Mount.h
View File

@ -84,7 +84,8 @@ public:
Mount_Type_Alexmos = 3, /// Alexmos mount
Mount_Type_SToRM32 = 4, /// SToRM32 mount using MAVLink protocol
Mount_Type_SToRM32_serial = 5, /// SToRM32 mount using custom serial protocol
Mount_Type_Gremsy = 6 /// Gremsy gimbal using MAVLink v2 Gimbal protocol
Mount_Type_Gremsy = 6, /// Gremsy gimbal using MAVLink v2 Gimbal protocol
Mount_Type_BrushlessPWM = 7 /// Brushless (stabilized) gimbal using PWM protocol
};
// init - detect and initialise all mounts
@ -173,9 +174,6 @@ protected:
// Parameters
AP_Int8 _type; // mount type (None, Servo or MAVLink, see MountType enum)
AP_Int8 _default_mode; // default mode on startup and when control is returned from autopilot
AP_Int8 _stab_roll; // 1 = mount should stabilize earth-frame roll axis, 0 = no stabilization
AP_Int8 _stab_tilt; // 1 = mount should stabilize earth-frame pitch axis
AP_Int8 _stab_pan; // 1 = mount should stabilize earth-frame yaw axis
// Mount's physical limits
AP_Int16 _roll_angle_min; // min roll in 0.01 degree units

3
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -60,9 +60,6 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet)
{
set_mode((MAV_MOUNT_MODE)packet.mount_mode);
_state._stab_roll.set(packet.stab_roll);
_state._stab_tilt.set(packet.stab_pitch);
_state._stab_pan.set(packet.stab_yaw);
}
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS

26
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -134,48 +134,30 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
{
const AP_AHRS &ahrs = AP::ahrs();
// only do the full 3D frame transform if we are stabilising yaw
if (_state._stab_pan) {
Matrix3f m; // 3 x 3 rotation matrix used as temporary variable in calculations
Matrix3f ef_to_cam; // rotation matrix from earth-frame to camera. Desired camera from input.
Matrix3f gimbal_target_bf; // rotation matrix from vehicle to camera then Euler angles to the servos
Vector3f gimbal_angle_bf_rad; // gimbal angle targets in body-frame euler angles
m = ahrs.get_rotation_body_to_ned();
m.transpose();
ef_to_cam.from_euler(angle_rad.roll, angle_rad.pitch, get_ef_yaw_angle(angle_rad));
gimbal_target_bf = m * ef_to_cam;
gimbal_target_bf.to_euler(&gimbal_angle_bf_rad.x, &gimbal_angle_bf_rad.y, &gimbal_angle_bf_rad.z);
_angle_bf_output_deg.x = _state._stab_roll ? degrees(gimbal_angle_bf_rad.x) : degrees(angle_rad.roll);
_angle_bf_output_deg.y = _state._stab_tilt ? degrees(gimbal_angle_bf_rad.y) : degrees(angle_rad.pitch);
_angle_bf_output_deg.z = degrees(gimbal_angle_bf_rad.z);
} else {
// otherwise base roll and pitch on the ahrs roll and pitch angle plus any requested angle
// roll and pitch are based on the ahrs roll and pitch angle plus any requested angle
_angle_bf_output_deg.x = degrees(angle_rad.roll);
_angle_bf_output_deg.y = degrees(angle_rad.pitch);
_angle_bf_output_deg.z = degrees(get_bf_yaw_angle(angle_rad));
if (_state._stab_roll) {
if (requires_stabilization) {
_angle_bf_output_deg.x -= degrees(ahrs.roll);
}
if (_state._stab_tilt) {
_angle_bf_output_deg.y -= degrees(ahrs.pitch);
}
// lead filter
const Vector3f &gyro = ahrs.get_gyro();
if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) {
if (requires_stabilization && !is_zero(_state._roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) {
// Compute rate of change of euler roll angle
float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll());
_angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;
}
if (_state._stab_tilt && !is_zero(_state._pitch_stb_lead)) {
if (requires_stabilization && !is_zero(_state._pitch_stb_lead)) {
// Compute rate of change of euler pitch angle
float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z;
_angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;
}
}
}
// closest_limit - returns closest angle to 'angle' taking into account limits. all angles are in degrees * 10
int16_t AP_Mount_Servo::closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max)

6
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -21,8 +21,9 @@ class AP_Mount_Servo : public AP_Mount_Backend
{
public:
// Constructor
AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance):
AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state &state, bool requires_stab, uint8_t instance):
AP_Mount_Backend(frontend, state, instance),
requires_stabilization(requires_stab),
_roll_idx(SRV_Channel::k_none),
_tilt_idx(SRV_Channel::k_none),
_pan_idx(SRV_Channel::k_none),
@ -55,6 +56,9 @@ private:
/// moves servo with the given function id to the specified angle. all angles are in body-frame and degrees * 10
void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);
/// Servo gimbals require stabilization, BrushlessPWM gimbals self-stabilize
const bool requires_stabilization;
// SRV_Channel - different id numbers are used depending upon the instance number
SRV_Channel::Aux_servo_function_t _roll_idx; // SRV_Channel mount roll function index
SRV_Channel::Aux_servo_function_t _tilt_idx; // SRV_Channel mount tilt function index