diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index ade89f6c7e18..1acaebb6386f 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -58,13 +58,54 @@ void RoverAckermannGuidance::updateParams() RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state) { - // Initializations - float desired_speed{0.f}; - float vehicle_yaw{0.f}; - float actual_speed{0.f}; - bool mission_finished{false}; + updateSubscriptions(); - // uORB subscriber updates + // Catch return to launch + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = _distance_to_next_wp < _acceptance_radius; + } + + // Guidance logic + if (_mission_finished) { // Mission is finished + _desired_steering = 0.f; + _desired_speed = 0.f; + + } else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command + _desired_speed = 0.f; + + } else { // Regular guidance algorithm + _desired_speed = calcDesiredSpeed(_param_ra_miss_vel_def.get(), _param_ra_miss_vel_min.get(), + _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius, + _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state); + + _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + _param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get()); + + } + + // Calculate throttle setpoint + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(), + dt); + + // Publish ackermann controller status (logging) + _rover_ackermann_guidance_status.timestamp = _timestamp; + _rover_ackermann_guidance_status.desired_speed = _desired_speed; + _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); + + // Return motor setpoints + motor_setpoint motor_setpoint_temp; + motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint + motor_setpoint_temp.throttle = throttle; + return motor_setpoint_temp; +} + +void RoverAckermannGuidance::updateSubscriptions() +{ if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); @@ -82,7 +123,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c _curr_pos_ned = Vector2f(local_position.x, local_position.y); const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - actual_speed = rover_velocity.norm(); + _actual_speed = rover_velocity.norm(); } if (_home_position_sub.updated()) { @@ -92,158 +133,63 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c } if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); + updateWaypointsAndAcceptanceRadius(); } if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } if (_mission_result_sub.updated()) { mission_result_s mission_result{}; _mission_result_sub.copy(&mission_result); - mission_finished = mission_result.finished; - } - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0), - _next_wp(1)) < _acceptance_radius) { // Return to launch - mission_finished = true; - } - - // Guidance logic - if (mission_finished) { - _desired_steering = 0.f; - - } else { - const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), - _prev_wp(1)); - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), - _curr_wp(1)); - - if (distance_to_curr_wp < _acceptance_radius) { // Catch delay command - desired_speed = 0.f; - - } else { - // Calculate desired speed - if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() - && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect - if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { // Straight line speed - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON - && _acceptance_radius > FLT_EPSILON) { - float max_velocity{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp, 0.f); - - } else { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed); - } - - desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - } - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - - // Calculate desired steering - _desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_wheel_base.get(), - desired_speed, vehicle_yaw); - _desired_steering = math::constrain(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - } - } - - // Throttle PID - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - float throttle = 0.f; - - if (desired_speed < FLT_EPSILON) { - pid_reset_integral(&_pid_throttle); - - } else { - throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, - dt); - } - - if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term - throttle += math::interpolate(desired_speed, - 0.f, _param_ra_max_speed.get(), - 0.f, 1.f); + _mission_finished = mission_result.finished; } - - // Publish ackermann controller status (logging) - _rover_ackermann_guidance_status.timestamp = _timestamp; - _rover_ackermann_guidance_status.desired_speed = desired_speed; - _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Return motor setpoints - motor_setpoint motor_setpoint_temp; - motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), - -1.f, 1.f); - motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); - return motor_setpoint_temp; } -void RoverAckermannGuidance::updateWaypoints() +void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates + Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid + Vector2d curr_wp(0, 0); + Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - } else { - _curr_wp = Vector2d(0, 0); } if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - } else { - _prev_wp = _curr_pos; } if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - } else { - _next_wp = _home_position; // Enables corner slow down with RTL } + // Distances to waypoints + _distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + prev_wp(0), prev_wp(1)); + _distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + curr_wp(0), curr_wp(1)); + _distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + next_wp(0), next_wp(1)); + // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); + _curr_wp_ned = _global_ned_proj_ref.project(curr_wp(0), curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(prev_wp(0), prev_wp(1)); + _next_wp_ned = _global_ned_proj_ref.project(next_wp(0), next_wp(1)); // Update acceptance radius _prev_acceptance_radius = _acceptance_radius; @@ -287,24 +233,90 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned return acceptance_radius; } -float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw) +float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min, + const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state) +{ + // Catch improper values + if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) { + return miss_vel_def; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / prev_acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } + + // Straight line speed + if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float max_velocity{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp, 0.f); + + } else { + const float cornering_speed = miss_vel_gain / acc_rad; + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::constrain(max_velocity, miss_vel_min, miss_vel_def); + + } else { + return miss_vel_def; + } + +} + +float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, + const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, + const float vehicle_yaw, const float max_steering) { - // Calculate desired steering to reach lookahead point - const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, desired_speed); - const float lookahead_distance = _pure_pursuit.getLookaheadDistance(); + const float lookahead_distance = pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); // For logging _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); + float desired_steering{0.f}; + if (math::abs_t(heading_error) <= M_PI_2_F) { - return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + } else { - return atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - sign(heading_error) * M_PI_2_F)) / - lookahead_distance); + desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - + sign(heading_error) * M_PI_2_F)) / lookahead_distance); + } + + return math::constrain(desired_steering, -max_steering, max_steering); + +} + +float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed, + const float actual_speed, const float max_speed, const float dt) +{ + float throttle = 0.f; + + if (desired_speed < FLT_EPSILON) { + pid_reset_integral(&pid_throttle); + + } else { + throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt); + } + + if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term + throttle += math::interpolate(desired_speed, 0.f, max_speed, 0.f, 1.f); } + return math::constrain(throttle, 0.f, 1.f); } diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 592083556f13..23f63ed9280e 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -73,58 +73,106 @@ class RoverAckermannGuidance : public ModuleParams RoverAckermannGuidance(ModuleParams *parent); ~RoverAckermannGuidance() = default; + /** + * @brief Struct for steering and throttle setpoints. + * @param steering Steering setpoint. + * @param throttle Throttle setpoint. + */ struct motor_setpoint { float steering{0.f}; float throttle{0.f}; }; /** - * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. - * @param nav_state Vehicle navigation state + * @brief Calculate motor setpoints based on the mission plan. + * @param nav_state Vehicle navigation state. + * @return Motor setpoints for throttle and steering. */ motor_setpoint computeGuidance(int nav_state); +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions + */ + void updateSubscriptions(); + /** - * @brief Update global/NED waypoint coordinates and acceptance radius + * @brief Update global/NED waypoint coordinates and acceptance radius. */ - void updateWaypoints(); + void updateWaypointsAndAcceptanceRadius(); /** - * @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of - * the vehicle. - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param next_wp_ned Next waypoint in NED frame. - * @param default_acceptance_radius Default acceptance radius for waypoints. - * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners. - * @param acceptance_radius_max Maximum value for the acceptance radius. - * @param wheel_base Rover wheelbase. - * @param max_steer_angle Rover maximum steer angle. + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param next_wp_ned Next waypoint in NED frame [m]. + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. */ float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + /** - * @brief Calculate and return desired steering input - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param curr_pos_ned Current position of the vehicle in NED frame. - * @param wheel_base Rover wheelbase. - * @param desired_speed Desired speed for the rover. - * @param vehicle_yaw Current yaw of the rover. + * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse + * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory + * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the + * maximum acceleration and jerk. + * @param miss_vel_def Default desired velocity for the rover during mission [m/s]. + * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s]. + * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_accel Maximum allowed acceleration for the rover [m/s^2]. + * @param max_jerk Maximum allowed jerk for the rover [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @return Speed setpoint for the rover [m/s]. */ - float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - float wheel_base, float desired_speed, float vehicle_yaw); + float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state); -protected: /** - * @brief Update the parameters of the module. + * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to + * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). + * @param pure_pursuit Pure pursuit class instance. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param curr_pos_ned Current position of the vehicle in NED frame [m]. + * @param wheel_base Rover wheelbase [m]. + * @param desired_speed Desired speed for the rover [m/s]. + * @param vehicle_yaw Current yaw of the rover [rad]. + * @param max_steering Maximum steering angle of the rover [rad]. + * @return Steering setpoint for the rover [rad]. */ - void updateParams() override; + float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering); + + /** + * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between + * the desired/actual speed and a feedforward term based on the full throttle speed. + * @param pid_throttle Reference to PID instance. + * @param desired_speed Reference speed for the rover [m/s]. + * @param actual_speed Actual speed of the rover [m/s]. + * @param max_speed Rover speed at full throttle [m/s]. + * @param dt Time interval since last update [s]. + * @return Normalized throttle setpoint [0, 1]. + */ + float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt); -private: // uORB subscriptions uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -138,27 +186,31 @@ class RoverAckermannGuidance : public ModuleParams uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates. + // Class instances + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates PurePursuit _pure_pursuit{this}; // Pure pursuit library // Rover variables + float _desired_steering{0.f}; + float _vehicle_yaw{0.f}; + float _desired_speed{0.f}; + float _actual_speed{0.f}; Vector2d _curr_pos{}; Vector2f _curr_pos_ned{}; PID_t _pid_throttle; hrt_abstime _timestamp{0}; - float _desired_steering{0.f}; // Waypoint variables - Vector2d _curr_wp{}; - Vector2d _next_wp{}; - Vector2d _prev_wp{}; Vector2d _home_position{}; Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; + float _distance_to_prev_wp{0.f}; + float _distance_to_curr_wp{0.f}; + float _distance_to_next_wp{0.f}; float _acceptance_radius{0.5f}; float _prev_acceptance_radius{0.5f}; + bool _mission_finished{false}; // Parameters DEFINE_PARAMETERS(