vehicle_local_position: rename yaw -> heading and add reset logic

- vehicle_global_position yaw removed (redundant)
This commit is contained in:
Daniel Agar
2020-08-10 11:42:03 +02:00
committed by Mathieu Bresciani
parent 33fb9d0c62
commit 97fc1db768
24 changed files with 75 additions and 86 deletions
+7 -1
View File
@@ -132,14 +132,20 @@ void OutputBase::_handle_position_update(bool force_update)
}
vehicle_global_position_s vehicle_global_position{};
vehicle_local_position_s vehicle_local_position{};
if (force_update) {
_vehicle_global_position_sub.copy(&vehicle_global_position);
_vehicle_local_position_sub.copy(&vehicle_local_position);
} else {
if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
return;
}
if (!_vehicle_local_position_sub.update(&vehicle_local_position)) {
return;
}
}
const double &vlat = vehicle_global_position.lat;
@@ -159,7 +165,7 @@ void OutputBase::_handle_position_update(bool force_update)
_angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
}
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_global_position.yaw;
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading;
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;
+2
View File
@@ -47,6 +47,7 @@
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
namespace vmount
{
@@ -124,6 +125,7 @@ protected:
private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
};