fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:13:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_local_position: rename yaw -> heading and add reset logic

Browse Source 
- 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
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
+6 -10
View File
@@ -32,7 +32,6 @@ bool FlightTask::updateInitialize()
_time_stamp_last = _time_stamp_current;
_sub_vehicle_local_position.update();
_sub_attitude.update();
_sub_home_position.update();
_evaluateVehicleLocalPosition();
@@ -69,10 +68,9 @@ void FlightTask::_checkEkfResetCounters()
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
}
if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) {
float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi();
_ekfResetHandlerHeading(delta_psi);
_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
}
}
@@ -117,14 +115,12 @@ void FlightTask::_evaluateVehicleLocalPosition()
_yaw = NAN;
_dist_to_bottom = NAN;
if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
// yaw
_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi();
}
// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
// yaw
_yaw = _sub_vehicle_local_position.get().heading;
// position
if (_sub_vehicle_local_position.get().xy_valid) {
_position(0) = _sub_vehicle_local_position.get().x;
src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
+1 -2
View File
@@ -60,7 +60,7 @@ struct ekf_reset_counters_s {
uint8_t vxy;
uint8_t z;
uint8_t vz;
uint8_t quat;
uint8_t heading;
};
class FlightTask : public ModuleParams
@@ -189,7 +189,6 @@ public:
protected:
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_attitude_s> _sub_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
/** Reset all setpoints to NAN */