diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6f2814c9a0..9a9ead35ca 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1211,13 +1211,28 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: - // Switch to orbit state and let the orbit task handle the command further - if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags, - _internal_state)) { + transition_result_t main_ret; + + if (_status.in_transition_mode) { + main_ret = TRANSITION_DENIED; + + } else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // for fixed wings the behavior of orbit is the same as loiter + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, + _status_flags, _internal_state); + + } else { + // Switch to orbit state and let the orbit task handle the command further + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags, + _internal_state); + } + + if ((main_ret != TRANSITION_DENIED)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected"); } break; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index ef268801bd..23b2e3abee 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -42,7 +42,7 @@ #pragma once #include "FlightTaskManualAltitudeSmoothVel.hpp" -#include +#include #include #include "StraightLine.hpp" #include @@ -114,7 +114,7 @@ private: float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; - uORB::Publication _orbit_status_pub{ORB_ID(orbit_status)}; + uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d2e6835e47..2811b45e9f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -763,6 +763,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c _att_sp.pitch_reset_integral = false; _att_sp.yaw_reset_integral = false; + if (pos_sp_curr.valid && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + publishOrbitStatus(pos_sp_curr); + } + const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr); _type = position_sp_type; // for publication @@ -2200,6 +2204,19 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo tecs_status_publish(); } +void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp) +{ + orbit_status_s orbit_status{}; + orbit_status.timestamp = hrt_absolute_time(); + orbit_status.radius = static_cast(pos_sp.loiter_direction) * pos_sp.loiter_radius; + orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL + orbit_status.x = static_cast(pos_sp.lat); + orbit_status.y = static_cast(pos_sp.lon); + orbit_status.z = pos_sp.alt; + orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE; + _orbit_status_pub.publish(orbit_status); +} + int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { bool vtol = false; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 58984b5e5e..60cba3c122 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include #include @@ -156,6 +158,7 @@ private: uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication + uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items @@ -349,6 +352,8 @@ private: Vector2f get_nav_speed_2d(const Vector2f &ground_speed); void set_control_mode_current(bool pos_sp_curr_valid); + void publishOrbitStatus(const position_setpoint_s pos_sp); + /* * Call TECS : a wrapper function to call the TECS implementation */ diff --git a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp index ddf529dafb..70e53f49d9 100644 --- a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp +++ b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp @@ -49,34 +49,40 @@ public: unsigned get_size() override { - return _orbit_status_sub.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _orbit_status_subs.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _orbit_status_sub{ORB_ID(orbit_status)}; + uORB::SubscriptionMultiArray _orbit_status_subs{ORB_ID::orbit_status}; bool send() override { orbit_status_s orbit_status; + bool updated = false; - if (_orbit_status_sub.update(&orbit_status)) { - mavlink_orbit_execution_status_t msg{}; + for (auto &orbit_sub : _orbit_status_subs) { - msg.time_usec = orbit_status.timestamp; - msg.radius = orbit_status.radius; - msg.frame = orbit_status.frame; - msg.x = orbit_status.x * 1e7; - msg.y = orbit_status.y * 1e7; - msg.z = orbit_status.z; + if (orbit_sub.update(&orbit_status)) { + updated = true; + mavlink_orbit_execution_status_t msg_orbit_execution_status{}; - mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &msg); + msg_orbit_execution_status.time_usec = orbit_status.timestamp; + msg_orbit_execution_status.radius = orbit_status.radius; + msg_orbit_execution_status.frame = orbit_status.frame; + msg_orbit_execution_status.x = orbit_status.x * 1e7; + msg_orbit_execution_status.y = orbit_status.y * 1e7; + msg_orbit_execution_status.z = orbit_status.z; - return true; + mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &msg_orbit_execution_status); + + // only one subscription should ever be active at any time, so we can exit here + break; + } } - return false; + return updated; } }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index e1549e8b92..6d112d31eb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -441,4 +441,6 @@ private: void publish_mission_result(); void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); + + bool geofence_allows_position(const vehicle_global_position_s &pos); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fd73bf53ee..b448b7823b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -247,26 +247,13 @@ Navigator::run() bool reposition_valid = true; - if (have_geofence_position_data && - ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && - (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) { + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = cmd.param5; + position_setpoint.lon = cmd.param6; + position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; - if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { - - vehicle_global_position_s test_reposition_validity {}; - test_reposition_validity.lat = cmd.param5; - test_reposition_validity.lon = cmd.param6; - - if (PX4_ISFINITE(cmd.param7)) { - test_reposition_validity.alt = cmd.param7; - - } else { - test_reposition_validity.alt = get_global_position()->alt; - } - - reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos, - home_position_valid()); - } + if (have_geofence_position_data) { + reposition_valid = geofence_allows_position(position_setpoint); } if (reposition_valid) { @@ -279,10 +266,11 @@ Navigator::run() rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; - rep->current.loiter_radius = get_loiter_radius(); - rep->current.loiter_direction = 1; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + bool only_alt_change_requested = false; + // If no argument for ground speed, use default value. if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { rep->current.cruising_speed = get_cruising_speed(); @@ -321,6 +309,7 @@ Navigator::run() rep->current.lat = curr->current.lat; rep->current.lon = curr->current.lon; rep->current.alt = cmd.param7; + only_alt_change_requested = true; } else { // All three set to NaN - pause vehicle @@ -354,6 +343,15 @@ Navigator::run() } } + if (only_alt_change_requested && PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) { + rep->current.loiter_radius = curr->current.loiter_radius; + rep->current.loiter_direction = curr->current.loiter_direction; + + } else { + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + } + rep->previous.valid = true; rep->previous.timestamp = hrt_absolute_time(); @@ -370,6 +368,45 @@ Navigator::run() // CMD_DO_REPOSITION is acknowledged by commander + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT && + get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + // for multicopters the orbit command is directly executed by the orbit flighttask + + bool orbit_location_valid = true; + + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat; + position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon; + position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; + + if (have_geofence_position_data) { + orbit_location_valid = geofence_allows_position(position_setpoint); + } + + if (orbit_location_valid) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + rep->current.cruising_throttle = get_cruising_throttle(); + + if (PX4_ISFINITE(cmd.param1)) { + rep->current.loiter_radius = fabsf(cmd.param1); + rep->current.loiter_direction = math::signNoZero(cmd.param1); + } + + rep->current.lat = position_setpoint.lat; + rep->current.lon = position_setpoint.lon; + rep->current.alt = position_setpoint.alt; + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Orbit is outside geofence"); + } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { position_setpoint_triplet_s *rep = get_takeoff_triplet(); @@ -1512,6 +1549,20 @@ Navigator::release_gimbal_control() publish_vehicle_cmd(&vcmd); } +bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) +{ + if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) { + + if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon)) { + return _geofence.check(pos, _gps_pos, _home_pos, + home_position_valid()); + } + } + + return true; +} + int Navigator::print_usage(const char *reason) { if (reason) {