dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vmount: refactor ROI position update for readability

This commit is contained in:
Matthias Grob 2019-02-22 17:40:55 +01:00 committed by Daniel Agar
parent cefffe652f
commit 35409b4079
1 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/vmount/output.cpp
View File

@ -174,26 +174,29 @@ void OutputBase::_handle_position_update(bool force_update)
vehicle_global_position_s vehicle_global_position;
orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
const double &vlat = vehicle_global_position.lat;
const double &vlon = vehicle_global_position.lon;
float pitch;
const double &lon = _cur_control_data->type_data.lonlat.lon;
const double &lat = _cur_control_data->type_data.lonlat.lat;
const double &lon = _cur_control_data->type_data.lonlat.lon;
const float &alt = _cur_control_data->type_data.lonlat.altitude;
_angle_setpoints[0] = _cur_control_data->type_data.lonlat.roll_angle;
// interface: use fixed pitch value > -pi otherwise consider ROI altitude
if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
_angle_setpoints[1] = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
} else {
pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
_angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
}
float roll = _cur_control_data->type_data.lonlat.roll_angle;
float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon)
- vehicle_global_position.yaw;
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_global_position.yaw;
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;
_angle_setpoints[2] += _cur_control_data->type_data.lonlat.yaw_angle_offset;
_angle_setpoints[0] = roll;
_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)