2013-05-29 20:51:24 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2012-06-13 15:55:19 -03:00
2015-08-11 03:28:44 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_Param/AP_Param.h>
# include "AP_Mount.h"
# include "AP_Mount_Backend.h"
# include "AP_Mount_Servo.h"
# include "AP_Mount_MAVLink.h"
# include "AP_Mount_Alexmos.h"
# include "AP_Mount_SToRM32.h"
# include "AP_Mount_SToRM32_serial.h"
2012-08-26 20:33:45 -03:00
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_Mount : : var_info [ ] = {
2015-01-12 08:11:32 -04:00
// @Param: _DEFLT_MODE
2015-01-12 07:41:12 -04:00
// @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
2012-08-17 03:20:30 -03:00
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _DEFLT_MODE " , 0 , AP_Mount , state [ 0 ] . _default_mode , MAV_MOUNT_MODE_RC_TARGETING ) ,
2012-07-03 18:36:34 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _RETRACT_X
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount roll angle when in retracted position
// @Description: Mount roll angle when in retracted position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2013-01-02 03:47:39 -04:00
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
// @Param: _RETRACT_Y
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount tilt/pitch angle when in retracted position
// @Description: Mount tilt/pitch angle when in retracted position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2013-01-02 03:47:39 -04:00
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
// @Param: _RETRACT_Z
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount yaw/pan angle when in retracted position
// @Description: Mount yaw/pan angle when in retracted position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2012-08-17 03:20:30 -03:00
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _RETRACT " , 1 , AP_Mount , state [ 0 ] . _retract_angles , 0 ) ,
2012-07-03 18:36:34 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _NEUTRAL_X
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount roll angle when in neutral position
// @Description: Mount roll angle when in neutral position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2012-08-17 03:20:30 -03:00
// @Increment: 1
// @User: Standard
2012-07-03 18:36:34 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _NEUTRAL_Y
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount tilt/pitch angle when in neutral position
// @Description: Mount tilt/pitch angle when in neutral position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2012-08-17 03:20:30 -03:00
// @Increment: 1
// @User: Standard
2013-01-02 03:47:39 -04:00
2015-01-12 08:11:32 -04:00
// @Param: _NEUTRAL_Z
2013-01-02 03:47:39 -04:00
// @DisplayName: Mount pan/yaw angle when in neutral position
// @Description: Mount pan/yaw angle when in neutral position
2014-08-23 08:49:30 -03:00
// @Units: Degrees
// @Range: -180.00 179.99
2013-01-02 03:47:39 -04:00
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _NEUTRAL " , 2 , AP_Mount , state [ 0 ] . _neutral_angles , 0 ) ,
2013-01-02 03:47:39 -04:00
2015-01-08 16:10:48 -04:00
// 3 was used for control_angles
2013-01-02 03:47:39 -04:00
2015-01-12 08:11:32 -04:00
// @Param: _STAB_ROLL
2013-01-02 03:47:39 -04:00
// @DisplayName: Stabilize mount's roll angle
2014-01-08 22:34:10 -04:00
// @Description: enable roll stabilisation relative to Earth
2012-08-17 03:20:30 -03:00
// @Values: 0:Disabled,1:Enabled
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _STAB_ROLL " , 4 , AP_Mount , state [ 0 ] . _stab_roll , 0 ) ,
2012-07-03 18:36:34 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _STAB_TILT
2013-01-02 03:47:39 -04:00
// @DisplayName: Stabilize mount's pitch/tilt angle
// @Description: enable tilt/pitch stabilisation relative to Earth
2012-08-17 03:20:30 -03:00
// @Values: 0:Disabled,1:Enabled
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _STAB_TILT " , 5 , AP_Mount , state [ 0 ] . _stab_tilt , 0 ) ,
2012-07-03 18:36:34 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _STAB_PAN
2013-01-02 03:47:39 -04:00
// @DisplayName: Stabilize mount pan/yaw angle
// @Description: enable pan/yaw stabilisation relative to Earth
2012-08-17 03:20:30 -03:00
// @Values: 0:Disabled,1:Enabled
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _STAB_PAN " , 6 , AP_Mount , state [ 0 ] . _stab_pan , 0 ) ,
2012-08-05 18:48:57 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _RC_IN_ROLL
2012-08-17 03:20:30 -03:00
// @DisplayName: roll RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control roll movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2012-08-17 03:20:30 -03:00
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _RC_IN_ROLL " , 7 , AP_Mount , state [ 0 ] . _roll_rc_in , 0 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMIN_ROL
2012-08-17 03:20:30 -03:00
// @DisplayName: Minimum roll angle
// @Description: Minimum physical roll angular position of mount.
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMIN_ROL " , 8 , AP_Mount , state [ 0 ] . _roll_angle_min , - 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMAX_ROL
2012-08-17 03:20:30 -03:00
// @DisplayName: Maximum roll angle
// @Description: Maximum physical roll angular position of the mount
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMAX_ROL " , 9 , AP_Mount , state [ 0 ] . _roll_angle_max , 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _RC_IN_TILT
2012-08-17 03:20:30 -03:00
// @DisplayName: tilt (pitch) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2012-08-17 03:20:30 -03:00
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _RC_IN_TILT " , 10 , AP_Mount , state [ 0 ] . _tilt_rc_in , 0 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMIN_TIL
2012-08-17 03:20:30 -03:00
// @DisplayName: Minimum tilt angle
// @Description: Minimum physical tilt (pitch) angular position of mount.
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMIN_TIL " , 11 , AP_Mount , state [ 0 ] . _tilt_angle_min , - 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMAX_TIL
2012-08-17 03:20:30 -03:00
// @DisplayName: Maximum tilt angle
// @Description: Maximum physical tilt (pitch) angular position of the mount
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMAX_TIL " , 12 , AP_Mount , state [ 0 ] . _tilt_angle_max , 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _RC_IN_PAN
2012-08-17 03:20:30 -03:00
// @DisplayName: pan (yaw) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2012-08-17 03:20:30 -03:00
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _RC_IN_PAN " , 13 , AP_Mount , state [ 0 ] . _pan_rc_in , 0 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMIN_PAN
2012-08-17 03:20:30 -03:00
// @DisplayName: Minimum pan angle
// @Description: Minimum physical pan (yaw) angular position of mount.
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMIN_PAN " , 14 , AP_Mount , state [ 0 ] . _pan_angle_min , - 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _ANGMAX_PAN
2012-08-17 03:20:30 -03:00
// @DisplayName: Maximum pan angle
// @Description: Maximum physical pan (yaw) angular position of the mount
2013-01-02 03:47:39 -04:00
// @Units: Centi-Degrees
2012-08-17 03:20:30 -03:00
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _ANGMAX_PAN " , 15 , AP_Mount , state [ 0 ] . _pan_angle_max , 4500 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _JSTICK_SPD
2012-08-17 03:20:30 -03:00
// @DisplayName: mount joystick speed
2013-03-07 23:39:32 -04:00
// @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.
// @Range: 0 100
2012-08-17 03:20:30 -03:00
// @Increment: 1
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _JSTICK_SPD " , 16 , AP_Mount , _joystick_speed , 0 ) ,
2012-08-17 03:20:30 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _LEAD_RLL
2014-08-26 09:52:04 -03:00
// @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: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _LEAD_RLL " , 17 , AP_Mount , state [ 0 ] . _roll_stb_lead , 0.0f ) ,
2014-08-26 09:52:04 -03:00
2015-01-12 08:11:32 -04:00
// @Param: _LEAD_PTCH
2014-08-26 09:52:04 -03:00
// @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: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _LEAD_PTCH " , 18 , AP_Mount , state [ 0 ] . _pitch_stb_lead , 0.0f ) ,
2015-01-08 16:10:48 -04:00
2015-01-12 08:11:32 -04:00
// @Param: _TYPE
2015-01-08 16:10:48 -04:00
// @DisplayName: Mount Type
// @Description: Mount Type (None, Servo or MAVLink)
2015-07-07 01:28:16 -03:00
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
2015-12-25 20:41:41 -04:00
// @RebootRequired: True
2015-01-08 16:10:48 -04:00
// @User: Standard
2015-01-12 08:11:32 -04:00
AP_GROUPINFO ( " _TYPE " , 19 , AP_Mount , state [ 0 ] . _type , 0 ) ,
2014-08-26 09:52:04 -03:00
2015-04-20 03:31:17 -03:00
// @Param: _OFF_JNT_X
// @DisplayName: MAVLink Mount's roll angle offsets
// @Description: MAVLink Mount's roll angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_JNT_Y
// @DisplayName: MAVLink Mount's pitch angle offsets
// @Description: MAVLink Mount's pitch angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_JNT_Z
// @DisplayName: MAVLink Mount's yaw angle offsets
// @Description: MAVLink Mount's yaw angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
2015-03-30 19:48:13 -03:00
AP_GROUPINFO ( " _OFF_JNT " , 20 , AP_Mount , state [ 0 ] . _gimbalParams . joint_angles_offsets , 0 ) ,
2015-04-20 03:31:17 -03:00
// @Param: _OFF_ACC_X
// @DisplayName: MAVLink Mount's roll velocity offsets
// @Description: MAVLink Mount's roll velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
// @Param: _OFF_ACC_Y
// @DisplayName: MAVLink Mount's pitch velocity offsets
// @Description: MAVLink Mount's pitch velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
// @Param: _OFF_ACC_Z
// @DisplayName: MAVLink Mount's yaw velocity offsets
// @Description: MAVLink Mount's yaw velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
2015-03-30 19:48:13 -03:00
AP_GROUPINFO ( " _OFF_ACC " , 21 , AP_Mount , state [ 0 ] . _gimbalParams . delta_velocity_offsets , 0 ) ,
2015-04-20 03:31:17 -03:00
// @Param: _OFF_GYRO_X
// @DisplayName: MAVLink Mount's roll gyro offsets
// @Description: MAVLink Mount's roll gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_GYRO_Y
// @DisplayName: MAVLink Mount's pitch gyro offsets
// @Description: MAVLink Mount's pitch gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_GYRO_Z
// @DisplayName: MAVLink Mount's yaw gyro offsets
// @Description: MAVLink Mount's yaw gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
2015-03-30 19:48:13 -03:00
AP_GROUPINFO ( " _OFF_GYRO " , 22 , AP_Mount , state [ 0 ] . _gimbalParams . delta_angles_offsets , 0 ) ,
2015-04-20 03:31:17 -03:00
// @Param: _K_RATE
// @DisplayName: MAVLink Mount's rate gain
// @Description: MAVLink Mount's rate gain
// @Range: 0 10
// @User: Advanced
2015-03-30 19:48:13 -03:00
AP_GROUPINFO ( " _K_RATE " , 23 , AP_Mount , state [ 0 ] . _gimbalParams . K_gimbalRate , 5.0f ) ,
2015-01-12 08:49:46 -04:00
// 20 ~ 24 reserved for future parameters
# if AP_MOUNT_MAX_INSTANCES > 1
// @Param: 2_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
// @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: Degrees
// @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: Degrees
// @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: Degrees
// @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: Degrees
// @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: Degrees
// @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: Degrees
// @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
// @Param: 2_STAB_ROLL
// @DisplayName: Stabilize Mount2's roll angle
// @Description: enable roll stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO ( " 2_STAB_ROLL " , 28 , AP_Mount , state [ 1 ] . _stab_roll , 0 ) ,
// @Param: 2_STAB_TILT
// @DisplayName: Stabilize Mount2's pitch/tilt angle
// @Description: enable tilt/pitch stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO ( " 2_STAB_TILT " , 29 , AP_Mount , state [ 1 ] . _stab_tilt , 0 ) ,
// @Param: 2_STAB_PAN
// @DisplayName: Stabilize mount2 pan/yaw angle
// @Description: enable pan/yaw stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO ( " 2_STAB_PAN " , 30 , AP_Mount , state [ 1 ] . _stab_pan , 0 ) ,
// @Param: 2_RC_IN_ROLL
// @DisplayName: Mount2's roll RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control roll movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2015-01-12 08:49:46 -04:00
// @User: Standard
AP_GROUPINFO ( " 2_RC_IN_ROLL " , 31 , AP_Mount , state [ 1 ] . _roll_rc_in , 0 ) ,
// @Param: 2_ANGMIN_ROL
// @DisplayName: Mount2's minimum roll angle
// @Description: Mount2's minimum physical roll angular position
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @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: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " 2_ANGMAX_ROL " , 33 , AP_Mount , state [ 1 ] . _roll_angle_max , 4500 ) ,
// @Param: 2_RC_IN_TILT
// @DisplayName: Mount2's tilt (pitch) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2015-01-12 08:49:46 -04:00
// @User: Standard
AP_GROUPINFO ( " 2_RC_IN_TILT " , 34 , AP_Mount , state [ 1 ] . _tilt_rc_in , 0 ) ,
// @Param: 2_ANGMIN_TIL
// @DisplayName: Mount2's minimum tilt angle
// @Description: Mount2's minimum physical tilt (pitch) angular position
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @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: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " 2_ANGMAX_TIL " , 36 , AP_Mount , state [ 1 ] . _tilt_angle_max , 4500 ) ,
// @Param: 2_RC_IN_PAN
// @DisplayName: Mount2's pan (yaw) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements
2015-09-29 03:35:43 -03:00
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8,9:RC9,10:RC10,11:RC11,12:RC12
2015-01-12 08:49:46 -04:00
// @User: Standard
AP_GROUPINFO ( " 2_RC_IN_PAN " , 37 , AP_Mount , state [ 1 ] . _pan_rc_in , 0 ) ,
// @Param: 2_ANGMIN_PAN
// @DisplayName: Mount2's minimum pan angle
// @Description: Mount2's minimum physical pan (yaw) angular position
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @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: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @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: Seconds
// @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: Seconds
// @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)
2015-07-07 01:28:16 -03:00
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
2015-01-12 08:49:46 -04:00
// @User: Standard
AP_GROUPINFO ( " 2_TYPE " , 42 , AP_Mount , state [ 1 ] . _type , 0 ) ,
# endif // AP_MOUNT_MAX_INSTANCES > 1
2012-08-17 03:20:30 -03:00
AP_GROUPEND
2012-07-02 21:21:01 -03:00
} ;
2015-02-25 00:07:44 -04:00
AP_Mount : : AP_Mount ( const AP_AHRS_TYPE & ahrs , const struct Location & current_loc ) :
2014-08-13 11:00:04 -03:00
_ahrs ( ahrs ) ,
2015-01-08 16:10:48 -04:00
_current_loc ( current_loc ) ,
_num_instances ( 0 ) ,
_primary ( 0 )
2012-06-13 15:55:19 -03:00
{
2012-12-12 17:52:04 -04:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2012-06-13 15:55:19 -03:00
2015-01-15 02:29:06 -04:00
// initialise backend pointers and mode
2015-01-08 16:10:48 -04:00
for ( uint8_t i = 0 ; i < AP_MOUNT_MAX_INSTANCES ; i + + ) {
_backends [ i ] = NULL ;
}
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// init - detect and initialise all mounts
2015-01-19 09:38:17 -04:00
void AP_Mount : : init ( const AP_SerialManager & serial_manager )
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
// check init has not been called before
if ( _num_instances ! = 0 ) {
return ;
2012-08-17 03:20:30 -03:00
}
2015-06-10 03:01:12 -03:00
// default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
2015-10-24 23:33:40 -03:00
if ( ! state [ 0 ] . _type . configured ( ) ) {
2015-06-10 03:01:12 -03:00
if ( RC_Channel_aux : : function_assigned ( RC_Channel_aux : : Aux_servo_function_t : : k_mount_pan ) | |
RC_Channel_aux : : function_assigned ( RC_Channel_aux : : Aux_servo_function_t : : k_mount_tilt ) | |
RC_Channel_aux : : function_assigned ( RC_Channel_aux : : Aux_servo_function_t : : k_mount_roll ) ) {
state [ 0 ] . _type . set_and_save ( Mount_Type_Servo ) ;
}
}
2015-01-18 21:21:47 -04:00
// primary is reset to the first instantiated mount
bool primary_set = false ;
2015-01-08 16:10:48 -04:00
// create each instance
for ( uint8_t instance = 0 ; instance < AP_MOUNT_MAX_INSTANCES ; instance + + ) {
2015-07-16 02:12:26 -03:00
// default instance's state
state [ instance ] . _mode = ( enum MAV_MOUNT_MODE ) state [ instance ] . _default_mode . get ( ) ;
2015-01-08 16:10:48 -04:00
MountType mount_type = get_mount_type ( instance ) ;
2012-08-17 03:20:30 -03:00
2015-01-08 16:10:48 -04:00
// check for servo mounts
if ( mount_type = = Mount_Type_Servo ) {
2015-01-18 21:39:55 -04:00
_backends [ instance ] = new AP_Mount_Servo ( * this , state [ instance ] , instance ) ;
2015-01-08 16:10:48 -04:00
_num_instances + + ;
2012-08-17 03:20:30 -03:00
2015-02-01 17:11:34 -04:00
# if AP_AHRS_NAVEKF_AVAILABLE
2015-01-08 16:10:48 -04:00
// check for MAVLink mounts
} else if ( mount_type = = Mount_Type_MAVLink ) {
2015-01-18 21:39:55 -04:00
_backends [ instance ] = new AP_Mount_MAVLink ( * this , state [ instance ] , instance ) ;
2015-01-08 16:10:48 -04:00
_num_instances + + ;
2015-02-01 17:11:34 -04:00
# endif
2015-01-15 18:47:59 -04:00
2015-01-16 04:17:34 -04:00
// check for Alexmos mounts
2015-01-15 18:47:59 -04:00
} else if ( mount_type = = Mount_Type_Alexmos ) {
2015-01-18 21:39:55 -04:00
_backends [ instance ] = new AP_Mount_Alexmos ( * this , state [ instance ] , instance ) ;
2015-01-15 18:47:59 -04:00
_num_instances + + ;
2015-02-14 08:27:58 -04:00
2015-05-26 04:21:12 -03:00
// check for SToRM32 mounts using MAVLink protocol
2015-02-14 08:27:58 -04:00
} else if ( mount_type = = Mount_Type_SToRM32 ) {
_backends [ instance ] = new AP_Mount_SToRM32 ( * this , state [ instance ] , instance ) ;
_num_instances + + ;
2015-05-26 04:21:12 -03:00
// check for SToRM32 mounts using serial protocol
} else if ( mount_type = = Mount_Type_SToRM32_serial ) {
_backends [ instance ] = new AP_Mount_SToRM32_serial ( * this , state [ instance ] , instance ) ;
_num_instances + + ;
2012-08-17 03:20:30 -03:00
}
2012-08-26 20:33:45 -03:00
2015-01-08 16:10:48 -04:00
// init new instance
if ( _backends [ instance ] ! = NULL ) {
2015-01-19 09:38:17 -04:00
_backends [ instance ] - > init ( serial_manager ) ;
2015-01-18 21:21:47 -04:00
if ( ! primary_set ) {
_primary = instance ;
primary_set = true ;
}
2015-01-08 16:10:48 -04:00
}
2012-08-17 03:20:30 -03:00
}
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// update - give mount opportunity to update servos. should be called at 10hz or higher
void AP_Mount : : update ( )
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
// update each instance
for ( uint8_t instance = 0 ; instance < AP_MOUNT_MAX_INSTANCES ; instance + + ) {
if ( _backends [ instance ] ! = NULL ) {
_backends [ instance ] - > update ( ) ;
}
}
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// get_mount_type - returns the type of mount
AP_Mount : : MountType AP_Mount : : get_mount_type ( uint8_t instance ) const
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
if ( instance > = AP_MOUNT_MAX_INSTANCES ) {
return Mount_Type_None ;
}
return ( MountType ) state [ instance ] . _type . get ( ) ;
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// has_pan_control - returns true if the mount has yaw control (required for copters)
bool AP_Mount : : has_pan_control ( uint8_t instance ) const
2011-10-31 18:55:58 -03:00
{
2015-01-08 16:10:48 -04:00
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return false ;
2012-08-17 03:20:30 -03:00
}
2011-10-31 18:55:58 -03:00
2015-01-08 16:10:48 -04:00
// ask backend if it support pan
return _backends [ instance ] - > has_pan_control ( ) ;
2011-10-31 18:55:58 -03:00
}
2015-01-08 16:10:48 -04:00
// get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
MAV_MOUNT_MODE AP_Mount : : get_mode ( uint8_t instance ) const
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
// sanity check instance
if ( instance > = AP_MOUNT_MAX_INSTANCES ) {
return MAV_MOUNT_MODE_RETRACT ;
}
2012-07-24 23:00:38 -03:00
2015-01-12 07:41:12 -04:00
return state [ instance ] . _mode ;
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter
2015-11-03 09:46:29 -04:00
// this operation requires 60us on a Pixhawk/PX4
2015-01-08 16:10:48 -04:00
void AP_Mount : : set_mode_to_default ( uint8_t instance )
2012-06-13 15:55:19 -03:00
{
2015-01-12 07:41:12 -04:00
set_mode ( instance , ( enum MAV_MOUNT_MODE ) state [ instance ] . _default_mode . get ( ) ) ;
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// set_mode - sets mount's mode
void AP_Mount : : set_mode ( uint8_t instance , enum MAV_MOUNT_MODE mode )
2012-08-05 18:48:57 -03:00
{
2015-01-08 16:10:48 -04:00
// sanity check instance
2015-01-12 07:41:12 -04:00
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return ;
2015-01-08 16:10:48 -04:00
}
2015-01-12 07:41:12 -04:00
// call backend's set_mode
_backends [ instance ] - > set_mode ( mode ) ;
2012-08-05 18:48:57 -03:00
}
2015-03-21 08:58:57 -03:00
// set_angle_targets - sets angle targets in degrees
void AP_Mount : : set_angle_targets ( uint8_t instance , float roll , float tilt , float pan )
{
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return ;
}
// send command to backend
_backends [ instance ] - > set_angle_targets ( roll , tilt , pan ) ;
}
2015-01-08 16:10:48 -04:00
/// Change the configuration of the mount
/// triggered by a MavLink packet.
void AP_Mount : : configure_msg ( uint8_t instance , mavlink_message_t * msg )
2012-08-05 18:48:57 -03:00
{
2015-01-08 16:10:48 -04:00
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return ;
}
// send message to backend
_backends [ instance ] - > configure_msg ( msg ) ;
2012-08-05 18:48:57 -03:00
}
2015-01-08 16:10:48 -04:00
/// Control the mount (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount : : control_msg ( uint8_t instance , mavlink_message_t * msg )
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return ;
2014-06-16 14:50:19 -03:00
}
2015-01-08 16:10:48 -04:00
// send message to backend
_backends [ instance ] - > control_msg ( msg ) ;
2012-06-13 15:55:19 -03:00
}
2015-08-13 02:31:31 -03:00
void AP_Mount : : control ( uint8_t instance , int32_t pitch_or_lat , int32_t roll_or_lon , int32_t yaw_or_alt , enum MAV_MOUNT_MODE mount_mode )
{
if ( instance > = AP_MOUNT_MAX_INSTANCES | | _backends [ instance ] = = NULL ) {
return ;
}
// send message to backend
_backends [ instance ] - > control ( pitch_or_lat , roll_or_lon , yaw_or_alt , mount_mode ) ;
}
2015-01-08 16:10:48 -04:00
/// Return mount status information
void AP_Mount : : status_msg ( mavlink_channel_t chan )
2012-06-13 15:55:19 -03:00
{
2015-01-08 16:10:48 -04:00
// call status_msg for each instance
for ( uint8_t instance = 0 ; instance < AP_MOUNT_MAX_INSTANCES ; instance + + ) {
if ( _backends [ instance ] ! = NULL ) {
_backends [ instance ] - > status_msg ( chan ) ;
2014-08-26 09:52:04 -03:00
}
2012-08-17 03:20:30 -03:00
}
2012-06-13 15:55:19 -03:00
}
2015-01-08 16:10:48 -04:00
// set_roi_target - sets target location that mount should attempt to point towards
void AP_Mount : : set_roi_target ( uint8_t instance , const struct Location & target_loc )
2012-08-05 18:48:57 -03:00
{
2015-01-08 16:10:48 -04:00
// call instance's set_roi_cmd
if ( instance < AP_MOUNT_MAX_INSTANCES & & _backends [ instance ] ! = NULL ) {
_backends [ instance ] - > set_roi_target ( target_loc ) ;
2012-08-17 03:20:30 -03:00
}
2012-08-05 18:48:57 -03:00
}
2015-01-29 05:01:55 -04:00
// pass a GIMBAL_REPORT message to the backend
void AP_Mount : : handle_gimbal_report ( mavlink_channel_t chan , mavlink_message_t * msg )
{
for ( uint8_t instance = 0 ; instance < AP_MOUNT_MAX_INSTANCES ; instance + + ) {
if ( _backends [ instance ] ! = NULL ) {
_backends [ instance ] - > handle_gimbal_report ( chan , msg ) ;
}
}
}
// send a GIMBAL_REPORT message to the GCS
void AP_Mount : : send_gimbal_report ( mavlink_channel_t chan )
{
for ( uint8_t instance = 0 ; instance < AP_MOUNT_MAX_INSTANCES ; instance + + ) {
if ( _backends [ instance ] ! = NULL ) {
_backends [ instance ] - > send_gimbal_report ( chan ) ;
}
}
}