diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp index 6833d510b669be28fa412efa80bef79ec35eda69..8ba014793dbe8e750676bffcd833a5301b4b2778 100644 --- a/src/boards/Groundstation/Automated/Hub.cpp +++ b/src/boards/Groundstation/Automated/Hub.cpp @@ -75,7 +75,6 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) static const std::map<MavCommandList, Events> commandToEvent{ {MAV_CMD_FORCE_INIT, TMTC_ARP_FORCE_INIT}, {MAV_CMD_RESET_ALGORITHM, TMTC_ARP_RESET_ALGORITHM}, - {MAV_CMD_FORCE_REBOOT, TMTC_ARP_RESET_BOARD}, {MAV_CMD_ARP_FORCE_NO_FEEDBACK, TMTC_ARP_FORCE_NO_FEEDBACK}, {MAV_CMD_ARM, TMTC_ARP_ARM}, {MAV_CMD_DISARM, TMTC_ARP_DISARM}, @@ -88,6 +87,11 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) MavCommandList commandId = static_cast<MavCommandList>( mavlink_msg_arp_command_tc_get_command_id(&msg)); + if (commandId == MAV_CMD_FORCE_REBOOT) + { + reboot(); + } + auto it = commandToEvent.find(commandId); if (it != commandToEvent.end()) diff --git a/src/boards/Groundstation/Automated/SMA/SMA.cpp b/src/boards/Groundstation/Automated/SMA/SMA.cpp index c14fbf72303055ba9348456fb4b7715eefd8e2f6..7ae4e1da99c375e38c16d9c56137f0664dd1405e 100644 --- a/src/boards/Groundstation/Automated/SMA/SMA.cpp +++ b/src/boards/Groundstation/Automated/SMA/SMA.cpp @@ -360,11 +360,6 @@ State SMA::state_config(const Event& event) { return transition(&SMA::state_init); } - case TMTC_ARP_RESET_BOARD: - { - reboot(); - return HANDLED; - } default: { return UNHANDLED;