diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index a535f02d49..236b56c838 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -56,7 +56,6 @@ uint32 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3 # param value for VEHI uint32 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| uint32 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| -uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45f9e6a4da..29bf2860b4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -945,38 +945,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - case vehicle_command_s::VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) - - // Increase by 0.5f and rely on the integer cast - // implicit floor(). This is the *safest* way to - // convert from floats representing small ints to actual ints. - unsigned int mav_goto = (cmd->param1 + 0.5f); - - if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(&mavlink_log_pub, "Pause mission cmd"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(&mavlink_log_pub, "Continue mission cmd"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(&mavlink_log_pub, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); - } - } - break; - - /* Flight termination */ case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 1.5f) { armed_local->lockdown = true;