public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { giveComport = true; byte[] buffer; mavlink_command_long_t req = new mavlink_command_long_t(); req.target_system = MAV.sysid; req.target_component = MAV.compid; if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) { req.target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; } req.command = (ushort)actionid; req.param1 = p1; req.param2 = p2; req.param3 = p3; req.param4 = p4; req.param5 = p5; req.param6 = p6; req.param7 = p7; generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req); DateTime start = DateTime.Now; int retrys = 3; int timeout = 2000; // imu calib take a little while if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) { // this is for advanced accel offsets, and blocks execution return true; } else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) { retrys = 1; timeout = 25000; } else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) { generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req); giveComport = false; return true; } else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) { // 10 seconds as may need an imu calib timeout = 10000; } while (true) { if (!(start.AddMilliseconds(timeout) > DateTime.Now)) { if (retrys > 0) { log.Info("doAction Retry " + retrys); generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - doAction"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == (byte)MAVLINK_MSG_ID.COMMAND_ACK) { var ack = buffer.ByteArrayToStructure<mavlink_command_ack_t>(6); if (ack.result == (byte)MAV_RESULT.ACCEPTED) { giveComport = false; return true; } else { giveComport = false; return false; } } } } }
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { giveComport = true; MAVLinkMessage buffer; mavlink_command_long_t req = new mavlink_command_long_t(); req.target_system = MAV.sysid; req.target_component = MAV.compid; req.command = (ushort) actionid; req.param1 = p1; req.param2 = p2; req.param3 = p3; req.param4 = p4; req.param5 = p5; req.param6 = p6; req.param7 = p7; log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6, p7); generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req); DateTime start = DateTime.Now; int retrys = 3; int timeout = 2000; // imu calib take a little while if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) { // this is for advanced accel offsets, and blocks execution return true; } else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) { retrys = 1; timeout = 25000; } else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) { generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req); giveComport = false; return true; } else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) { // 10 seconds as may need an imu calib timeout = 10000; } else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p6 == 1) { // compassmot // send again just incase generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req); giveComport = false; return true; } else if (actionid == MAV_CMD.GET_HOME_POSITION) { return true; } while (true) { if (!(start.AddMilliseconds(timeout) > DateTime.Now)) { if (retrys > 0) { log.Info("doAction Retry " + retrys); generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - doAction"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK) { var ack = buffer.ToStructure<mavlink_command_ack_t>(); if (ack.result == (byte) MAV_RESULT.ACCEPTED) { giveComport = false; return true; } else { giveComport = false; return false; } } } } }
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { MainV2.giveComport = true; byte[] buffer; mavlink_command_long_t req = new mavlink_command_long_t(); req.target_system = sysid; req.target_component = compid; if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) { req.target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; } req.command = (ushort)actionid; req.param1 = p1; req.param2 = p2; req.param3 = p3; req.param4 = p4; req.param5 = p5; req.param6 = p6; req.param7 = p7; generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req); DateTime start = DateTime.Now; int retrys = 3; int timeout = 2000; // imu calib take a little while if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) { retrys = 1; timeout = 25000; } while (true) { if (!(start.AddMilliseconds(timeout) > DateTime.Now)) { if (retrys > 0) { log.Info("doAction Retry " + retrys); generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req); start = DateTime.Now; retrys--; continue; } MainV2.giveComport = false; throw new Exception("Timeout on read - doAction"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_COMMAND_ACK) { var ack = buffer.ByteArrayToStructure<mavlink_command_ack_t>(6); if (ack.result == (byte)MAV_RESULT.ACCEPTED) { MainV2.giveComport = false; return true; } else { MainV2.giveComport = false; return false; } } } } #else MainV2.giveComport = true; byte[] buffer; mavlink_waypoint_set_current_t req = new mavlink_waypoint_set_current_t(); req.target_system = sysid; req.target_component = compid; req.seq = index; generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); DateTime start = DateTime.Now; int retrys = 5; while (true) { if (!(start.AddMilliseconds(2000) > DateTime.Now)) { if (retrys > 0) { log.Info("setWPCurrent Retry " + retrys); generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); start = DateTime.Now; retrys--; continue; } MainV2.giveComport = false; throw new Exception("Timeout on read - setWPCurrent"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT) { MainV2.giveComport = false; return true; } } } }