From 1a5b06d0cfd14fee32ff5407c5fa2eb6af3cfe98 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 7 Dec 2019 15:13:27 +0100 Subject: [PATCH] FlightTasks: refactor NAN initializations, spacing --- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 6 +-- .../AutoMapper2/FlightTaskAutoMapper2.cpp | 6 +-- .../tasks/FlightTask/FlightTask.cpp | 10 ++--- .../tasks/FlightTask/FlightTask.hpp | 39 +++++++++---------- .../FlightTaskManualAltitudeSmoothVel.hpp | 2 - .../FlightTaskManualPositionSmooth.cpp | 1 - .../tasks/Transition/FlightTaskTransition.cpp | 7 +--- 7 files changed, 32 insertions(+), 39 deletions(-) diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index b750f4379e..c8c9f9f081 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -65,7 +65,7 @@ bool FlightTaskAutoMapper::update() // vehicle exits idle. if (_type_previous == WaypointType::idle) { - _thrust_setpoint = Vector3f(NAN, NAN, NAN); + _thrust_setpoint.setNaN(); } // during mission and reposition, raise the landing gears but only @@ -112,8 +112,8 @@ void FlightTaskAutoMapper::_reset() void FlightTaskAutoMapper::_generateIdleSetpoints() { // Send zero thrust setpoint - _position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints - _velocity_setpoint = Vector3f(NAN, NAN, NAN); + _position_setpoint.setNaN(); // Don't require any position/velocity setpoints + _velocity_setpoint.setNaN(); _thrust_setpoint.zero(); } diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index 15dbb88144..5986036d41 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -57,7 +57,7 @@ bool FlightTaskAutoMapper2::update() // vehicle exits idle. if (_type_previous == WaypointType::idle) { - _thrust_setpoint = Vector3f(NAN, NAN, NAN); + _thrust_setpoint.setNaN(); } // during mission and reposition, raise the landing gears but only @@ -120,8 +120,8 @@ void FlightTaskAutoMapper2::_reset() void FlightTaskAutoMapper2::_prepareIdleSetpoints() { // Send zero thrust setpoint - _position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints - _velocity_setpoint = Vector3f(NAN, NAN, NAN); + _position_setpoint.setNaN(); // Don't require any position/velocity setpoints + _velocity_setpoint.setNaN(); _thrust_setpoint.zero(); } diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index 2ee5151a7c..d1720a576f 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -105,11 +105,11 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() void FlightTask::_resetSetpoints() { - _position_setpoint.setAll(NAN); - _velocity_setpoint.setAll(NAN); - _acceleration_setpoint.setAll(NAN); - _jerk_setpoint.setAll(NAN); - _thrust_setpoint.setAll(NAN); + _position_setpoint.setNaN(); + _velocity_setpoint.setNaN(); + _acceleration_setpoint.setNaN(); + _jerk_setpoint.setNaN(); + _thrust_setpoint.setNaN(); _yaw_setpoint = _yawspeed_setpoint = NAN; } diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index d65e1b48c2..5bd8a751ee 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -168,10 +168,9 @@ public: const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } protected: - - uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; - uORB::SubscriptionData _sub_attitude{ORB_ID(vehicle_attitude)}; - uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; + uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _sub_attitude{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; /** Reset all setpoints to NAN */ void _resetSetpoints(); @@ -201,18 +200,18 @@ protected: /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ - float _time = 0; /**< passed time in seconds since the task was activated */ - float _deltatime = 0; /**< passed time in seconds since the task was last updated */ - hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */ - hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */ - hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */ + float _time{}; /**< passed time in seconds since the task was activated */ + float _deltatime{}; /**< passed time in seconds since the task was last updated */ + hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */ + hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */ + hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */ /* Current vehicle state */ matrix::Vector3f _position; /**< current vehicle position */ matrix::Vector3f _velocity; /**< current vehicle velocity */ - float _yaw = 0.f; /**< current vehicle yaw heading */ - float _dist_to_bottom = 0.f; /**< current height above ground level */ - float _dist_to_ground = 0.f; /**< equals _dist_to_bottom if valid, height above home otherwise */ + float _yaw{}; /**< current vehicle yaw heading */ + float _dist_to_bottom{}; /**< current height above ground level */ + float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */ /** * Setpoints which the position controller has to execute. @@ -227,20 +226,20 @@ protected: matrix::Vector3f _acceleration_setpoint; matrix::Vector3f _jerk_setpoint; matrix::Vector3f _thrust_setpoint; - float _yaw_setpoint; - float _yawspeed_setpoint; + float _yaw_setpoint{}; + float _yawspeed_setpoint{}; matrix::Vector3f _velocity_setpoint_feedback; matrix::Vector3f _thrust_setpoint_feedback; /* Counters for estimator local position resets */ struct { - uint8_t xy = 0; - uint8_t vxy = 0; - uint8_t z = 0; - uint8_t vz = 0; - uint8_t quat = 0; - } _reset_counters; + uint8_t xy; + uint8_t vxy; + uint8_t z; + uint8_t vz; + uint8_t quat; + } _reset_counters{}; /** * Vehicle constraints. diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 9be1385ddb..2818ef37af 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -52,7 +52,6 @@ public: void reActivate() override; protected: - virtual void _updateSetpoints() override; /** Reset position or velocity setpoints in case of EKF reset event */ @@ -66,7 +65,6 @@ protected: ) private: - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); void _updateTrajConstraints(); diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp index 9f807a1cf9..053359f43b 100644 --- a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp +++ b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp @@ -65,5 +65,4 @@ void FlightTaskManualPositionSmooth::_updateSetpoints() /* Check for altitude lock*/ _updateAltitudeLock(); - } diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp index b17337a724..1f2a0c9cb3 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp @@ -69,7 +69,7 @@ void FlightTaskTransition::updateAccelerationEstimate() if (!PX4_ISFINITE(_acceleration_setpoint(0)) || !PX4_ISFINITE(_acceleration_setpoint(1)) || !PX4_ISFINITE(_acceleration_setpoint(2))) { - _acceleration_setpoint.setAll(0.f); + _acceleration_setpoint.setZero(); } _velocity_prev = _velocity; @@ -78,11 +78,8 @@ void FlightTaskTransition::updateAccelerationEstimate() bool FlightTaskTransition::update() { // level wings during the transition, altitude should be controlled - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f; - _thrust_setpoint(2) = NAN; - _position_setpoint *= NAN; - _velocity_setpoint *= NAN; _position_setpoint(2) = _transition_altitude; + _thrust_setpoint.xy() = matrix::Vector2f(0.f, 0.f); updateAccelerationEstimate();