From fdf4eb0bd65e07ea269650dfaede10fec73f5a2b Mon Sep 17 00:00:00 2001 From: Matthew Edwards Date: Sat, 21 Oct 2017 10:29:37 +1300 Subject: [PATCH] vmount: Store offset in radians and calculated scale factor in OutputConfig instead of raw parameters. --- src/drivers/vmount/output.h | 22 +++++++++++++--------- src/drivers/vmount/output_mavlink.cpp | 6 +++--- src/drivers/vmount/output_rc.cpp | 9 +++------ src/drivers/vmount/vmount.cpp | 12 ++++++------ 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/drivers/vmount/output.h b/src/drivers/vmount/output.h index e7e06403fe..7589af45a3 100644 --- a/src/drivers/vmount/output.h +++ b/src/drivers/vmount/output.h @@ -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; }; diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp index 9f06f828a9..230c938f1d 100644 --- a/src/drivers/vmount/output_mavlink.cpp +++ b/src/drivers/vmount/output_mavlink.cpp @@ -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); diff --git a/src/drivers/vmount/output_rc.cpp b/src/drivers/vmount/output_rc.cpp index 591a6065df..59731f41b8 100644 --- a/src/drivers/vmount/output_rc.cpp +++ b/src/drivers/vmount/output_rc.cpp @@ -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; diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp index 0f5d7c0d40..94a775d840 100644 --- a/src/drivers/vmount/vmount.cpp +++ b/src/drivers/vmount/vmount.cpp @@ -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;