From a0372c618322a11f537642bc8a58061c984c23bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 15 Mar 2018 11:41:10 +0100 Subject: [PATCH] vmount: implement VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET --- msg/vehicle_roi.msg | 8 +++++--- src/drivers/vmount/common.h | 2 ++ src/drivers/vmount/input.cpp | 2 ++ src/drivers/vmount/input_mavlink.cpp | 7 +++++-- src/drivers/vmount/output.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- 6 files changed, 19 insertions(+), 10 deletions(-) diff --git a/msg/vehicle_roi.msg b/msg/vehicle_roi.msg index 9c87901fea..fb61b1a6fe 100644 --- a/msg/vehicle_roi.msg +++ b/msg/vehicle_roi.msg @@ -12,6 +12,8 @@ float64 lat # Latitude to point to float64 lon # Longitude to point to float32 alt # Altitude to point to -float32 pitchOffset # Additional pitch offset to next waypoint -float32 rollOffset # Additional roll offset to next waypoint -float32 yawOffset # Additional yaw offset to next waypoint +# additional angle offsets to next waypoint (only used with ROI_WPNEXT) +float32 roll_offset # angle offset in rad +float32 pitch_offset # angle offset in rad +float32 yaw_offset # angle offset in rad + diff --git a/src/drivers/vmount/common.h b/src/drivers/vmount/common.h index e6ad7a33d0..956a639269 100644 --- a/src/drivers/vmount/common.h +++ b/src/drivers/vmount/common.h @@ -75,6 +75,8 @@ struct ControlData { double lat; /**< latitude in [deg] */ float altitude; /**< altitude in [m] */ float roll_angle; /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */ + float pitch_angle_offset; /**< angular offset for pitch [rad] */ + float yaw_angle_offset; /**< angular offset for yaw [rad] */ float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */ } lonlat; } type_data; diff --git a/src/drivers/vmount/input.cpp b/src/drivers/vmount/input.cpp index 69ac551bce..2bf3200c38 100644 --- a/src/drivers/vmount/input.cpp +++ b/src/drivers/vmount/input.cpp @@ -72,6 +72,8 @@ void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, _control_data.type_data.lonlat.altitude = altitude; _control_data.type_data.lonlat.roll_angle = roll_angle; _control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle; + _control_data.type_data.lonlat.pitch_angle_offset = 0.f; + _control_data.type_data.lonlat.yaw_angle_offset = 0.f; } } /* namespace vmount */ diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index 4e21a1fbfb..51e4d748ac 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -116,12 +116,15 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ *control_data = &_control_data; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + _control_data.type = ControlData::Type::LonLat; _read_control_data_from_position_setpoint_sub(); - _control_data.type_data.lonlat.roll_angle = 0.f; _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; - *control_data = &_control_data; + _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; + _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; + _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + *control_data = &_control_data; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp index 4c056cb621..020470d093 100644 --- a/src/drivers/vmount/output.cpp +++ b/src/drivers/vmount/output.cpp @@ -191,8 +191,8 @@ void OutputBase::_handle_position_update(bool force_update) - vehicle_global_position.yaw; _angle_setpoints[0] = roll; - _angle_setpoints[1] = pitch; - _angle_setpoints[2] = yaw; + _angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset; + _angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset; } void OutputBase::_calculate_output_angles(const hrt_abstime &t) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c5f03e73af..7d70d9dc86 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -503,9 +503,9 @@ Navigator::run() case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: _vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT; - _vroi.pitchOffset = cmd.param5; - _vroi.rollOffset = cmd.param6; - _vroi.yawOffset = cmd.param7; + _vroi.pitch_offset = (float)cmd.param5 * M_DEG_TO_RAD_F; + _vroi.roll_offset = (float)cmd.param6 * M_DEG_TO_RAD_F; + _vroi.yaw_offset = (float)cmd.param7 * M_DEG_TO_RAD_F; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: