fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

vmount: Store offset in radians and calculated scale factor in OutputConfig instead of raw parameters.

Browse Source
This commit is contained in:
Matthew Edwards 2017-10-21 10:29:37 +13:00 committed by Beat Küng
parent ccf3e71b56
commit fdf4eb0bd6
4 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/drivers/vmount/output.h
View File

@ -50,17 +50,21 @@ namespace vmount
{
struct OutputConfig {
float gimbal_retracted_mode_value; /**< mixer output value for selecting gimbal retracted mode */
float gimbal_normal_mode_value; /**< mixer output value for selecting gimbal normal mode */
float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */
float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */
float pitch_range;
float roll_range;
float yaw_range;
float pitch_offset;
float roll_offset;
float yaw_offset;
/** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float pitch_scale;
/** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float roll_scale;
/** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float yaw_scale;
uint32_t mavlink_sys_id; /**< mavlink target system id for mavlink output */
float pitch_offset; /**< Offset for pitch channel in radians */
float roll_offset; /**< Offset for roll channel in radians */
float yaw_offset; /**< Offset for yaw channel in radians */
uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */
uint32_t mavlink_comp_id;
};

6
src/drivers/vmount/output_mavlink.cpp
View File

@ -113,9 +113,9 @@ int OutputMavlink::update(const ControlData *control_data)
// vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
// vmount uses radians, MAVLink uses degrees
vehicle_command.param1 = _angle_outputs[1] * M_RAD_TO_DEG_F + _config.pitch_offset;
vehicle_command.param2 = _angle_outputs[0] * M_RAD_TO_DEG_F + _config.roll_offset;
vehicle_command.param3 = _angle_outputs[2] * M_RAD_TO_DEG_F + _config.yaw_offset;
vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F;
vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);

9
src/drivers/vmount/output_rc.cpp
View File

@ -74,12 +74,9 @@ int OutputRC::update(const ControlData *control_data)
actuator_controls_s actuator_controls;
actuator_controls.timestamp = hrt_absolute_time();
// _angle_outputs are in radians, actuator_controls are in [-1, 1]
actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset * M_DEG_TO_RAD_F) /
(_config.roll_range * (M_DEG_TO_RAD_F / 2.0f));
actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset * M_DEG_TO_RAD_F) /
(_config.pitch_range * (M_DEG_TO_RAD_F / 2.0f));
actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset * M_DEG_TO_RAD_F) /
(_config.yaw_range * (M_DEG_TO_RAD_F / 2.0f));
actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset) * _config.roll_scale;
actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale;
actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale;
actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
int instance;

12
src/drivers/vmount/vmount.cpp
View File

@ -256,12 +256,12 @@ static int vmount_thread_main(int argc, char *argv[])
output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
output_config.pitch_range = params.mnt_range_pitch;
output_config.roll_range = params.mnt_range_roll;
output_config.yaw_range = params.mnt_range_yaw;
output_config.pitch_offset = params.mnt_off_pitch;
output_config.roll_offset = params.mnt_off_roll;
output_config.yaw_offset = params.mnt_off_yaw;
output_config.pitch_scale = 1.0f / ((params.mnt_range_pitch / 2.0f) * M_DEG_TO_RAD_F);
output_config.roll_scale = 1.0f / ((params.mnt_range_roll / 2.0f) * M_DEG_TO_RAD_F);
output_config.yaw_scale = 1.0f / ((params.mnt_range_yaw / 2.0f) * M_DEG_TO_RAD_F);
output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F;
output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F;
output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F;
output_config.mavlink_sys_id = params.mnt_mav_sysid;
output_config.mavlink_comp_id = params.mnt_mav_compid;