add support for DO_CHANGE_ALTITUDE

Do the same as DO_REPOSITION wit only the altitude field populated
and MAV_DO_REPOSITION_FLAGS set, which means:
- switch to Loiter mode if not already in it
- set the current altitude to what is specified in the altitdue field,
keep current altitude setpoint otherwise
- keep current position setpoint
- fall back to current estimated position in case a position setpoint
is not finite

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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;