mirror of https://github.com/ArduPilot/ardupilot
AP_Mount: Servo mount's STAB params replaced by BrushlessPWM type
This commit is contained in:
Randy Mackay
parent
0d9526f1c5
commit
ac769baa3c
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -134,46 +134,28 @@ 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
|
||||
_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) {
|
||||
_angle_bf_output_deg.x -= degrees(ahrs.roll);
|
||||
}
|
||||
if (_state._stab_tilt) {
|
||||
_angle_bf_output_deg.y -= degrees(ahrs.pitch);
|
||||
}
|
||||
// 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 (requires_stabilization) {
|
||||
_angle_bf_output_deg.x -= degrees(ahrs.roll);
|
||||
_angle_bf_output_deg.y -= degrees(ahrs.pitch);
|
||||
}
|
||||
|
||||
// lead filter
|
||||
const Vector3f &gyro = ahrs.get_gyro();
|
||||
// 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) {
|
||||
// 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 (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)) {
|
||||
// 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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue