yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

vmount: implement VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET

Browse Source
This commit is contained in:
Beat Küng
2018-03-15 11:41:10 +01:00
parent cc777a80ff
commit a0372c6183
6 changed files with 19 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_roi.msg
View File

@@ -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

2
src/drivers/vmount/common.h
View File

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

2
src/drivers/vmount/input.cpp
View File

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

7
src/drivers/vmount/input_mavlink.cpp
View File

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

4
src/drivers/vmount/output.cpp
View File

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

6
src/modules/navigator/navigator_main.cpp
View File

@@ -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: