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

AP_Mount: parameter angles in degrees 

Also MNTx_RC_RATE param per backend
This commit is contained in:
Randy Mackay 2022-08-30 20:15:10 +09:00
parent ac769baa3c
commit c20ec27c54
18 changed files with 275 additions and 413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

354
libraries/AP_Mount/AP_Mount.cpp
View File

@ -16,325 +16,15 @@
const AP_Param::GroupInfo AP_Mount::var_info[] = { const AP_Param::GroupInfo AP_Mount::var_info[] = {
// @Param: _TYPE // @Group: 1
// @DisplayName: Mount Type // @Path: AP_Mount_Params.cpp
// @Description: Mount Type (None, Servo or MAVLink) AP_SUBGROUPINFO(_params[0], "1", 43, AP_Mount, AP_Mount_Params),
// @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),
// @Param: _DEFLT_MODE
// @DisplayName: Mount default operating mode
// @Description: Mount default operating mode on startup and after control is returned from autopilot
// @Values: 0:Retracted,1:Neutral,2:MavLink Targeting,3:RC Targeting,4:GPS Point,6:Home Location
// @User: Standard
AP_GROUPINFO("_DEFLT_MODE", 0, AP_Mount, state[0]._default_mode, MAV_MOUNT_MODE_RC_TARGETING),
// @Param: _RETRACT_X
// @DisplayName: Mount roll angle when in retracted position
// @Description: Mount roll angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: _RETRACT_Y
// @DisplayName: Mount tilt/pitch angle when in retracted position
// @Description: Mount tilt/pitch angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: _RETRACT_Z
// @DisplayName: Mount yaw/pan angle when in retracted position
// @Description: Mount yaw/pan angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_RETRACT", 1, AP_Mount, state[0]._retract_angles, 0),
// @Param: _NEUTRAL_X
// @DisplayName: Mount roll angle when in neutral position
// @Description: Mount roll angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: _NEUTRAL_Y
// @DisplayName: Mount tilt/pitch angle when in neutral position
// @Description: Mount tilt/pitch angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: _NEUTRAL_Z
// @DisplayName: Mount pan/yaw angle when in neutral position
// @Description: Mount pan/yaw angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_NEUTRAL", 2, AP_Mount, state[0]._neutral_angles, 0),
// 3 was used for control_angles
// 4 was _STAB_ROLL
// 5 was _STAB_TILT
// 6 was _STAB_PAN
// 7 was _RC_IN_ROLL
// @Param: _ANGMIN_ROL
// @DisplayName: Minimum roll angle
// @Description: Minimum physical roll angular position of mount.
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMIN_ROL", 8, AP_Mount, state[0]._roll_angle_min, -4500),
// @Param: _ANGMAX_ROL
// @DisplayName: Maximum roll angle
// @Description: Maximum physical roll angular position of the mount
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMAX_ROL", 9, AP_Mount, state[0]._roll_angle_max, 4500),
// 10 was _RC_IN_TILT
// @Param: _ANGMIN_TIL
// @DisplayName: Minimum tilt angle
// @Description: Minimum physical tilt (pitch) angular position of mount.
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMIN_TIL", 11, AP_Mount, state[0]._tilt_angle_min, -4500),
// @Param: _ANGMAX_TIL
// @DisplayName: Maximum tilt angle
// @Description: Maximum physical tilt (pitch) angular position of the mount
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMAX_TIL", 12, AP_Mount, state[0]._tilt_angle_max, 4500),
// 13 was _RC_IN_PAN
// @Param: _ANGMIN_PAN
// @DisplayName: Minimum pan angle
// @Description: Minimum physical pan (yaw) angular position of mount.
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMIN_PAN", 14, AP_Mount, state[0]._pan_angle_min, -4500),
// @Param: _ANGMAX_PAN
// @DisplayName: Maximum pan angle
// @Description: Maximum physical pan (yaw) angular position of the mount
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("_ANGMAX_PAN", 15, AP_Mount, state[0]._pan_angle_max, 4500),
// 16 was _JSTICK_SPD
// @Param: _LEAD_RLL
// @DisplayName: Roll stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate, compensating for servo delay. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("_LEAD_RLL", 17, AP_Mount, state[0]._roll_stb_lead, 0.0f),
// @Param: _LEAD_PTCH
// @DisplayName: Pitch stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("_LEAD_PTCH", 18, AP_Mount, state[0]._pitch_stb_lead, 0.0f),
// 19 _TYPE, now at top with enable flag
// 20 formerly _OFF_JNT
// 21 formerly _OFF_ACC
// 22 formerly _OFF_GYRO
// 23 formerly _K_RATE
// @Param: _RC_RATE
// @DisplayName: Mount RC Rate
// @Description: Pilot rate control's maximum rate. Set to zero to use angle control
// @Units: deg/s
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_RC_RATE", 24, AP_Mount, _rc_rate_max, 0),
#if AP_MOUNT_MAX_INSTANCES > 1 #if AP_MOUNT_MAX_INSTANCES > 1
// @Param: 2_DEFLT_MODE // @Group: 2
// @DisplayName: Mount default operating mode // @Path: AP_Mount_Params.cpp
// @Description: Mount default operating mode on startup and after control is returned from autopilot AP_SUBGROUPINFO(_params[1], "2", 44, AP_Mount, AP_Mount_Params),
// @Values: 0:Retracted,1:Neutral,2:MavLink Targeting,3:RC Targeting,4:GPS Point,6:Home Location #endif
// @User: Standard
AP_GROUPINFO("2_DEFLT_MODE", 25, AP_Mount, state[1]._default_mode, MAV_MOUNT_MODE_RC_TARGETING),
// @Param: 2_RETRACT_X
// @DisplayName: Mount2 roll angle when in retracted position
// @Description: Mount2 roll angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: 2_RETRACT_Y
// @DisplayName: Mount2 tilt/pitch angle when in retracted position
// @Description: Mount2 tilt/pitch angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: 2_RETRACT_Z
// @DisplayName: Mount2 yaw/pan angle when in retracted position
// @Description: Mount2 yaw/pan angle when in retracted position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("2_RETRACT", 26, AP_Mount, state[1]._retract_angles, 0),
// @Param: 2_NEUTRAL_X
// @DisplayName: Mount2 roll angle when in neutral position
// @Description: Mount2 roll angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: 2_NEUTRAL_Y
// @DisplayName: Mount2 tilt/pitch angle when in neutral position
// @Description: Mount2 tilt/pitch angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: 2_NEUTRAL_Z
// @DisplayName: Mount2 pan/yaw angle when in neutral position
// @Description: Mount2 pan/yaw angle when in neutral position
// @Units: deg
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("2_NEUTRAL", 27, AP_Mount, state[1]._neutral_angles, 0),
// 3 was used for control_angles
// 28 was 2_STAB_ROLL
// 29 was 2_STAB_TILT
// 30 was 2_STAB_PAN
// 31 was 2_RC_IN_ROLL
// @Param: 2_ANGMIN_ROL
// @DisplayName: Mount2's minimum roll angle
// @Description: Mount2's minimum physical roll angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMIN_ROL", 32, AP_Mount, state[1]._roll_angle_min, -4500),
// @Param: 2_ANGMAX_ROL
// @DisplayName: Mount2's maximum roll angle
// @Description: Mount2's maximum physical roll angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMAX_ROL", 33, AP_Mount, state[1]._roll_angle_max, 4500),
// 34 was 2_RC_IN_TILT
// @Param: 2_ANGMIN_TIL
// @DisplayName: Mount2's minimum tilt angle
// @Description: Mount2's minimum physical tilt (pitch) angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMIN_TIL", 35, AP_Mount, state[1]._tilt_angle_min, -4500),
// @Param: 2_ANGMAX_TIL
// @DisplayName: Mount2's maximum tilt angle
// @Description: Mount2's maximum physical tilt (pitch) angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMAX_TIL", 36, AP_Mount, state[1]._tilt_angle_max, 4500),
// 37 was 2_RC_IN_PAN
// @Param: 2_ANGMIN_PAN
// @DisplayName: Mount2's minimum pan angle
// @Description: Mount2's minimum physical pan (yaw) angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMIN_PAN", 38, AP_Mount, state[1]._pan_angle_min, -4500),
// @Param: 2_ANGMAX_PAN
// @DisplayName: Mount2's maximum pan angle
// @Description: MOunt2's maximum physical pan (yaw) angular position
// @Units: cdeg
// @Range: -18000 17999
// @Increment: 10
// @User: Standard
AP_GROUPINFO("2_ANGMAX_PAN", 39, AP_Mount, state[1]._pan_angle_max, 4500),
// @Param: 2_LEAD_RLL
// @DisplayName: Mount2's Roll stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate, compensating for servo delay. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("2_LEAD_RLL", 40, AP_Mount, state[1]._roll_stb_lead, 0.0f),
// @Param: 2_LEAD_PTCH
// @DisplayName: Mount2's Pitch stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("2_LEAD_PTCH", 41, AP_Mount, state[1]._pitch_stb_lead, 0.0f),
// @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, 7:BrushlessPWM
// @User: Standard
AP_GROUPINFO("2_TYPE", 42, AP_Mount, state[1]._type, 0),
#endif // AP_MOUNT_MAX_INSTANCES > 1
AP_GROUPEND AP_GROUPEND
}; };
@ -361,11 +51,11 @@ void AP_Mount::init()
} }
// default mount to servo mount if rc output channels to control roll, tilt or pan have been defined // default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
if (!state[0]._type.configured()) { if (!_params[0].type.configured()) {
if (SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_pan) || if (SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_pan) ||
SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_tilt) || SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_tilt) ||
SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_roll)) { SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t::k_mount_roll)) {
state[0]._type.set_and_save(Mount_Type_Servo); _params[0].type.set_and_save(Mount_Type_Servo);
} }
} }
@ -382,49 +72,49 @@ void AP_Mount::init()
// check for servo mounts // check for servo mounts
if (mount_type == Mount_Type_Servo) { if (mount_type == Mount_Type_Servo) {
#if HAL_MOUNT_SERVO_ENABLED #if HAL_MOUNT_SERVO_ENABLED
_backends[instance] = new AP_Mount_Servo(*this, state[instance], true, instance); _backends[instance] = new AP_Mount_Servo(*this, _params[instance], true, instance);
_num_instances++; _num_instances++;
#endif #endif
#if HAL_SOLO_GIMBAL_ENABLED #if HAL_SOLO_GIMBAL_ENABLED
// check for Solo mounts // check for Solo mounts
} else if (mount_type == Mount_Type_SoloGimbal) { } else if (mount_type == Mount_Type_SoloGimbal) {
_backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance); _backends[instance] = new AP_Mount_SoloGimbal(*this, _params[instance], instance);
_num_instances++; _num_instances++;
#endif // HAL_SOLO_GIMBAL_ENABLED #endif // HAL_SOLO_GIMBAL_ENABLED
#if HAL_MOUNT_ALEXMOS_ENABLED #if HAL_MOUNT_ALEXMOS_ENABLED
// check for Alexmos mounts // check for Alexmos mounts
} else if (mount_type == Mount_Type_Alexmos) { } else if (mount_type == Mount_Type_Alexmos) {
_backends[instance] = new AP_Mount_Alexmos(*this, state[instance], instance); _backends[instance] = new AP_Mount_Alexmos(*this, _params[instance], instance);
_num_instances++; _num_instances++;
#endif #endif
#if HAL_MOUNT_STORM32MAVLINK_ENABLED #if HAL_MOUNT_STORM32MAVLINK_ENABLED
// check for SToRM32 mounts using MAVLink protocol // check for SToRM32 mounts using MAVLink protocol
} else if (mount_type == Mount_Type_SToRM32) { } else if (mount_type == Mount_Type_SToRM32) {
_backends[instance] = new AP_Mount_SToRM32(*this, state[instance], instance); _backends[instance] = new AP_Mount_SToRM32(*this, _params[instance], instance);
_num_instances++; _num_instances++;
#endif #endif
#if HAL_MOUNT_STORM32SERIAL_ENABLED #if HAL_MOUNT_STORM32SERIAL_ENABLED
// check for SToRM32 mounts using serial protocol // check for SToRM32 mounts using serial protocol
} else if (mount_type == Mount_Type_SToRM32_serial) { } else if (mount_type == Mount_Type_SToRM32_serial) {
_backends[instance] = new AP_Mount_SToRM32_serial(*this, state[instance], instance); _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance);
_num_instances++; _num_instances++;
#endif #endif
#if HAL_MOUNT_GREMSY_ENABLED #if HAL_MOUNT_GREMSY_ENABLED
// check for Gremsy mounts // check for Gremsy mounts
} else if (mount_type == Mount_Type_Gremsy) { } else if (mount_type == Mount_Type_Gremsy) {
_backends[instance] = new AP_Mount_Gremsy(*this, state[instance], instance); _backends[instance] = new AP_Mount_Gremsy(*this, _params[instance], instance);
_num_instances++; _num_instances++;
#endif // HAL_MOUNT_GREMSY_ENABLED #endif // HAL_MOUNT_GREMSY_ENABLED
#if HAL_MOUNT_SERVO_ENABLED #if HAL_MOUNT_SERVO_ENABLED
// check for BrushlessPWM mounts (uses Servo backend) // check for BrushlessPWM mounts (uses Servo backend)
} else if (mount_type == Mount_Type_BrushlessPWM) { } else if (mount_type == Mount_Type_BrushlessPWM) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], false, instance); _backends[instance] = new AP_Mount_Servo(*this, _params[instance], false, instance);
_num_instances++; _num_instances++;
#endif #endif
} }
@ -476,7 +166,7 @@ AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const
return Mount_Type_None; return Mount_Type_None;
} }
return (MountType)state[instance]._type.get(); return (MountType)_params[instance].type.get();
} }
// has_pan_control - returns true if the mount has yaw control (required for copters) // has_pan_control - returns true if the mount has yaw control (required for copters)
@ -502,11 +192,11 @@ MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const
return _backends[instance]->get_mode(); return _backends[instance]->get_mode();
} }
// set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter // set_mode_to_default - restores the mode to it's default mode held in the MNTx__DEFLT_MODE parameter
// this operation requires 60us on a Pixhawk/PX4 // this operation requires 60us on a Pixhawk/PX4
void AP_Mount::set_mode_to_default(uint8_t instance) void AP_Mount::set_mode_to_default(uint8_t instance)
{ {
set_mode(instance, (enum MAV_MOUNT_MODE)state[instance]._default_mode.get()); set_mode(instance, (enum MAV_MOUNT_MODE)_params[instance].default_mode.get());
} }
// set_mode - sets mount's mode // set_mode - sets mount's mode
@ -696,7 +386,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
{ {
// check type parameters // check type parameters
for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) { for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
if ((state[i]._type != Mount_Type_None) && (_backends[i] == nullptr)) { if ((_params[i].type != Mount_Type_None) && (_backends[i] == nullptr)) {
strncpy(failure_msg, "check TYPE", failure_msg_len); strncpy(failure_msg, "check TYPE", failure_msg_len);
return false; return false;
} }
@ -820,10 +510,10 @@ void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg
void AP_Mount::convert_params() void AP_Mount::convert_params()
{ {
// convert JSTICK_SPD to RC_RATE // convert JSTICK_SPD to RC_RATE
if (!_rc_rate_max.configured()) { if (!_params[0].rc_rate_max.configured()) {
int8_t jstick_spd = 0; int8_t jstick_spd = 0;
if (AP_Param::get_param_by_index(this, 16, AP_PARAM_INT8, &jstick_spd) && (jstick_spd > 0)) { if (AP_Param::get_param_by_index(this, 16, AP_PARAM_INT8, &jstick_spd) && (jstick_spd > 0)) {
_rc_rate_max.set_and_save(jstick_spd * 0.3); _params[0].rc_rate_max.set_and_save(jstick_spd * 0.3);
} }
} }
} }

31
libraries/AP_Mount/AP_Mount.h
View File

@ -14,7 +14,7 @@
* Usage: Use in main code to control mounts attached to * * Usage: Use in main code to control mounts attached to *
* vehicle. * * vehicle. *
* * * *
* Comments: All angles in degrees * 100, distances in meters* * Comments: All angles in degrees, distances in meters *
* unless otherwise stated. * * unless otherwise stated. *
************************************************************/ ************************************************************/
#pragma once #pragma once
@ -35,6 +35,7 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Mount_Params.h"
// maximum number of mounts // maximum number of mounts
#define AP_MOUNT_MAX_INSTANCES 1 #define AP_MOUNT_MAX_INSTANCES 1
@ -114,7 +115,7 @@ public:
void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); } void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); }
void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode); void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode);
// set_mode_to_default - restores the mode to it's default mode held in the MNT_DEFLT_MODE parameter // set_mode_to_default - restores the mode to it's default mode held in the MNTx_DEFLT_MODE parameter
// this operation requires 60us on a Pixhawk/PX4 // this operation requires 60us on a Pixhawk/PX4
void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default() { set_mode_to_default(_primary); }
void set_mode_to_default(uint8_t instance); void set_mode_to_default(uint8_t instance);
@ -161,36 +162,14 @@ protected:
static AP_Mount *_singleton; static AP_Mount *_singleton;
// frontend parameters // parameters for backends
AP_Int16 _rc_rate_max; // Pilot rate control's maximum rate. Set to zero to use angle control AP_Mount_Params _params[AP_MOUNT_MAX_INSTANCES];
// front end members // front end members
uint8_t _num_instances; // number of mounts instantiated uint8_t _num_instances; // number of mounts instantiated
uint8_t _primary; // primary mount uint8_t _primary; // primary mount
AP_Mount_Backend *_backends[AP_MOUNT_MAX_INSTANCES]; // pointers to instantiated mounts AP_Mount_Backend *_backends[AP_MOUNT_MAX_INSTANCES]; // pointers to instantiated mounts
// backend state including parameters
struct mount_state {
// 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
// Mount's physical limits
AP_Int16 _roll_angle_min; // min roll in 0.01 degree units
AP_Int16 _roll_angle_max; // max roll in 0.01 degree units
AP_Int16 _tilt_angle_min; // min tilt in 0.01 degree units
AP_Int16 _tilt_angle_max; // max tilt in 0.01 degree units
AP_Int16 _pan_angle_min; // min pan in 0.01 degree units
AP_Int16 _pan_angle_max; // max pan in 0.01 degree units
AP_Vector3f _retract_angles; // retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan
AP_Vector3f _neutral_angles; // neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan
AP_Float _roll_stb_lead; // roll lead control gain
AP_Float _pitch_stb_lead; // pitch lead control gain
} state[AP_MOUNT_MAX_INSTANCES];
private: private:
// Check if instance backend is ok // Check if instance backend is ok
bool check_primary() const; bool check_primary() const;

6
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -15,7 +15,7 @@ void AP_Mount_Alexmos::init()
_initialised = true; _initialised = true;
get_boardinfo(); get_boardinfo();
read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get());
} }
} }
@ -32,7 +32,7 @@ void AP_Mount_Alexmos::update()
switch (get_mode()) { switch (get_mode()) {
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT: { case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _state._retract_angles.get(); const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = radians(target.x); _angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y); _angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z); _angle_rad.yaw = radians(target.z);
@ -42,7 +42,7 @@ void AP_Mount_Alexmos::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _state._neutral_angles.get(); const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x); _angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y); _angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z); _angle_rad.yaw = radians(target.z);

4
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -68,8 +68,8 @@ class AP_Mount_Alexmos : public AP_Mount_Backend
{ {
public: public:
//constructor //constructor
AP_Mount_Alexmos(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance): AP_Mount_Alexmos(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance):
AP_Mount_Backend(frontend, state, instance) AP_Mount_Backend(frontend, params, instance)
{} {}
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance

18
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -230,7 +230,7 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_
bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const
{ {
// exit immediately if RC is not providing rate targets // exit immediately if RC is not providing rate targets
if (_frontend._rc_rate_max <= 0) { if (_params.rc_rate_max <= 0) {
return false; return false;
} }
@ -239,7 +239,7 @@ bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const
get_rc_input(roll_in, pitch_in, yaw_in); get_rc_input(roll_in, pitch_in, yaw_in);
// calculate rates // calculate rates
const float rc_rate_max_rads = radians(_frontend._rc_rate_max.get()); const float rc_rate_max_rads = radians(_params.rc_rate_max.get());
rate_rads.roll = roll_in * rc_rate_max_rads; rate_rads.roll = roll_in * rc_rate_max_rads;
rate_rads.pitch = pitch_in * rc_rate_max_rads; rate_rads.pitch = pitch_in * rc_rate_max_rads;
rate_rads.yaw = yaw_in * rc_rate_max_rads; rate_rads.yaw = yaw_in * rc_rate_max_rads;
@ -255,7 +255,7 @@ bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const
bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const
{ {
// exit immediately if RC is not providing angle targets // exit immediately if RC is not providing angle targets
if (_frontend._rc_rate_max > 0) { if (_params.rc_rate_max > 0) {
return false; return false;
} }
@ -264,10 +264,10 @@ bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const
get_rc_input(roll_in, pitch_in, yaw_in); get_rc_input(roll_in, pitch_in, yaw_in);
// roll angle // roll angle
angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_state._roll_angle_max - _state._roll_angle_min) + _state._roll_angle_min)*0.01f); angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min));
// pitch angle // pitch angle
angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_state._tilt_angle_max - _state._tilt_angle_min) + _state._tilt_angle_min)*0.01f); angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min));
// yaw angle // yaw angle
angle_rad.yaw_is_ef = _yaw_lock; angle_rad.yaw_is_ef = _yaw_lock;
@ -276,7 +276,7 @@ bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const
angle_rad.yaw = yaw_in * M_PI; angle_rad.yaw = yaw_in * M_PI;
} else { } else {
// yaw target in body frame so apply body frame limits // yaw target in body frame so apply body frame limits
angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_state._pan_angle_max - _state._pan_angle_min) + _state._pan_angle_min)*0.01f); angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min));
} }
return true; return true;
@ -359,8 +359,8 @@ float AP_Mount_Backend::get_ef_yaw_angle(const MountTarget& angle_rad) const
void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const
{ {
// update roll and pitch angles and apply limits // update roll and pitch angles and apply limits
angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_state._roll_angle_min * 0.01), radians(_state._roll_angle_max * 0.01)); angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
angle_rad.pitch = constrain_float(angle_rad.pitch + rate_rad.pitch * AP_MOUNT_UPDATE_DT, radians(_state._tilt_angle_min * 0.01), radians(_state._tilt_angle_max * 0.01)); angle_rad.pitch = constrain_float(angle_rad.pitch + rate_rad.pitch * AP_MOUNT_UPDATE_DT, radians(_params.pitch_angle_min), radians(_params.pitch_angle_max));
// ensure angle yaw frames matches rate yaw frame // ensure angle yaw frames matches rate yaw frame
if (angle_rad.yaw_is_ef != rate_rad.yaw_is_ef) { if (angle_rad.yaw_is_ef != rate_rad.yaw_is_ef) {
@ -379,7 +379,7 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
angle_rad.yaw = wrap_PI(angle_rad.yaw); angle_rad.yaw = wrap_PI(angle_rad.yaw);
} else { } else {
// if body-frame constrain yaw to body-frame limits // if body-frame constrain yaw to body-frame limits
angle_rad.yaw = constrain_float(angle_rad.yaw, radians(_state._pan_angle_min * 0.01), radians(_state._pan_angle_max * 0.01)); angle_rad.yaw = constrain_float(angle_rad.yaw, radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
} }
} }

8
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -28,9 +28,9 @@ class AP_Mount_Backend
{ {
public: public:
// Constructor // Constructor
AP_Mount_Backend(AP_Mount &frontend, AP_Mount::mount_state& state, uint8_t instance) : AP_Mount_Backend(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
_frontend(frontend), _frontend(frontend),
_state(state), _params(params),
_instance(instance) _instance(instance)
{} {}
@ -117,7 +117,7 @@ protected:
// returns true if user has configured a valid yaw angle range // returns true if user has configured a valid yaw angle range
// allows user to disable yaw even on 3-axis gimbal // allows user to disable yaw even on 3-axis gimbal
bool yaw_range_valid() const { return (_state._pan_angle_min < _state._pan_angle_max); } bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }
// returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal) // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
virtual bool suppress_heartbeat() const { return false; } virtual bool suppress_heartbeat() const { return false; }
@ -170,7 +170,7 @@ protected:
void send_warning_to_GCS(const char* warning_str); void send_warning_to_GCS(const char* warning_str);
AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount &_frontend; // reference to the front end which holds parameters
AP_Mount::mount_state &_state; // references to the parameters and state for this backend AP_Mount_Params &_params; // parameters for this backend
uint8_t _instance; // this instance's number uint8_t _instance; // this instance's number
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)

18
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -8,8 +8,8 @@ extern const AP_HAL::HAL& hal;
#define AP_MOUNT_GREMSY_SEARCH_MS 60000 // search for gimbal for 1 minute after startup #define AP_MOUNT_GREMSY_SEARCH_MS 60000 // search for gimbal for 1 minute after startup
#define AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US 10000 // send ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE at 100hz #define AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US 10000 // send ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE at 100hz
AP_Mount_Gremsy::AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_Gremsy::AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance) AP_Mount_Backend(frontend, params, instance)
{} {}
// update mount position // update mount position
@ -32,7 +32,7 @@ void AP_Mount_Gremsy::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _state._neutral_angles.get(); const Vector3f &angle_bf_target = _params.neutral_angles.get();
send_gimbal_device_set_attitude(ToRad(angle_bf_target.x), ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); send_gimbal_device_set_attitude(ToRad(angle_bf_target.x), ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
} }
break; break;
@ -193,12 +193,12 @@ void AP_Mount_Gremsy::handle_gimbal_device_information(const mavlink_message_t &
mavlink_msg_gimbal_device_information_decode(&msg, &info); mavlink_msg_gimbal_device_information_decode(&msg, &info);
// set parameter defaults from gimbal information // set parameter defaults from gimbal information
_state._roll_angle_min.set_default(degrees(info.roll_min) * 100); _params.roll_angle_min.set_default(degrees(info.roll_min));
_state._roll_angle_max.set_default(degrees(info.roll_max) * 100); _params.roll_angle_max.set_default(degrees(info.roll_max));
_state._tilt_angle_min.set_default(degrees(info.pitch_min) * 100); _params.pitch_angle_min.set_default(degrees(info.pitch_min));
_state._tilt_angle_max.set_default(degrees(info.pitch_max) * 100); _params.pitch_angle_max.set_default(degrees(info.pitch_max));
_state._pan_angle_min.set_default(degrees(info.yaw_min) * 100); _params.yaw_angle_min.set_default(degrees(info.yaw_min));
_state._pan_angle_max.set_default(degrees(info.yaw_max) * 100); _params.yaw_angle_max.set_default(degrees(info.yaw_max));
const uint8_t fw_ver_major = info.firmware_version & 0x000000FF; const uint8_t fw_ver_major = info.firmware_version & 0x000000FF;
const uint8_t fw_ver_minor = (info.firmware_version & 0x0000FF00) >> 8; const uint8_t fw_ver_minor = (info.firmware_version & 0x0000FF00) >> 8;

2
libraries/AP_Mount/AP_Mount_Gremsy.h
View File

@ -22,7 +22,7 @@ class AP_Mount_Gremsy : public AP_Mount_Backend
public: public:
// Constructor // Constructor
AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance);
// init // init
void init() override {} void init() override {}

160
libraries/AP_Mount/AP_Mount_Params.cpp Normal file
View File

@ -0,0 +1,160 @@
#include "AP_Mount_Params.h"
#include <GCS_MAVLink/GCS_MAVLink.h>
// table of user settable parameters
const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
// 0 should not be used
// @Param: _TYPE
// @DisplayName: Mount Type
// @Description: Mount Type
// @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", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE),
// @Param: _DEFLT_MODE
// @DisplayName: Mount default operating mode
// @Description: Mount default operating mode on startup and after control is returned from autopilot
// @Values: 0:Retracted,1:Neutral,2:MavLink Targeting,3:RC Targeting,4:GPS Point,6:Home Location
// @User: Standard
AP_GROUPINFO("_DEFLT_MODE", 2, AP_Mount_Params, default_mode, MAV_MOUNT_MODE_RC_TARGETING),
// @Param: _RC_RATE
// @DisplayName: Mount RC Rate
// @Description: Pilot rate control's maximum rate. Set to zero to use angle control
// @Units: deg/s
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_RC_RATE", 3, AP_Mount_Params, rc_rate_max, 0),
// @Param: _ROLL_MIN
// @DisplayName: Mount Roll angle minimum
// @Description: Mount Roll angle minimum
// @Units: deg
// @Range: -180 180
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_ROLL_MIN", 4, AP_Mount_Params, roll_angle_min, -30),
// @Param: _ROLL_MAX
// @DisplayName: Mount Roll angle maximum
// @Description: Mount Roll angle maximum
// @Units: deg
// @Range: -180 180
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_ROLL_MAX", 5, AP_Mount_Params, roll_angle_max, 30),
// @Param: _PITCH_MIN
// @DisplayName: Mount Pitch angle minimum
// @Description: Mount Pitch angle minimum
// @Units: deg
// @Range: -90 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_PITCH_MIN", 6, AP_Mount_Params, pitch_angle_min, -90),
// @Param: _PITCH_MAX
// @DisplayName: Mount Pitch angle maximum
// @Description: Mount Pitch angle maximum
// @Units: deg
// @Range: -90 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_PITCH_MAX", 7, AP_Mount_Params, pitch_angle_max, 20),
// @Param: _YAW_MIN
// @DisplayName: Mount Yaw angle minimum
// @Description: Mount Yaw angle minimum
// @Units: deg
// @Range: -180 180
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_YAW_MIN", 8, AP_Mount_Params, yaw_angle_min, -180),
// @Param: _YAW_MAX
// @DisplayName: Mount Yaw angle maximum
// @Description: Mount Yaw angle maximum
// @Units: deg
// @Range: -180 180
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_YAW_MAX", 9, AP_Mount_Params, yaw_angle_max, 180),
// @Param: _RETRACT_X
// @DisplayName: Mount roll angle when in retracted position
// @Description: Mount roll angle when in retracted position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
// @Param: _RETRACT_Y
// @DisplayName: Mount pitch angle when in retracted position
// @Description: Mount pitch angle when in retracted position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
// @Param: _RETRACT_Z
// @DisplayName: Mount yaw angle when in retracted position
// @Description: Mount yaw angle when in retracted position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_RETRACT", 10, AP_Mount_Params, retract_angles, 0),
// @Param: _NEUTRAL_X
// @DisplayName: Mount roll angle when in neutral position
// @Description: Mount roll angle when in neutral position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
// @Param: _NEUTRAL_Y
// @DisplayName: Mount pitch angle when in neutral position
// @Description: Mount pitch angle when in neutral position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
// @Param: _NEUTRAL_Z
// @DisplayName: Mount yaw angle when in neutral position
// @Description: Mount yaw angle when in neutral position
// @Units: deg
// @Range: -180.0 180.0
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_NEUTRAL", 11, AP_Mount_Params, neutral_angles, 0),
// @Param: _LEAD_RLL
// @DisplayName: Mount Roll stabilization lead time
// @Description: Servo mount roll angle output leads the vehicle angle by this amount of time based on current roll rate. Increase until the servo is responsive but does not overshoot
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("_LEAD_RLL", 12, AP_Mount_Params, roll_stb_lead, 0.0f),
// @Param: _LEAD_PTCH
// @DisplayName: Mount Pitch stabilization lead time
// @Description: Servo mount pitch angle output leads the vehicle angle by this amount of time based on current pitch rate. Increase until the servo is responsive but does not overshoot
// @Units: s
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("_LEAD_PTCH", 13, AP_Mount_Params, pitch_stb_lead, 0.0f),
AP_GROUPEND
};
AP_Mount_Params::AP_Mount_Params(void) {
AP_Param::setup_object_defaults(this, var_info);
}

33
libraries/AP_Mount/AP_Mount_Params.h Normal file
View File

@ -0,0 +1,33 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
class AP_Mount_Params {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_Mount_Params(void);
/* Do not allow copies */
AP_Mount_Params(const AP_Mount_Params &other) = delete;
AP_Mount_Params &operator=(const AP_Mount_Params&) = delete;
AP_Int8 type; // mount type (see MountType enum)
AP_Int8 default_mode; // default mode on startup and when control is returned from autopilot
AP_Int16 rc_rate_max; // Pilot rate control's maximum rate. Set to zero to use angle control
AP_Int16 roll_angle_min; // roll angle min in degrees
AP_Int16 roll_angle_max; // roll angle max in degrees
AP_Int16 pitch_angle_min; // pitch angle min in degrees
AP_Int16 pitch_angle_max; // pitch angle max in degrees
AP_Int16 yaw_angle_min; // yaw angle min in degrees
AP_Int16 yaw_angle_max; // yaw angle max in degrees
AP_Vector3f retract_angles; // retracted position in degrees. vector.x = roll vector.y = pitch, vector.z=yaw
AP_Vector3f neutral_angles; // neutral position in degrees. vector.x = roll vector.y = pitch, vector.z=yaw
AP_Float roll_stb_lead; // roll lead control gain (only used by servo backend)
AP_Float pitch_stb_lead; // pitch lead control gain (only used by servo backend)
};

8
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -9,8 +9,8 @@ extern const AP_HAL::HAL& hal;
#define AP_MOUNT_STORM32_RESEND_MS 1000 // resend angle targets to gimbal once per second #define AP_MOUNT_STORM32_RESEND_MS 1000 // resend angle targets to gimbal once per second
#define AP_MOUNT_STORM32_SEARCH_MS 60000 // search for gimbal for 1 minute after startup #define AP_MOUNT_STORM32_SEARCH_MS 60000 // search for gimbal for 1 minute after startup
AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance), AP_Mount_Backend(frontend, params, instance),
_chan(MAVLINK_COMM_0) _chan(MAVLINK_COMM_0)
{} {}
@ -30,7 +30,7 @@ void AP_Mount_SToRM32::update()
switch(get_mode()) { switch(get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: { case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _state._retract_angles.get(); const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = radians(target.x); _angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y); _angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z); _angle_rad.yaw = radians(target.z);
@ -40,7 +40,7 @@ void AP_Mount_SToRM32::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _state._neutral_angles.get(); const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x); _angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y); _angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z); _angle_rad.yaw = radians(target.z);

2
libraries/AP_Mount/AP_Mount_SToRM32.h
View File

@ -21,7 +21,7 @@ class AP_Mount_SToRM32 : public AP_Mount_Backend
public: public:
// Constructor // Constructor
AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance);
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
void init() override {} void init() override {}

10
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -8,8 +8,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Mount_SToRM32_serial::AP_Mount_SToRM32_serial(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_SToRM32_serial::AP_Mount_SToRM32_serial(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance), AP_Mount_Backend(frontend, params, instance),
_reply_type(ReplyType_UNKNOWN) _reply_type(ReplyType_UNKNOWN)
{} {}
@ -21,7 +21,7 @@ void AP_Mount_SToRM32_serial::init()
_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_SToRM32, 0); _port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_SToRM32, 0);
if (_port) { if (_port) {
_initialised = true; _initialised = true;
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get());
} }
} }
@ -43,7 +43,7 @@ void AP_Mount_SToRM32_serial::update()
switch(get_mode()) { switch(get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: { case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _state._retract_angles.get(); const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = ToRad(target.x); _angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y); _angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z); _angle_rad.yaw = ToRad(target.z);
@ -53,7 +53,7 @@ void AP_Mount_SToRM32_serial::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _state._neutral_angles.get(); const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = ToRad(target.x); _angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y); _angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z); _angle_rad.yaw = ToRad(target.z);

2
libraries/AP_Mount/AP_Mount_SToRM32_serial.h
View File

@ -24,7 +24,7 @@ class AP_Mount_SToRM32_serial : public AP_Mount_Backend
public: public:
// Constructor // Constructor
AP_Mount_SToRM32_serial(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); AP_Mount_SToRM32_serial(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance);
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
void init() override; void init() override;

18
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -26,7 +26,7 @@ void AP_Mount_Servo::update()
switch (get_mode()) { switch (get_mode()) {
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT: { case MAV_MOUNT_MODE_RETRACT: {
_angle_bf_output_deg = _state._retract_angles.get(); _angle_bf_output_deg = _params.retract_angles.get();
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = radians(_angle_bf_output_deg.x); _angle_rad.roll = radians(_angle_bf_output_deg.x);
@ -38,7 +38,7 @@ void AP_Mount_Servo::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
_angle_bf_output_deg = _state._neutral_angles.get(); _angle_bf_output_deg = _params.neutral_angles.get();
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = radians(_angle_bf_output_deg.x); _angle_rad.roll = radians(_angle_bf_output_deg.x);
@ -109,9 +109,9 @@ void AP_Mount_Servo::update()
move_servo(_open_idx, mount_open, 0, 1); move_servo(_open_idx, mount_open, 0, 1);
// write the results to the servos // write the results to the servos
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _state._roll_angle_min*0.1f, _state._roll_angle_max*0.1f); move_servo(_roll_idx, _angle_bf_output_deg.x*10, _params.roll_angle_min*10, _params.roll_angle_max*10);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _state._tilt_angle_min*0.1f, _state._tilt_angle_max*0.1f); move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _params.pitch_angle_min*10, _params.pitch_angle_max*10);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _state._pan_angle_min*0.1f, _state._pan_angle_max*0.1f); move_servo(_pan_idx, _angle_bf_output_deg.z*10, _params.yaw_angle_min*10, _params.yaw_angle_max*10);
} }
// returns true if this mount can control its pan (required for multicopters) // returns true if this mount can control its pan (required for multicopters)
@ -146,16 +146,16 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
// lead filter // lead filter
const Vector3f &gyro = ahrs.get_gyro(); const Vector3f &gyro = ahrs.get_gyro();
if (requires_stabilization && !is_zero(_state._roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) { if (requires_stabilization && !is_zero(_params.roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) {
// Compute rate of change of euler roll angle // 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()); 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; _angle_bf_output_deg.x -= degrees(roll_rate) * _params.roll_stb_lead;
} }
if (requires_stabilization && !is_zero(_state._pitch_stb_lead)) { if (requires_stabilization && !is_zero(_params.pitch_stb_lead)) {
// Compute rate of change of euler pitch angle // Compute rate of change of euler pitch angle
float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z; 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; _angle_bf_output_deg.y -= degrees(pitch_rate) * _params.pitch_stb_lead;
} }
} }

4
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -21,8 +21,8 @@ class AP_Mount_Servo : public AP_Mount_Backend
{ {
public: public:
// Constructor // Constructor
AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state &state, bool requires_stab, uint8_t instance): AP_Mount_Servo(AP_Mount &frontend, AP_Mount_Params &params, bool requires_stab, uint8_t instance):
AP_Mount_Backend(frontend, state, instance), AP_Mount_Backend(frontend, params, instance),
requires_stabilization(requires_stab), requires_stabilization(requires_stab),
_roll_idx(SRV_Channel::k_none), _roll_idx(SRV_Channel::k_none),
_tilt_idx(SRV_Channel::k_none), _tilt_idx(SRV_Channel::k_none),

8
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -10,8 +10,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance), AP_Mount_Backend(frontend, params, instance),
_gimbal() _gimbal()
{} {}
@ -19,7 +19,7 @@ AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_sta
void AP_Mount_SoloGimbal::init() void AP_Mount_SoloGimbal::init()
{ {
_initialised = true; _initialised = true;
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get());
} }
void AP_Mount_SoloGimbal::update_fast() void AP_Mount_SoloGimbal::update_fast()
@ -47,7 +47,7 @@ void AP_Mount_SoloGimbal::update()
// move mount to a neutral position, typically pointing forward // move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: { case MAV_MOUNT_MODE_NEUTRAL: {
_gimbal.set_lockedToBody(false); _gimbal.set_lockedToBody(false);
const Vector3f &target = _state._neutral_angles.get(); const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x); _angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y); _angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z); _angle_rad.yaw = radians(target.z);

2
libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -21,7 +21,7 @@ class AP_Mount_SoloGimbal : public AP_Mount_Backend
public: public:
// Constructor // Constructor
AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount_Params &params, uint8_t instance);
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
void init() override; void init() override;