diff --git a/app/telemetry/action/create_cmd_helper.hpp b/app/telemetry/action/create_cmd_helper.hpp index cc3ae2b0f..136c3d011 100644 --- a/app/telemetry/action/create_cmd_helper.hpp +++ b/app/telemetry/action/create_cmd_helper.hpp @@ -6,13 +6,18 @@ // Here we have various util methods to create mavlink_command_long_t commands namespace cmd::helper{ -static mavlink_command_long_t create_cmd_reboot(int target_sysid,int target_compid,bool reboot){ +// @Param: companion_computer: If set to true the message generated is for openhd +// otherwise,it is for the FC +static mavlink_command_long_t create_cmd_reboot(int target_sysid,int target_compid,bool reboot,bool companion_computer){ mavlink_command_long_t cmd{}; cmd.target_system=target_sysid; cmd.target_component=target_compid; cmd.command=MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN; - cmd.param1=(reboot ? 1 : 2); - //cmd.param2=(reboot ? 1 : 2); + if(companion_computer){ + cmd.param2=(reboot ? 1 : 2); + }else{ + cmd.param1=(reboot ? 1 : 2); + } return cmd; } diff --git a/app/telemetry/action/fcaction.cpp b/app/telemetry/action/fcaction.cpp index 140878d87..37c478314 100644 --- a/app/telemetry/action/fcaction.cpp +++ b/app/telemetry/action/fcaction.cpp @@ -177,7 +177,7 @@ void FCAction::request_home_position_from_fc() bool FCAction::send_command_reboot(bool reboot) { const auto fc_id=MavlinkTelemetry::instance().get_fc_mav_id(); - auto command=cmd::helper::create_cmd_reboot(fc_id.sys_id,fc_id.comp_id,reboot); + auto command=cmd::helper::create_cmd_reboot(fc_id.sys_id,fc_id.comp_id,reboot,false); const auto res=CmdSender::instance().send_command_long_blocking(command); return res==CmdSender::Result::CMD_SUCCESS; } diff --git a/app/telemetry/action/ohdaction.cpp b/app/telemetry/action/ohdaction.cpp index 15a279415..44042545d 100644 --- a/app/telemetry/action/ohdaction.cpp +++ b/app/telemetry/action/ohdaction.cpp @@ -20,14 +20,14 @@ OHDAction& OHDAction::instance() bool OHDAction::send_command_reboot_air(bool reboot) { - auto command=cmd::helper::create_cmd_reboot(OHD_SYS_ID_AIR,MAV_COMP_ID_ONBOARD_COMPUTER,reboot); + auto command=cmd::helper::create_cmd_reboot(OHD_SYS_ID_AIR,MAV_COMP_ID_ONBOARD_COMPUTER,reboot,true); const auto res=CmdSender::instance().send_command_long_blocking(command); return res==CmdSender::Result::CMD_SUCCESS; } bool OHDAction::send_command_reboot_gnd(bool reboot) { - auto command=cmd::helper::create_cmd_reboot(OHD_SYS_ID_GROUND,MAV_COMP_ID_ONBOARD_COMPUTER,reboot); + auto command=cmd::helper::create_cmd_reboot(OHD_SYS_ID_GROUND,MAV_COMP_ID_ONBOARD_COMPUTER,reboot,true); const auto res=CmdSender::instance().send_command_long_blocking(command); return res==CmdSender::Result::CMD_SUCCESS; }