public bool doReboot(bool bootloadermode = false, bool currentvehicle = true)
{
int param1 = 1;
if (bootloadermode)
{
param1 = 3;
}
// reboot the current selected mav
if (currentvehicle)
{
doCommand(MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, param1, 0, 0, 0, 0, 0, 0);
doCommand(MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 0, 0, 0, 0, 0);
}
else
{
// scan for hb on unknown mav
MAVLinkMessage buffer = getHeartBeat();
if (buffer.Length > 5)
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
mavlinkversion = hb.mavlink_version;
MAV.aptype = (MAV_TYPE)hb.type;
MAV.apname = (MAV_AUTOPILOT)hb.autopilot;
MAV.sysid = buffer.sysid;
MAV.compid = buffer.compid;
}
// reboot if we have seen hb
if (MAV.sysid != 0 && MAV.compid != 0)
{
doCommand(MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, param1, 0, 0, 0, 0, 0, 0);
doCommand(MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 0, 0, 0, 0, 0);
}
}
giveComport = false;
return true;
}