mirror of https://github.com/ArduPilot/ardupilot
Mount: parent class becomes front-end
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
ee30cb9537
commit
88db50c3a7
|
|
@ -4,31 +4,9 @@
|
||||||
#include <AP_Progmem.h>
|
#include <AP_Progmem.h>
|
||||||
#include <AP_Param.h>
|
#include <AP_Param.h>
|
||||||
#include <AP_Mount.h>
|
#include <AP_Mount.h>
|
||||||
|
#include <AP_Mount_Backend.h>
|
||||||
// Just so that it's completely clear...
|
#include <AP_Mount_Servo.h>
|
||||||
#define ENABLED 1
|
#include <AP_Mount_MAVLink.h>
|
||||||
#define DISABLED 0
|
|
||||||
|
|
||||||
#define MASK_TILT (1<<0)
|
|
||||||
#define MASK_ROLL (1<<1)
|
|
||||||
#define MASK_YAW (1<<2)
|
|
||||||
#define MASK_RETRACT (1<<3)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#if defined( __AVR_ATmega1280__ )
|
|
||||||
# define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount
|
|
||||||
# define MNT_RETRACT_OPTION DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings)
|
|
||||||
# define MNT_GPSPOINT_OPTION ENABLED // Point the mount to a GPS point defined via a mouse click in the Mission Planner GUI
|
|
||||||
# define MNT_STABILIZE_OPTION DISABLED // stabilize camera using frame attitude information
|
|
||||||
# define MNT_MOUNT2_OPTION DISABLED // second mount, can for example be used to keep an antenna pointed at the home position
|
|
||||||
#else
|
|
||||||
# define MNT_JSTICK_SPD_OPTION ENABLED // uses 844 bytes of memory
|
|
||||||
# define MNT_RETRACT_OPTION ENABLED // uses 244 bytes of memory
|
|
||||||
# define MNT_GPSPOINT_OPTION ENABLED // uses 580 bytes of memory
|
|
||||||
# define MNT_STABILIZE_OPTION ENABLED // uses 2424 bytes of memory
|
|
||||||
# define MNT_MOUNT2_OPTION ENABLED // uses 58 bytes of memory (must also be enabled in APM_Config.h)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Param: MODE
|
// @Param: MODE
|
||||||
|
|
@ -36,9 +14,8 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Description: Camera or antenna mount operation mode
|
// @Description: Camera or antenna mount operation mode
|
||||||
// @Values: 0:retract,1:neutral,2:MavLink_targeting,3:RC_targeting,4:GPS_point
|
// @Values: 0:retract,1:neutral,2:MavLink_targeting,3:RC_targeting,4:GPS_point
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("MODE", 0, AP_Mount, _mount_mode, MAV_MOUNT_MODE_RETRACT), // see MAV_MOUNT_MODE at ardupilotmega.h
|
AP_GROUPINFO("MODE", 0, AP_Mount, state[0]._mode, MAV_MOUNT_MODE_RETRACT), // see MAV_MOUNT_MODE at ardupilotmega.h
|
||||||
|
|
||||||
#if MNT_RETRACT_OPTION == ENABLED
|
|
||||||
// @Param: RETRACT_X
|
// @Param: RETRACT_X
|
||||||
// @DisplayName: Mount roll angle when in retracted position
|
// @DisplayName: Mount roll angle when in retracted position
|
||||||
// @Description: Mount roll angle when in retracted position
|
// @Description: Mount roll angle when in retracted position
|
||||||
|
|
@ -62,8 +39,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -180.00 179.99
|
// @Range: -180.00 179.99
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("RETRACT", 1, AP_Mount, _retract_angles, 0),
|
AP_GROUPINFO("RETRACT", 1, AP_Mount, state[0]._retract_angles, 0),
|
||||||
#endif
|
|
||||||
|
|
||||||
// @Param: NEUTRAL_X
|
// @Param: NEUTRAL_X
|
||||||
// @DisplayName: Mount roll angle when in neutral position
|
// @DisplayName: Mount roll angle when in neutral position
|
||||||
|
|
@ -88,59 +64,37 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -180.00 179.99
|
// @Range: -180.00 179.99
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("NEUTRAL", 2, AP_Mount, _neutral_angles, 0),
|
AP_GROUPINFO("NEUTRAL", 2, AP_Mount, state[0]._neutral_angles, 0),
|
||||||
|
|
||||||
// @Param: CONTROL_X
|
// 3 was used for control_angles
|
||||||
// @DisplayName: Mount roll angle command from groundstation
|
|
||||||
// @Description: Mount roll angle when in MavLink or RC control operation mode
|
|
||||||
// @Units: Degrees
|
|
||||||
// @Range: -180.00 179.99
|
|
||||||
// @Increment: 1
|
|
||||||
|
|
||||||
// @Param: CONTROL_Y
|
|
||||||
// @DisplayName: Mount tilt/pitch angle command from groundstation
|
|
||||||
// @Description: Mount tilt/pitch angle when in MavLink or RC control operation mode
|
|
||||||
// @Units: Degrees
|
|
||||||
// @Range: -180.00 179.99
|
|
||||||
// @Increment: 1
|
|
||||||
|
|
||||||
// @Param: CONTROL_Z
|
|
||||||
// @DisplayName: Mount pan/yaw angle command from groundstation
|
|
||||||
// @Description: Mount pan/yaw angle when in MavLink or RC control operation mode
|
|
||||||
// @Units: Degrees
|
|
||||||
// @Range: -180.00 179.99
|
|
||||||
// @Increment: 1
|
|
||||||
AP_GROUPINFO("CONTROL", 3, AP_Mount, _control_angles, 0),
|
|
||||||
|
|
||||||
#if MNT_STABILIZE_OPTION == ENABLED
|
|
||||||
// @Param: STAB_ROLL
|
// @Param: STAB_ROLL
|
||||||
// @DisplayName: Stabilize mount's roll angle
|
// @DisplayName: Stabilize mount's roll angle
|
||||||
// @Description: enable roll stabilisation relative to Earth
|
// @Description: enable roll stabilisation relative to Earth
|
||||||
// @Values: 0:Disabled,1:Enabled
|
// @Values: 0:Disabled,1:Enabled
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll, 0),
|
AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, state[0]._stab_roll, 0),
|
||||||
|
|
||||||
// @Param: STAB_TILT
|
// @Param: STAB_TILT
|
||||||
// @DisplayName: Stabilize mount's pitch/tilt angle
|
// @DisplayName: Stabilize mount's pitch/tilt angle
|
||||||
// @Description: enable tilt/pitch stabilisation relative to Earth
|
// @Description: enable tilt/pitch stabilisation relative to Earth
|
||||||
// @Values: 0:Disabled,1:Enabled
|
// @Values: 0:Disabled,1:Enabled
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("STAB_TILT", 5, AP_Mount, _stab_tilt, 0),
|
AP_GROUPINFO("STAB_TILT", 5, AP_Mount, state[0]._stab_tilt, 0),
|
||||||
|
|
||||||
// @Param: STAB_PAN
|
// @Param: STAB_PAN
|
||||||
// @DisplayName: Stabilize mount pan/yaw angle
|
// @DisplayName: Stabilize mount pan/yaw angle
|
||||||
// @Description: enable pan/yaw stabilisation relative to Earth
|
// @Description: enable pan/yaw stabilisation relative to Earth
|
||||||
// @Values: 0:Disabled,1:Enabled
|
// @Values: 0:Disabled,1:Enabled
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("STAB_PAN", 6, AP_Mount, _stab_pan, 0),
|
AP_GROUPINFO("STAB_PAN", 6, AP_Mount, state[0]._stab_pan, 0),
|
||||||
#endif
|
|
||||||
|
|
||||||
// @Param: RC_IN_ROLL
|
// @Param: RC_IN_ROLL
|
||||||
// @DisplayName: roll RC input channel
|
// @DisplayName: roll RC input channel
|
||||||
// @Description: 0 for none, any other for the RC channel to be used to control roll movements
|
// @Description: 0 for none, any other for the RC channel to be used to control roll movements
|
||||||
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("RC_IN_ROLL", 7, AP_Mount, _roll_rc_in, 0),
|
AP_GROUPINFO("RC_IN_ROLL", 7, AP_Mount, state[0]._roll_rc_in, 0),
|
||||||
|
|
||||||
// @Param: ANGMIN_ROL
|
// @Param: ANGMIN_ROL
|
||||||
// @DisplayName: Minimum roll angle
|
// @DisplayName: Minimum roll angle
|
||||||
|
|
@ -149,7 +103,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMIN_ROL", 8, AP_Mount, _roll_angle_min, -4500),
|
AP_GROUPINFO("ANGMIN_ROL", 8, AP_Mount, state[0]._roll_angle_min, -4500),
|
||||||
|
|
||||||
// @Param: ANGMAX_ROL
|
// @Param: ANGMAX_ROL
|
||||||
// @DisplayName: Maximum roll angle
|
// @DisplayName: Maximum roll angle
|
||||||
|
|
@ -158,14 +112,14 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMAX_ROL", 9, AP_Mount, _roll_angle_max, 4500),
|
AP_GROUPINFO("ANGMAX_ROL", 9, AP_Mount, state[0]._roll_angle_max, 4500),
|
||||||
|
|
||||||
// @Param: RC_IN_TILT
|
// @Param: RC_IN_TILT
|
||||||
// @DisplayName: tilt (pitch) RC input channel
|
// @DisplayName: tilt (pitch) RC input channel
|
||||||
// @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements
|
// @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements
|
||||||
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("RC_IN_TILT", 10, AP_Mount, _tilt_rc_in, 0),
|
AP_GROUPINFO("RC_IN_TILT", 10, AP_Mount, state[0]._tilt_rc_in, 0),
|
||||||
|
|
||||||
// @Param: ANGMIN_TIL
|
// @Param: ANGMIN_TIL
|
||||||
// @DisplayName: Minimum tilt angle
|
// @DisplayName: Minimum tilt angle
|
||||||
|
|
@ -174,7 +128,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMIN_TIL", 11, AP_Mount, _tilt_angle_min, -4500),
|
AP_GROUPINFO("ANGMIN_TIL", 11, AP_Mount, state[0]._tilt_angle_min, -4500),
|
||||||
|
|
||||||
// @Param: ANGMAX_TIL
|
// @Param: ANGMAX_TIL
|
||||||
// @DisplayName: Maximum tilt angle
|
// @DisplayName: Maximum tilt angle
|
||||||
|
|
@ -183,14 +137,14 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMAX_TIL", 12, AP_Mount, _tilt_angle_max, 4500),
|
AP_GROUPINFO("ANGMAX_TIL", 12, AP_Mount, state[0]._tilt_angle_max, 4500),
|
||||||
|
|
||||||
// @Param: RC_IN_PAN
|
// @Param: RC_IN_PAN
|
||||||
// @DisplayName: pan (yaw) RC input channel
|
// @DisplayName: pan (yaw) RC input channel
|
||||||
// @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements
|
// @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements
|
||||||
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("RC_IN_PAN", 13, AP_Mount, _pan_rc_in, 0),
|
AP_GROUPINFO("RC_IN_PAN", 13, AP_Mount, state[0]._pan_rc_in, 0),
|
||||||
|
|
||||||
// @Param: ANGMIN_PAN
|
// @Param: ANGMIN_PAN
|
||||||
// @DisplayName: Minimum pan angle
|
// @DisplayName: Minimum pan angle
|
||||||
|
|
@ -199,7 +153,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMIN_PAN", 14, AP_Mount, _pan_angle_min, -4500),
|
AP_GROUPINFO("ANGMIN_PAN", 14, AP_Mount, state[0]._pan_angle_min, -4500),
|
||||||
|
|
||||||
// @Param: ANGMAX_PAN
|
// @Param: ANGMAX_PAN
|
||||||
// @DisplayName: Maximum pan angle
|
// @DisplayName: Maximum pan angle
|
||||||
|
|
@ -208,9 +162,8 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: -18000 17999
|
// @Range: -18000 17999
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("ANGMAX_PAN", 15, AP_Mount, _pan_angle_max, 4500),
|
AP_GROUPINFO("ANGMAX_PAN", 15, AP_Mount, state[0]._pan_angle_max, 4500),
|
||||||
|
|
||||||
#if MNT_JSTICK_SPD_OPTION == ENABLED
|
|
||||||
// @Param: JSTICK_SPD
|
// @Param: JSTICK_SPD
|
||||||
// @DisplayName: mount joystick speed
|
// @DisplayName: mount joystick speed
|
||||||
// @Description: 0 for position control, small for low speeds, 100 for max speed. A good general value is 10 which gives a movement speed of 3 degrees per second.
|
// @Description: 0 for position control, small for low speeds, 100 for max speed. A good general value is 10 which gives a movement speed of 3 degrees per second.
|
||||||
|
|
@ -218,7 +171,6 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0),
|
AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0),
|
||||||
#endif
|
|
||||||
|
|
||||||
// @Param: LEAD_RLL
|
// @Param: LEAD_RLL
|
||||||
// @DisplayName: Roll stabilization lead time
|
// @DisplayName: Roll stabilization lead time
|
||||||
|
|
@ -227,7 +179,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: 0.0 0.2
|
// @Range: 0.0 0.2
|
||||||
// @Increment: .005
|
// @Increment: .005
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("LEAD_RLL", 17, AP_Mount, _roll_stb_lead, 0.0f),
|
AP_GROUPINFO("LEAD_RLL", 17, AP_Mount, state[0]._roll_stb_lead, 0.0f),
|
||||||
|
|
||||||
// @Param: LEAD_PTCH
|
// @Param: LEAD_PTCH
|
||||||
// @DisplayName: Pitch stabilization lead time
|
// @DisplayName: Pitch stabilization lead time
|
||||||
|
|
@ -236,491 +188,168 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||||
// @Range: 0.0 0.2
|
// @Range: 0.0 0.2
|
||||||
// @Increment: .005
|
// @Increment: .005
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("LEAD_PTCH", 18, AP_Mount, _pitch_stb_lead, 0.0f),
|
AP_GROUPINFO("LEAD_PTCH", 18, AP_Mount, state[0]._pitch_stb_lead, 0.0f),
|
||||||
|
|
||||||
|
// @Param: TYPE
|
||||||
|
// @DisplayName: Mount Type
|
||||||
|
// @Description: Mount Type (None, Servo or MAVLink)
|
||||||
|
// @Values: 0:None, 1:Servo, 2:MAVLink
|
||||||
|
// @User: Standard
|
||||||
|
AP_GROUPINFO("TYPE", 19, AP_Mount, state[0]._type, 0),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
AP_Mount::AP_Mount(const struct Location *current_loc, const AP_AHRS &ahrs, uint8_t id) :
|
AP_Mount::AP_Mount(const AP_AHRS &ahrs, const struct Location ¤t_loc) :
|
||||||
_ahrs(ahrs),
|
_ahrs(ahrs),
|
||||||
_roll_control_angle(0.0f),
|
_current_loc(current_loc),
|
||||||
_tilt_control_angle(0.0f),
|
_num_instances(0),
|
||||||
_pan_control_angle(0.0f),
|
_primary(0)
|
||||||
_roll_angle(0.0f),
|
|
||||||
_tilt_angle(0.0f),
|
|
||||||
_pan_angle(0.0f)
|
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
_current_loc = current_loc;
|
|
||||||
|
|
||||||
// default to zero angles
|
// initialise backend status
|
||||||
_retract_angles = Vector3f(0,0,0);
|
for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
|
||||||
_neutral_angles = Vector3f(0,0,0);
|
_backends[i] = NULL;
|
||||||
_control_angles = Vector3f(0,0,0);
|
|
||||||
|
|
||||||
// default unknown mount type
|
|
||||||
_mount_type = k_unknown;
|
|
||||||
|
|
||||||
#if MNT_MOUNT2_OPTION == ENABLED
|
|
||||||
if (id == 0) {
|
|
||||||
#endif
|
|
||||||
_roll_idx = RC_Channel_aux::k_mount_roll;
|
|
||||||
_tilt_idx = RC_Channel_aux::k_mount_tilt;
|
|
||||||
_pan_idx = RC_Channel_aux::k_mount_pan;
|
|
||||||
_open_idx = RC_Channel_aux::k_mount_open;
|
|
||||||
#if MNT_MOUNT2_OPTION == ENABLED
|
|
||||||
} else {
|
|
||||||
_roll_idx = RC_Channel_aux::k_mount2_roll;
|
|
||||||
_tilt_idx = RC_Channel_aux::k_mount2_tilt;
|
|
||||||
_pan_idx = RC_Channel_aux::k_mount2_pan;
|
|
||||||
_open_idx = RC_Channel_aux::k_mount2_open;
|
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
// init - detect and initialise all mounts
|
||||||
void
|
void AP_Mount::init()
|
||||||
AP_Mount::update_mount_type()
|
|
||||||
{
|
{
|
||||||
bool have_roll, have_tilt, have_pan;
|
// check init has not been called before
|
||||||
|
if (_num_instances != 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
have_roll = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_roll) ||
|
// create each instance
|
||||||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_roll);
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
||||||
|
MountType mount_type = get_mount_type(instance);
|
||||||
|
|
||||||
have_tilt = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_tilt) ||
|
// check for servo mounts
|
||||||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_tilt);
|
if (mount_type == Mount_Type_Servo) {
|
||||||
|
_backends[instance] = new AP_Mount_Servo(*this, instance);
|
||||||
|
_num_instances++;
|
||||||
|
|
||||||
have_pan = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_pan) ||
|
// check for MAVLink mounts
|
||||||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_pan);
|
} else if (mount_type == Mount_Type_MAVLink) {
|
||||||
|
_backends[instance] = new AP_Mount_MAVLink(*this, instance);
|
||||||
|
_num_instances++;
|
||||||
|
}
|
||||||
|
|
||||||
mount_axis_mask = 0;
|
// init new instance
|
||||||
|
if (_backends[instance] != NULL) {
|
||||||
if (have_pan)
|
_backends[instance]->init();
|
||||||
mount_axis_mask |= MASK_YAW;
|
}
|
||||||
if (have_tilt)
|
}
|
||||||
mount_axis_mask |= MASK_TILT;
|
|
||||||
if (have_roll)
|
|
||||||
mount_axis_mask |= MASK_ROLL;
|
|
||||||
|
|
||||||
if (have_pan && have_tilt && !have_roll)
|
|
||||||
_mount_type = k_pan_tilt;
|
|
||||||
if (!have_pan && have_tilt && have_roll)
|
|
||||||
_mount_type = k_tilt_roll;
|
|
||||||
if (have_pan && have_tilt && have_roll)
|
|
||||||
_mount_type = k_pan_tilt_roll;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// sets the servo angles for retraction, note angles are in degrees
|
// update - give mount opportunity to update servos. should be called at 10hz or higher
|
||||||
void AP_Mount::set_retract_angles(float roll, float tilt, float pan)
|
void AP_Mount::update()
|
||||||
{
|
{
|
||||||
_retract_angles = Vector3f(roll, tilt, pan);
|
// update each instance
|
||||||
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
||||||
|
if (_backends[instance] != NULL) {
|
||||||
|
_backends[instance]->update();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//sets the servo angles for neutral, note angles are in degrees
|
// get_mount_type - returns the type of mount
|
||||||
void AP_Mount::set_neutral_angles(float roll, float tilt, float pan)
|
AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const
|
||||||
{
|
{
|
||||||
_neutral_angles = Vector3f(roll, tilt, pan);
|
if (instance >= AP_MOUNT_MAX_INSTANCES) {
|
||||||
|
return Mount_Type_None;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (MountType)state[instance]._type.get();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// sets the servo angles for MAVLink, note angles are in degrees
|
// has_pan_control - returns true if the mount has yaw control (required for copters)
|
||||||
void AP_Mount::set_control_angles(float roll, float tilt, float pan)
|
bool AP_Mount::has_pan_control(uint8_t instance) const
|
||||||
{
|
{
|
||||||
_control_angles = Vector3f(roll, tilt, pan);
|
if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ask backend if it support pan
|
||||||
|
return _backends[instance]->has_pan_control();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// used to tell the mount to track GPS location
|
// get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
|
||||||
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
|
MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const
|
||||||
{
|
{
|
||||||
_target_GPS_location=targetGPSLocation;
|
// sanity check instance
|
||||||
|
if (instance >= AP_MOUNT_MAX_INSTANCES) {
|
||||||
|
return MAV_MOUNT_MODE_RETRACT;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (enum MAV_MOUNT_MODE)state[instance]._mode.get();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// This one should be called periodically
|
// set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter
|
||||||
void AP_Mount::update_mount_position()
|
// this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
|
||||||
|
void AP_Mount::set_mode_to_default(uint8_t instance)
|
||||||
{
|
{
|
||||||
#if MNT_RETRACT_OPTION == ENABLED
|
// sanity check instance
|
||||||
static bool mount_open = 0; // 0 is closed
|
if (instance >= AP_MOUNT_MAX_INSTANCES) {
|
||||||
#endif
|
return;
|
||||||
|
|
||||||
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
|
|
||||||
{
|
|
||||||
#if MNT_RETRACT_OPTION == ENABLED
|
|
||||||
// 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:
|
|
||||||
{
|
|
||||||
Vector3f vec = _retract_angles.get();
|
|
||||||
_roll_angle = vec.x;
|
|
||||||
_tilt_angle = vec.y;
|
|
||||||
_pan_angle = vec.z;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// move mount to a neutral position, typically pointing forward
|
|
||||||
case MAV_MOUNT_MODE_NEUTRAL:
|
|
||||||
{
|
|
||||||
Vector3f vec = _neutral_angles.get();
|
|
||||||
_roll_angle = vec.x;
|
|
||||||
_tilt_angle = vec.y;
|
|
||||||
_pan_angle = vec.z;
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// point to the angles given by a mavlink message
|
// load instance's state from eeprom
|
||||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
|
state[instance]._mode.load();
|
||||||
{
|
|
||||||
Vector3f vec = _control_angles.get();
|
|
||||||
_roll_control_angle = radians(vec.x);
|
|
||||||
_tilt_control_angle = radians(vec.y);
|
|
||||||
_pan_control_angle = radians(vec.z);
|
|
||||||
stabilize();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// RC radio manual angle control, but with stabilization from the AHRS
|
|
||||||
case MAV_MOUNT_MODE_RC_TARGETING:
|
|
||||||
{
|
|
||||||
#define rc_ch(i) RC_Channel::rc_channel(i-1)
|
|
||||||
#if MNT_JSTICK_SPD_OPTION == ENABLED
|
|
||||||
if (_joystick_speed) { // for spring loaded joysticks
|
|
||||||
// allow pilot speed position input to come directly from an RC_Channel
|
|
||||||
if (_roll_rc_in && rc_ch(_roll_rc_in)) {
|
|
||||||
_roll_control_angle += rc_ch(_roll_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
|
|
||||||
if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f);
|
|
||||||
if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f);
|
|
||||||
}
|
|
||||||
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
|
|
||||||
_tilt_control_angle += rc_ch(_tilt_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
|
|
||||||
if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f);
|
|
||||||
if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f);
|
|
||||||
}
|
|
||||||
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
|
|
||||||
_pan_control_angle += rc_ch(_pan_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
|
|
||||||
if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f);
|
|
||||||
if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
#endif
|
|
||||||
// allow pilot position input to come directly from an RC_Channel
|
|
||||||
if (_roll_rc_in && (rc_ch(_roll_rc_in))) {
|
|
||||||
_roll_control_angle = angle_input_rad(rc_ch(_roll_rc_in), _roll_angle_min, _roll_angle_max);
|
|
||||||
}
|
|
||||||
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
|
|
||||||
_tilt_control_angle = angle_input_rad(rc_ch(_tilt_rc_in), _tilt_angle_min, _tilt_angle_max);
|
|
||||||
}
|
|
||||||
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
|
|
||||||
_pan_control_angle = angle_input_rad(rc_ch(_pan_rc_in), _pan_angle_min, _pan_angle_max);
|
|
||||||
}
|
|
||||||
#if MNT_JSTICK_SPD_OPTION == ENABLED
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
stabilize();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if MNT_GPSPOINT_OPTION == ENABLED
|
|
||||||
// point mount to a GPS point given by the mission planner
|
|
||||||
case MAV_MOUNT_MODE_GPS_POINT:
|
|
||||||
{
|
|
||||||
if(_ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
|
|
||||||
calc_GPS_target_angle(&_target_GPS_location);
|
|
||||||
stabilize();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
default:
|
|
||||||
//do nothing
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if MNT_RETRACT_OPTION == ENABLED
|
|
||||||
// move mount to a "retracted position" into the fuselage with a fourth servo
|
|
||||||
bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1;
|
|
||||||
if (mount_open != mount_open_new) {
|
|
||||||
mount_open = mount_open_new;
|
|
||||||
move_servo(_open_idx, mount_open_new, 0, 1);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// write the results to the servos
|
|
||||||
move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1f, _roll_angle_max*0.1f);
|
|
||||||
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f);
|
|
||||||
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1f, _pan_angle_max*0.1f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
|
// set_mode - sets mount's mode
|
||||||
|
void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode)
|
||||||
{
|
{
|
||||||
_mount_mode = (int8_t)mode;
|
// sanity check instance
|
||||||
|
if (instance < AP_MOUNT_MAX_INSTANCES) {
|
||||||
|
state[instance]._mode = (int8_t)mode;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Change the configuration of the mount
|
/// Change the configuration of the mount
|
||||||
/// triggered by a MavLink packet.
|
/// triggered by a MavLink packet.
|
||||||
void AP_Mount::configure_msg(mavlink_message_t* msg)
|
void AP_Mount::configure_msg(uint8_t instance, mavlink_message_t* msg)
|
||||||
{
|
{
|
||||||
__mavlink_mount_configure_t packet;
|
if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
|
||||||
mavlink_msg_mount_configure_decode(msg, &packet);
|
return;
|
||||||
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
|
}
|
||||||
_stab_roll = packet.stab_roll;
|
|
||||||
_stab_tilt = packet.stab_pitch;
|
// send message to backend
|
||||||
_stab_pan = packet.stab_yaw;
|
_backends[instance]->configure_msg(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Control the mount (depends on the previously set mount configuration)
|
/// Control the mount (depends on the previously set mount configuration)
|
||||||
/// triggered by a MavLink packet.
|
/// triggered by a MavLink packet.
|
||||||
void AP_Mount::control_msg(mavlink_message_t *msg)
|
void AP_Mount::control_msg(uint8_t instance, mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
__mavlink_mount_control_t packet;
|
if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == NULL) {
|
||||||
mavlink_msg_mount_control_decode(msg, &packet);
|
return;
|
||||||
|
|
||||||
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
|
|
||||||
{
|
|
||||||
#if MNT_RETRACT_OPTION == ENABLED
|
|
||||||
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
|
|
||||||
set_retract_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
|
|
||||||
if (packet.save_position)
|
|
||||||
{
|
|
||||||
_retract_angles.save();
|
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
|
// send message to backend
|
||||||
set_neutral_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
|
_backends[instance]->control_msg(msg);
|
||||||
if (packet.save_position)
|
|
||||||
{
|
|
||||||
_neutral_angles.save();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
|
|
||||||
set_control_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
|
|
||||||
{
|
|
||||||
Vector3f vec = _neutral_angles.get();
|
|
||||||
_roll_angle = vec.x;
|
|
||||||
_tilt_angle = vec.y;
|
|
||||||
_pan_angle = vec.z;
|
|
||||||
// also reset control angles, in radians. This allows a
|
|
||||||
// MOUNT_CONTROL message to be used to reset the mount to zero
|
|
||||||
// offset when in joystick speed relative mode
|
|
||||||
_roll_control_angle = radians(_roll_angle);
|
|
||||||
_tilt_control_angle = radians(_tilt_angle);
|
|
||||||
_pan_control_angle = radians(_pan_angle);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
#if MNT_GPSPOINT_OPTION == ENABLED
|
|
||||||
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
|
|
||||||
Location targetGPSLocation;
|
|
||||||
targetGPSLocation.lat = packet.input_a;
|
|
||||||
targetGPSLocation.lng = packet.input_b;
|
|
||||||
targetGPSLocation.alt = packet.input_c;
|
|
||||||
set_GPS_target_location(targetGPSLocation);
|
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
case MAV_MOUNT_MODE_ENUM_END:
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
// do nothing
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Return mount status information
|
/// Return mount status information
|
||||||
void AP_Mount::status_msg(mavlink_channel_t chan)
|
void AP_Mount::status_msg(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
mavlink_msg_mount_status_send(chan,0,0, _tilt_angle*100, _roll_angle*100, _pan_angle*100);
|
// call status_msg for each instance
|
||||||
}
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
||||||
|
if (_backends[instance] != NULL) {
|
||||||
/// Set mount point/region of interest, triggered by mission script commands
|
_backends[instance]->status_msg(chan);
|
||||||
void AP_Mount::set_roi_cmd(const struct Location *target_loc)
|
|
||||||
{
|
|
||||||
#if MNT_GPSPOINT_OPTION == ENABLED
|
|
||||||
// set the target gps location
|
|
||||||
_target_GPS_location = *target_loc;
|
|
||||||
|
|
||||||
// set the mode to GPS tracking mode
|
|
||||||
set_mode(MAV_MOUNT_MODE_GPS_POINT);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Set mount configuration, triggered by mission script commands
|
|
||||||
void AP_Mount::configure_cmd()
|
|
||||||
{
|
|
||||||
// TODO get the information out of the mission command and use it
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Control the mount (depends on the previously set mount configuration), triggered by mission script commands
|
|
||||||
void AP_Mount::control_cmd()
|
|
||||||
{
|
|
||||||
// TODO get the information out of the mission command and use it
|
|
||||||
}
|
|
||||||
|
|
||||||
/// returns the angle (degrees*100) that the RC_Channel input is receiving
|
|
||||||
int32_t
|
|
||||||
AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
|
|
||||||
{
|
|
||||||
return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// returns the angle (radians) that the RC_Channel input is receiving
|
|
||||||
float
|
|
||||||
AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
|
|
||||||
{
|
|
||||||
return radians(angle_input(rc, angle_min, angle_max)*0.01f);
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
AP_Mount::calc_GPS_target_angle(const struct Location *target)
|
|
||||||
{
|
|
||||||
float GPS_vector_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f;
|
|
||||||
float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f;
|
|
||||||
float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
|
|
||||||
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
|
|
||||||
_roll_control_angle = 0;
|
|
||||||
|
|
||||||
if (mount_axis_mask & MASK_TILT) {
|
|
||||||
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
|
|
||||||
} else {
|
|
||||||
_tilt_control_angle = 0;
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (mount_axis_mask & MASK_YAW) {
|
// set_roi_target - sets target location that mount should attempt to point towards
|
||||||
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
|
void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_loc)
|
||||||
} else {
|
{
|
||||||
_pan_control_angle = 0;
|
// call instance's set_roi_cmd
|
||||||
|
if (instance < AP_MOUNT_MAX_INSTANCES && _backends[instance] != NULL) {
|
||||||
|
_backends[instance]->set_roi_target(target_loc);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Stabilizes mount relative to the Earth's frame
|
|
||||||
/// Inputs:
|
|
||||||
/// _roll_control_angle desired roll angle in radians,
|
|
||||||
/// _tilt_control_angle desired tilt/pitch angle in radians,
|
|
||||||
/// _pan_control_angle desired pan/yaw angle in radians
|
|
||||||
/// Outputs:
|
|
||||||
/// _roll_angle stabilized roll angle in degrees,
|
|
||||||
/// _tilt_angle stabilized tilt/pitch angle in degrees,
|
|
||||||
/// _pan_angle stabilized pan/yaw angle in degrees
|
|
||||||
void
|
|
||||||
AP_Mount::stabilize()
|
|
||||||
{
|
|
||||||
#if MNT_STABILIZE_OPTION == ENABLED
|
|
||||||
// only do the full 3D frame transform if we are doing pan control
|
|
||||||
if (_stab_pan) {
|
|
||||||
Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs
|
|
||||||
Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input.
|
|
||||||
Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
|
|
||||||
m = _ahrs.get_dcm_matrix();
|
|
||||||
m.transpose();
|
|
||||||
cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle);
|
|
||||||
gimbal_target = m * cam;
|
|
||||||
gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle);
|
|
||||||
_roll_angle = _stab_roll ? degrees(_roll_angle) : degrees(_roll_control_angle);
|
|
||||||
_tilt_angle = _stab_tilt ? degrees(_tilt_angle) : degrees(_tilt_control_angle);
|
|
||||||
_pan_angle = degrees(_pan_angle);
|
|
||||||
} else {
|
|
||||||
// otherwise base mount roll and tilt on the ahrs
|
|
||||||
// roll/tilt attitude, plus any requested angle
|
|
||||||
_roll_angle = degrees(_roll_control_angle);
|
|
||||||
_tilt_angle = degrees(_tilt_control_angle);
|
|
||||||
_pan_angle = degrees(_pan_control_angle);
|
|
||||||
if (_stab_roll) {
|
|
||||||
_roll_angle -= degrees(_ahrs.roll);
|
|
||||||
}
|
|
||||||
if (_stab_tilt) {
|
|
||||||
_tilt_angle -= degrees(_ahrs.pitch);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Add lead filter.
|
|
||||||
const Vector3f &gyro = _ahrs.get_gyro();
|
|
||||||
|
|
||||||
if (_stab_roll && _roll_stb_lead != 0.0f && 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());
|
|
||||||
_roll_angle -= degrees(roll_rate) * _roll_stb_lead;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_stab_tilt && _pitch_stb_lead != 0.0f) {
|
|
||||||
// Compute rate of change of euler pitch angle
|
|
||||||
float pitch_rate = _ahrs.cos_pitch() * gyro.y - _ahrs.sin_roll() * gyro.z;
|
|
||||||
_tilt_angle -= degrees(pitch_rate) * _pitch_stb_lead;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
/*
|
|
||||||
* /// For testing and development. Called in the medium loop.
|
|
||||||
* void
|
|
||||||
* AP_Mount::debug_output()
|
|
||||||
* { Serial3.print("current - ");
|
|
||||||
* Serial3.print("lat ");
|
|
||||||
* Serial3.print(_current_loc->lat);
|
|
||||||
* Serial3.print(",lon ");
|
|
||||||
* Serial3.print(_current_loc->lng);
|
|
||||||
* Serial3.print(",alt ");
|
|
||||||
* Serial3.println(_current_loc->alt);
|
|
||||||
*
|
|
||||||
* Serial3.print("gps - ");
|
|
||||||
* Serial3.print("lat ");
|
|
||||||
* Serial3.print(_gps->latitude);
|
|
||||||
* Serial3.print(",lon ");
|
|
||||||
* Serial3.print(_gps->longitude);
|
|
||||||
* Serial3.print(",alt ");
|
|
||||||
* Serial3.print(_gps->altitude);
|
|
||||||
* Serial3.println();
|
|
||||||
*
|
|
||||||
* Serial3.print("target - ");
|
|
||||||
* Serial3.print("lat ");
|
|
||||||
* Serial3.print(_target_GPS_location.lat);
|
|
||||||
* Serial3.print(",lon ");
|
|
||||||
* Serial3.print(_target_GPS_location.lng);
|
|
||||||
* Serial3.print(",alt ");
|
|
||||||
* Serial3.print(_target_GPS_location.alt);
|
|
||||||
* Serial3.print(" hdg to targ ");
|
|
||||||
* Serial3.print(degrees(_pan_control_angle));
|
|
||||||
* Serial3.println();
|
|
||||||
* }
|
|
||||||
*/
|
|
||||||
/// saturate to the closest angle limit if outside of [min max] angle interval
|
|
||||||
/// input angle is in degrees * 10
|
|
||||||
int16_t
|
|
||||||
AP_Mount::closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max)
|
|
||||||
{
|
|
||||||
// Make sure the angle lies in the interval [-180 .. 180[ degrees
|
|
||||||
while (angle < -1800) angle += 3600;
|
|
||||||
while (angle >= 1800) angle -= 3600;
|
|
||||||
|
|
||||||
// Make sure the angle limits lie in the interval [-180 .. 180[ degrees
|
|
||||||
while (*angle_min < -1800) *angle_min += 3600;
|
|
||||||
while (*angle_min >= 1800) *angle_min -= 3600;
|
|
||||||
while (*angle_max < -1800) *angle_max += 3600;
|
|
||||||
while (*angle_max >= 1800) *angle_max -= 3600;
|
|
||||||
// TODO call this function somehow, otherwise this will never work
|
|
||||||
//set_range(min, max);
|
|
||||||
|
|
||||||
// If the angle is outside servo limits, saturate the angle to the closest limit
|
|
||||||
// On a circle the closest angular position must be carefully calculated to account for wrap-around
|
|
||||||
if ((angle < *angle_min) && (angle > *angle_max)) {
|
|
||||||
// angle error if min limit is used
|
|
||||||
int16_t err_min = *angle_min - angle + (angle<*angle_min ? 0 : 3600); // add 360 degrees if on the "wrong side"
|
|
||||||
// angle error if max limit is used
|
|
||||||
int16_t err_max = angle - *angle_max + (angle>*angle_max ? 0 : 3600); // add 360 degrees if on the "wrong side"
|
|
||||||
angle = err_min<err_max ? *angle_min : *angle_max;
|
|
||||||
}
|
|
||||||
|
|
||||||
return angle;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// all angles are degrees * 10 units
|
|
||||||
void
|
|
||||||
AP_Mount::move_servo(uint8_t function_idx, int16_t angle, int16_t angle_min, int16_t angle_max)
|
|
||||||
{
|
|
||||||
// saturate to the closest angle limit if outside of [min max] angle interval
|
|
||||||
int16_t servo_out = closest_limit(angle, &angle_min, &angle_max);
|
|
||||||
RC_Channel_aux::move_servo((RC_Channel_aux::Aux_servo_function_t)function_idx, servo_out, angle_min, angle_max);
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -7,6 +7,7 @@
|
||||||
* Ritchie Wilson; *
|
* Ritchie Wilson; *
|
||||||
* Amilcar Lucas; *
|
* Amilcar Lucas; *
|
||||||
* Gregory Fletcher; *
|
* Gregory Fletcher; *
|
||||||
|
* heavily modified by Randy Mackay *
|
||||||
* *
|
* *
|
||||||
* Purpose: Move a 2 or 3 axis mount attached to vehicle, *
|
* Purpose: Move a 2 or 3 axis mount attached to vehicle, *
|
||||||
* Used for mount to track targets or stabilise *
|
* Used for mount to track targets or stabilise *
|
||||||
|
|
@ -28,112 +29,125 @@
|
||||||
#include <GCS_MAVLink.h>
|
#include <GCS_MAVLink.h>
|
||||||
#include <RC_Channel.h>
|
#include <RC_Channel.h>
|
||||||
|
|
||||||
|
// maximum number of mounts
|
||||||
|
#define AP_MOUNT_MAX_INSTANCES 1
|
||||||
|
|
||||||
|
// declare backend classes
|
||||||
|
class AP_Mount_Backend;
|
||||||
|
class AP_Mount_Servo;
|
||||||
|
class AP_Mount_MAVLink;
|
||||||
|
|
||||||
class AP_Mount
|
class AP_Mount
|
||||||
{
|
{
|
||||||
public:
|
// declare backends as friends
|
||||||
//Constructor
|
friend class AP_Mount_Backend;
|
||||||
AP_Mount(const struct Location *current_loc, const AP_AHRS &ahrs, uint8_t id);
|
friend class AP_Mount_Servo;
|
||||||
|
friend class AP_Mount_MAVLink;
|
||||||
|
|
||||||
//enums
|
public:
|
||||||
|
|
||||||
|
// Enums
|
||||||
enum MountType {
|
enum MountType {
|
||||||
k_unknown = 0, ///< unknown type
|
Mount_Type_None = 0, /// no mount
|
||||||
k_pan_tilt = 1, ///< yaw-pitch
|
Mount_Type_Servo = 1, /// servo controlled mount
|
||||||
k_tilt_roll = 2, ///< pitch-roll
|
Mount_Type_MAVLink = 2, /// MAVLink controlled mount
|
||||||
k_pan_tilt_roll = 3, ///< yaw-pitch-roll
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// get_mode - return current mount mode
|
// Constructor
|
||||||
enum MAV_MOUNT_MODE get_mode() const { return (enum MAV_MOUNT_MODE)_mount_mode.get(); }
|
AP_Mount(const AP_AHRS &ahrs, const struct Location ¤t_loc);
|
||||||
|
|
||||||
// set_mode_to_default - restores the mode to it's default held in the MNT_MODE parameter
|
// init - detect and initialise all mounts
|
||||||
|
void init();
|
||||||
|
|
||||||
|
// update - give mount opportunity to update servos. should be called at 10hz or higher
|
||||||
|
void update();
|
||||||
|
|
||||||
|
// _num_instances - returns number of mounts
|
||||||
|
uint8_t num_instances() { return _num_instances; }
|
||||||
|
|
||||||
|
// get_mount_type - returns the type of mount
|
||||||
|
AP_Mount::MountType get_mount_type() const { return get_mount_type(_primary); }
|
||||||
|
AP_Mount::MountType get_mount_type(uint8_t instance) const;
|
||||||
|
|
||||||
|
// has_pan_control - returns true if the mount has yaw control (required for copters)
|
||||||
|
bool has_pan_control() const { return has_pan_control(_primary); }
|
||||||
|
bool has_pan_control(uint8_t instance) const;
|
||||||
|
|
||||||
|
// get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
|
||||||
|
enum MAV_MOUNT_MODE get_mode() const { return get_mode(_primary); }
|
||||||
|
enum MAV_MOUNT_MODE get_mode(uint8_t instance) const;
|
||||||
|
|
||||||
|
// set_mode - sets mount's mode
|
||||||
|
void set_mode(enum MAV_MOUNT_MODE mode) { set_mode(_primary, 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_MODE parameter
|
||||||
// this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
|
// this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
|
||||||
void set_mode_to_default() { _mount_mode.load(); }
|
void set_mode_to_default() { set_mode_to_default(_primary); }
|
||||||
|
void set_mode_to_default(uint8_t instance);
|
||||||
|
|
||||||
// MAVLink methods
|
// set_roi_target - sets target location that mount should attempt to point towards
|
||||||
void configure_msg(mavlink_message_t* msg);
|
void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); }
|
||||||
void control_msg(mavlink_message_t* msg);
|
void set_roi_target(uint8_t instance, const struct Location &target_loc);
|
||||||
|
|
||||||
|
// configure_msg - process MOUNT_CONFIGURE messages received from GCS
|
||||||
|
void configure_msg(mavlink_message_t* msg) { configure_msg(_primary, msg); }
|
||||||
|
void configure_msg(uint8_t instance, mavlink_message_t* msg);
|
||||||
|
|
||||||
|
// control_msg - process MOUNT_CONTROL messages received from GCS
|
||||||
|
void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); }
|
||||||
|
void control_msg(uint8_t instance, mavlink_message_t* msg);
|
||||||
|
|
||||||
|
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
|
||||||
void status_msg(mavlink_channel_t chan);
|
void status_msg(mavlink_channel_t chan);
|
||||||
void set_roi_cmd(const struct Location *target_loc);
|
|
||||||
void configure_cmd();
|
|
||||||
void control_cmd();
|
|
||||||
|
|
||||||
// should be called periodically
|
// parameter var table
|
||||||
void update_mount_position();
|
|
||||||
void update_mount_type(); ///< Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
|
||||||
void debug_output(); ///< For testing and development. Called in the medium loop.
|
|
||||||
// Accessors
|
|
||||||
enum MountType get_mount_type() {
|
|
||||||
return _mount_type;
|
|
||||||
}
|
|
||||||
// hook for eeprom variables
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
void set_mode(enum MAV_MOUNT_MODE mode);
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
//methods
|
// private members
|
||||||
|
const AP_AHRS &_ahrs;
|
||||||
|
const struct Location &_current_loc; // reference to the vehicle's current location
|
||||||
|
|
||||||
void set_retract_angles(float roll, float tilt, float pan); ///< set mount retracted position
|
// frontend parameters
|
||||||
void set_neutral_angles(float roll, float tilt, float pan);
|
AP_Int8 _joystick_speed; // joystick gain
|
||||||
void set_control_angles(float roll, float tilt, float pan);
|
|
||||||
void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location
|
|
||||||
|
|
||||||
// internal methods
|
// front end members
|
||||||
void calc_GPS_target_angle(const struct Location *target);
|
uint8_t _num_instances; // number of mounts instantiated
|
||||||
void stabilize();
|
uint8_t _primary; // primary mount
|
||||||
int16_t closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max);
|
AP_Mount_Backend *_backends[AP_MOUNT_MAX_INSTANCES]; // pointers to instantiated mounts
|
||||||
void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);
|
|
||||||
int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
|
||||||
float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
|
||||||
|
|
||||||
//members
|
// backend state including parameters
|
||||||
const AP_AHRS &_ahrs; ///< Rotation matrix from earth to plane.
|
struct mount_state {
|
||||||
const struct Location * _current_loc;
|
// Parameters
|
||||||
struct Location _target_GPS_location;
|
AP_Int8 _type; // mount type (None, Servo or MAVLink, see MountType enum)
|
||||||
MountType _mount_type;
|
AP_Int8 _mode; // Retracted, Neutral, RC_Targeting, GPS Point
|
||||||
|
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
|
||||||
|
|
||||||
uint8_t _roll_idx; ///< RC_Channel_aux mount roll function index
|
// RC input channels from receiver used for direct angular input from pilot
|
||||||
uint8_t _tilt_idx; ///< RC_Channel_aux mount tilt function index
|
AP_Int8 _roll_rc_in; // pilot provides roll input on this channel
|
||||||
uint8_t _pan_idx; ///< RC_Channel_aux mount pan function index
|
AP_Int8 _tilt_rc_in; // pilot provides tilt input on this channel
|
||||||
uint8_t _open_idx; ///< RC_Channel_aux mount open function index
|
AP_Int8 _pan_rc_in; // pilot provides pan input on this channel
|
||||||
|
|
||||||
uint8_t mount_axis_mask; // used to track user input on each axis
|
// 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
|
||||||
|
|
||||||
float _roll_control_angle; ///< radians
|
AP_Vector3f _retract_angles; // retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
||||||
float _tilt_control_angle; ///< radians
|
AP_Vector3f _neutral_angles; // neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
||||||
float _pan_control_angle; ///< radians
|
|
||||||
|
|
||||||
float _roll_angle; ///< degrees
|
AP_Float _roll_stb_lead; // roll lead control gain
|
||||||
float _tilt_angle; ///< degrees
|
AP_Float _pitch_stb_lead; // pitch lead control gain
|
||||||
float _pan_angle; ///< degrees
|
|
||||||
|
|
||||||
// EEPROM parameters
|
struct Location _roi_target; // roi target location
|
||||||
AP_Int8 _stab_roll; ///< (1 = yes, 0 = no)
|
} state[AP_MOUNT_MAX_INSTANCES];
|
||||||
AP_Int8 _stab_tilt; ///< (1 = yes, 0 = no)
|
|
||||||
AP_Int8 _stab_pan; ///< (1 = yes, 0 = no)
|
|
||||||
|
|
||||||
AP_Int8 _mount_mode;
|
|
||||||
// RC_Channel for providing direct angular input from pilot
|
|
||||||
AP_Int8 _roll_rc_in;
|
|
||||||
AP_Int8 _tilt_rc_in;
|
|
||||||
AP_Int8 _pan_rc_in;
|
|
||||||
|
|
||||||
AP_Int16 _roll_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
|
||||||
AP_Int16 _roll_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
|
||||||
AP_Int16 _tilt_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
|
||||||
AP_Int16 _tilt_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
|
||||||
AP_Int16 _pan_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
|
||||||
AP_Int16 _pan_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
|
||||||
|
|
||||||
AP_Int8 _joystick_speed;
|
|
||||||
|
|
||||||
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_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
|
||||||
|
|
||||||
AP_Float _roll_stb_lead;
|
|
||||||
AP_Float _pitch_stb_lead;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_MOUNT_H__
|
#endif // __AP_MOUNT_H__
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue