diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 1ec401bdf729c..2c29141e23983 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -213,12 +213,13 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = { // 23 formerly _K_RATE - // @Param: _REL_PAN - // @DisplayName: Relative pan flag for Servo Mount - // @Description: Enable to calculate pan angle to GPS location relative to vehicle orientation (for type servo (1)) - // @Values: 0:Disabled,1:Enabled + // @Param: _OPTIONS + // @DisplayName: Options field + // @Description: User configurable options; 0 = Enable Relative Pan on Servo Mounts + // @Values: 0:RelativePan + // @Bitmask: 0:RelativePan // @User: Standard - AP_GROUPINFO("_REL_PAN", 24, AP_Mount, state[0]._rel_pan, 0), + AP_GROUPINFO("_OPTIONS", 24, AP_Mount, state[0]._options, 0), #if AP_MOUNT_MAX_INSTANCES > 1 // @Param: 2_DEFLT_MODE @@ -401,12 +402,13 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = { // @User: Standard AP_GROUPINFO("2_TYPE", 42, AP_Mount, state[1]._type, 0), - // @Param: 2_REL_PAN - // @DisplayName: Relative pan flag for Servo Mount 2 - // @Description: Enable to calculate pan angle to GPS location relative to vehicle orientation (for type servo (1)) - // @Values: 0:Disabled,1:Enabled + // @Param: 2_OPTIONS + // @DisplayName: Options field for Mount 2 + // @Description: User configurable options; 0 = Enable Relative Pan on Servo Mounts + // @Values: 0:RelativePan + // @Bitmask: 0:RelativePan // @User: Standard - AP_GROUPINFO("2_REL_PAN", 43, AP_Mount, state[0]._rel_pan, 0), + AP_GROUPINFO("2_OPTIONS", 43, AP_Mount, state[1]._options, 0), #endif // AP_MOUNT_MAX_INSTANCES > 1 AP_GROUPEND diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 7d65e40ce234a..c1e507fbb3812 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -40,6 +40,9 @@ // maximum number of mounts #define AP_MOUNT_MAX_INSTANCES 1 +// options (see _OPTIONS parameter) +#define AP_MOUNT_OPTION_RELATIVE_PAN (1<<0) + // declare backend classes class AP_Mount_Backend; class AP_Mount_Servo; @@ -190,7 +193,7 @@ class AP_Mount Location _target_sysid_location; // sysid target location bool _target_sysid_location_set; - AP_Int8 _rel_pan; // Use relative pan for servo mounts (0=Disable, 1=Enable) + AP_Int8 _options; // Options field, bit 0 = use relative pan } state[AP_MOUNT_MAX_INSTANCES]; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index c430f2f0ff024..89aeab5de0d2d 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -35,6 +35,8 @@ void AP_Mount_Servo::update() _last_check_servo_map_ms = now; } + uint8_t relative_pan = _state._options & AP_MOUNT_OPTION_RELATIVE_PAN; + 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 case MAV_MOUNT_MODE_RETRACT: @@ -70,7 +72,7 @@ void AP_Mount_Servo::update() // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: { - if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, _state._rel_pan)) { + if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, relative_pan)) { stabilize(); } break; @@ -83,7 +85,7 @@ void AP_Mount_Servo::update() } _state._roi_target = AP::ahrs().get_home(); _state._roi_target_set = true; - if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, _state._rel_pan)) { + if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, relative_pan)) { stabilize(); } break; @@ -92,7 +94,7 @@ void AP_Mount_Servo::update() if (calc_angle_to_sysid_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, - _state._rel_pan)) { + relative_pan)) { stabilize(); } break;