diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 29fa8ef154..13ac224582 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -39,6 +39,7 @@ uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desir uint16 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4480765169..6258e3f827 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -718,6 +718,30 @@ Commander::handle_command(const vehicle_command_s &cmd) } break; + case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE: { + + // Just switch the flight mode here, the navigator takes care of + // doing something sensible with the coordinates. Its designed + // to not require navigator and command to receive / process + // the data at the exact same time. + + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Change altitude command rejected\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_change_altitude_rejected"), + {events::Log::Error, events::LogInternal::Info}, + "Change altitude command rejected"); + } + + } + break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd.param1; uint8_t custom_main_mode = (uint8_t)cmd.param2; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3a561083cf..7d76bc44b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -370,6 +370,83 @@ void Navigator::run() // CMD_DO_REPOSITION is acknowledged by commander + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE + && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // only update the setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter, + // which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too) + + // A VEHICLE_CMD_DO_CHANGE_ALTITUDE has the exact same effect as a VEHICLE_CMD_DO_REPOSITION with only the altitude + // field populated, this logic is copied from above. + + // only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl + + bool change_altitude_valid = true; + + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = get_global_position()->lat; + position_setpoint.lon = get_global_position()->lon; + position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + + if (have_geofence_position_data) { + change_altitude_valid = geofence_allows_position(position_setpoint); + } + + if (change_altitude_valid) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); + + // store current position as previous position and goal as next + rep->previous.yaw = get_local_position()->heading; + rep->previous.lat = get_global_position()->lat; + rep->previous.lon = get_global_position()->lon; + rep->previous.alt = get_global_position()->alt; + + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + rep->current.cruising_speed = get_cruising_speed(); + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.yaw = NAN; + rep->current.yaw_valid = false; + + // Position is not changing, thus we keep the setpoint + rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat; + rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon; + + // set the altitude corresponding to command + rep->current.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { + + calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw); + rep->current.yaw_valid = true; + } + + if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { + rep->current.loiter_radius = curr->current.loiter_radius; + + } else { + rep->current.loiter_radius = get_loiter_radius(); + } + + rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; + + rep->previous.timestamp = hrt_absolute_time(); + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + rep->next.valid = false; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t"); + events::send(events::ID("navigator_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info}, + "Altitude change is outside geofence"); + } + + // DO_CHANGE_ALTITUDE 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) {