vmount: implement VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET

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
+

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;

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 */

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);

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)

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: