From e31e496418def90f7c697500f24559aabf3bdc5b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 24 Aug 2021 10:32:13 -0400 Subject: [PATCH] msg/position_setpoint.msg remove obsolete fields --- msg/position_setpoint.msg | 53 +++++++------------ .../tasks/Auto/FlightTaskAuto.cpp | 28 +--------- .../tasks/Auto/FlightTaskAuto.hpp | 4 -- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 13 ----- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 1 - .../TargetEstimator.hpp | 6 ++- src/modules/navigator/loiter.cpp | 2 - src/modules/navigator/mission.cpp | 8 --- src/modules/navigator/navigator_main.cpp | 1 - .../RoverPositionControl.cpp | 2 +- 10 files changed, 25 insertions(+), 93 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index a3a1668a46f6..75c100469266 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -1,45 +1,30 @@ # this file is only used in the position_setpoint triple as a dependency -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint8 SETPOINT_TYPE_POSITION=0 # position setpoint -uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint -uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint -uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint -uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing -uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) -uint8 SETPOINT_TYPE_FOLLOW_TARGET=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target +uint8 SETPOINT_TYPE_POSITION = 0 # position setpoint +uint8 SETPOINT_TYPE_LOITER = 1 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF = 2 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND = 3 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE = 4 # do nothing, switch off motors or keep at idle speed (MC) -uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED -uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED +uint8 type # setpoint type to adjust behavior of position controller -bool valid # true if setpoint is valid -uint8 type # setpoint type to adjust behavior of position controller +bool valid # true if setpoint is valid -float32 vx # local velocity setpoint in m/s in NED -float32 vy # local velocity setpoint in m/s in NED -float32 vz # local velocity setpoint in m/s in NED -bool velocity_valid # true if local velocity setpoint valid -uint8 velocity_frame # to set velocity setpoints in NED or body -bool alt_valid # do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control. +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m -float64 lat # latitude, in deg -float64 lon # longitude, in deg -float32 alt # altitude AMSL, in m -float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw -bool yaw_valid # true if yaw setpoint valid +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid -float32 yawspeed # yawspeed (only for multirotors, in rad/s) -bool yawspeed_valid # true if yawspeed setpoint valid +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW -int8 landing_gear # landing gear: see definition of the states in landing_gear.msg +float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation -float32 loiter_radius # loiter radius (only for fixed wing), in m -int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) -float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation - -float32 cruising_speed # the generally desired cruising speed (not a hard constraint) -float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) - -bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind +bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index d2a3d53609e3..04675e7db9fb 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -262,16 +262,13 @@ bool FlightTaskAuto::_evaluateTriplets() if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) { _updateInternalWaypoints(); - _mission_gear = _sub_triplet_setpoint.get().current.landing_gear; _yaw_lock = false; } if (_param_com_obs_avoid.get() && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN, + _triplet_next_wp, _sub_triplet_setpoint.get().next.yaw, (float)NAN, _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); } @@ -290,12 +287,6 @@ bool FlightTaskAuto::_evaluateTriplets() _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); } - - - } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) { - _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; - _yaw_setpoint = NAN; - } else { if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) && _sub_triplet_setpoint.get().current.yaw_valid) { @@ -419,23 +410,6 @@ void FlightTaskAuto::_setDefaultConstraints() } } -Vector2f FlightTaskAuto::_getTargetVelocityXY() -{ - // guard against any bad velocity values - const float vx = _sub_triplet_setpoint.get().current.vx; - const float vy = _sub_triplet_setpoint.get().current.vy; - bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && - _sub_triplet_setpoint.get().current.velocity_valid; - - if (velocity_valid) { - return Vector2f(vx, vy); - - } else { - // just return zero speed - return Vector2f{}; - } -} - State FlightTaskAuto::_getCurrentState() { // Calculate the vehicle current state based on the Navigator triplets and the current position. diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 55b8aea4d5c8..72f50677ee26 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -61,12 +61,10 @@ */ enum class WaypointType : int { position = position_setpoint_s::SETPOINT_TYPE_POSITION, - velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY, loiter = position_setpoint_s::SETPOINT_TYPE_LOITER, takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF, land = position_setpoint_s::SETPOINT_TYPE_LAND, idle = position_setpoint_s::SETPOINT_TYPE_IDLE, - follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET, }; enum class State { @@ -93,7 +91,6 @@ class FlightTaskAuto : public FlightTask protected: void _setDefaultConstraints() override; - matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ @@ -111,7 +108,6 @@ class FlightTaskAuto : public FlightTask State _current_state{State::none}; float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ - int _mission_gear{landing_gear_s::GEAR_KEEP}; float _yaw_sp_prev{NAN}; bool _yaw_sp_aligned{false}; diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 0699af4d379e..ad0de115bf87 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -91,10 +91,6 @@ bool FlightTaskAutoMapper::update() _prepareTakeoffSetpoints(); break; - case WaypointType::velocity: - _prepareVelocitySetpoints(); - break; - default: _preparePositionSetpoints(); break; @@ -148,15 +144,6 @@ void FlightTaskAutoMapper::_prepareTakeoffSetpoints() _gear.landing_gear = landing_gear_s::GEAR_DOWN; } -void FlightTaskAutoMapper::_prepareVelocitySetpoints() -{ - // XY Velocity waypoint - // TODO : Rewiew that. What is the expected behavior? - _position_setpoint = Vector3f(NAN, NAN, _position(2)); - Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; - _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); -} - void FlightTaskAutoMapper::_preparePositionSetpoints() { // Simple waypoint navigation: go to xyz target, with standard limitations diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 678d45f6b1ea..150ab620864d 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -57,7 +57,6 @@ class FlightTaskAutoMapper : public FlightTaskAuto void _prepareIdleSetpoints(); void _prepareLandSetpoints(); - void _prepareVelocitySetpoints(); void _prepareTakeoffSetpoints(); void _preparePositionSetpoints(); diff --git a/src/modules/follow_target_estimator/TargetEstimator.hpp b/src/modules/follow_target_estimator/TargetEstimator.hpp index ab0e3670493d..4ce78c98eb9f 100644 --- a/src/modules/follow_target_estimator/TargetEstimator.hpp +++ b/src/modules/follow_target_estimator/TargetEstimator.hpp @@ -53,11 +53,13 @@ #include #include -static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS = 3000.0f; // Duration after which the connection to the target is considered lost +static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS = + 3000.0f; // Duration after which the connection to the target is considered lost static constexpr float MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS = 500.0f; static constexpr float MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS = 100.0f; static constexpr float ACCELERATION_SATURATION = 20.0f; // 2*g -static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING = 0.1f; // speed threshold above which the target is considered to be moving +static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING = + 0.1f; // speed threshold above which the target is considered to be moving using namespace time_literals; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index fdfb0b08a9e5..0e33b847009f 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -105,7 +105,6 @@ Loiter::set_loiter_position() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->previous.valid = false; mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -130,7 +129,6 @@ Loiter::reposition() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading; pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 389aa795a26b..4f645abe5ad7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1863,19 +1863,11 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit { return ((p1->valid == p2->valid) && (p1->type == p2->type) && - (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && - (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && - (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (p1->velocity_valid == p2->velocity_valid) && - (p1->velocity_frame == p2->velocity_frame) && - (p1->alt_valid == p2->alt_valid) && (fabs(p1->lat - p2->lat) < DBL_EPSILON) && (fabs(p1->lon - p2->lon) < DBL_EPSILON) && (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && (p1->loiter_direction == p2->loiter_direction) && (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a215a9f6abac..92c30815e3e0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -822,7 +822,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) rep->current.alt = loiter_altitude_amsl; rep->current.valid = true; rep->current.loiter_radius = get_loiter_radius(); - rep->current.alt_valid = true; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; rep->current.loiter_direction = 1; rep->current.cruising_throttle = get_cruising_throttle(); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index f45ac9e189b6..21bdf1ccd9f1 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -349,7 +349,7 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) Vector3f desired_body_velocity; - if (_velocity_frame == VelocityFrame::NED) { + if (_velocity_frame == VelocityFrame::BODY) { desired_body_velocity = desired_velocity; } else {