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

FlightTasks: switch to uORB::Subscription

Browse Source 
- delete SubscriptionArray
 - all FlightTasks are now responsible for updating their own subscriptions (typically in updateInitialize()).
This commit is contained in:
Daniel Agar
2019-09-30 18:16:36 -04:00
committed by GitHub
parent ad728cc0c7
commit 045f6233d4
22 changed files with 192 additions and 507 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+40 -49
View File
@@ -9,19 +9,6 @@ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NA
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
return false;
}
if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
return false;
}
return true;
}
bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
{
_resetSetpoints();
@@ -43,6 +30,10 @@ bool FlightTask::updateInitialize()
_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
_deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
_time_stamp_last = _time_stamp_current;
_sub_vehicle_local_position.update();
_sub_attitude.update();
_evaluateVehicleLocalPosition();
_checkEkfResetCounters();
return true;
@@ -50,40 +41,40 @@ bool FlightTask::updateInitialize()
void FlightTask::_initEkfResetCounters()
{
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
_reset_counters.quat = _sub_attitude->get().quat_reset_counter;
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
}
void FlightTask::_checkEkfResetCounters()
{
// Check if a reset event has happened
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
_ekfResetHandlerPositionXY();
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
}
if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
_ekfResetHandlerVelocityXY();
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
}
if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
_ekfResetHandlerPositionZ();
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
}
if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
_ekfResetHandlerVelocityZ();
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
_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();
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;
_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
}
}
@@ -133,44 +124,44 @@ void FlightTask::_evaluateVehicleLocalPosition()
_yaw = NAN;
_dist_to_bottom = NAN;
if ((_time_stamp_current - _sub_attitude->get().timestamp) < _timeout) {
if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
// yaw
_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().q)).psi();
_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) {
if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
// position
if (_sub_vehicle_local_position->get().xy_valid) {
_position(0) = _sub_vehicle_local_position->get().x;
_position(1) = _sub_vehicle_local_position->get().y;
if (_sub_vehicle_local_position.get().xy_valid) {
_position(0) = _sub_vehicle_local_position.get().x;
_position(1) = _sub_vehicle_local_position.get().y;
}
if (_sub_vehicle_local_position->get().z_valid) {
_position(2) = _sub_vehicle_local_position->get().z;
if (_sub_vehicle_local_position.get().z_valid) {
_position(2) = _sub_vehicle_local_position.get().z;
}
// velocity
if (_sub_vehicle_local_position->get().v_xy_valid) {
_velocity(0) = _sub_vehicle_local_position->get().vx;
_velocity(1) = _sub_vehicle_local_position->get().vy;
if (_sub_vehicle_local_position.get().v_xy_valid) {
_velocity(0) = _sub_vehicle_local_position.get().vx;
_velocity(1) = _sub_vehicle_local_position.get().vy;
}
if (_sub_vehicle_local_position->get().v_z_valid) {
_velocity(2) = _sub_vehicle_local_position->get().vz;
if (_sub_vehicle_local_position.get().v_z_valid) {
_velocity(2) = _sub_vehicle_local_position.get().vz;
}
// distance to bottom
if (_sub_vehicle_local_position->get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position->get().dist_bottom;
if (_sub_vehicle_local_position.get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
}
// global frame reference coordinates to enable conversions
if (_sub_vehicle_local_position->get().xy_global && _sub_vehicle_local_position->get().z_global) {
globallocalconverter_init(_sub_vehicle_local_position->get().ref_lat, _sub_vehicle_local_position->get().ref_lon,
_sub_vehicle_local_position->get().ref_alt, _sub_vehicle_local_position->get().ref_timestamp);
if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
_sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
}
}
}
@@ -194,7 +185,7 @@ bool FlightTask::_checkTakeoff()
if (PX4_ISFINITE(_position_setpoint(2))) {
// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
float min_altitude = 0.2f;
const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
if (PX4_ISFINITE(min_distance_to_ground)) {
min_altitude = min_distance_to_ground + 0.05f;