/// <summary> /// used to injecy data into the gps ie rtcm/sbp/ubx /// </summary> /// <param name="data"></param> public void InjectGpsData(byte[] data, byte length) { mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t(); var len = (length % 128) == 0 ? length / 128 : (length / 128) + 1; for (int a = 0; a < len; a++) { gps.data = new byte[110]; int copy = Math.Min(length - a*110, 110); Array.Copy(data, a * 110, gps.data, 0, copy); gps.len = (byte)copy; gps.target_component = MAV.compid; gps.target_system = MAV.sysid; generatePacket((byte) MAVLINK_MSG_ID.GPS_INJECT_DATA, gps); } }
/// <summary> /// used to injecy data into the gps ie rtcm/sbp/ubx /// </summary> /// <param name="data"></param> public void InjectGpsData(byte[] data, byte length) { mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t(); gps.data = new byte[110]; Array.Copy(data, gps.data, length); gps.len = length; gps.target_component = MAV.compid; gps.target_system = MAV.sysid; generatePacket((byte) MAVLINK_MSG_ID.GPS_INJECT_DATA, gps); }
/// <summary> /// used to inject data into the gps ie rtcm/sbp/ubx /// </summary> /// <param name="data"></param> public void InjectGpsData(byte sysid, byte compid, byte[] data, byte length, bool rtcm_message = true) { // new message if (rtcm_message) { mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t(); var msglen = 180; if (length > msglen * 4) log.Error("Message too large " + length); // number of packets we need, not including a termination packet if needed var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; // flags = isfrag(1)/frag(2)/seq(5) for (int a = 0; a < len; a++) { // check if its a fragment if (len > 1) gps.flags = 1; else gps.flags = 0; // add fragment number gps.flags += (byte)((a & 0x3) << 1); // add seq number gps.flags += (byte)((inject_seq_no & 0x1f) << 3); // create the empty buffer gps.data = new byte[msglen]; // calc how much data we are copying int copy = Math.Min(length - a * msglen, msglen); // copy the data Array.Copy(data, a * msglen, gps.data, 0, copy); // set the length gps.len = (byte)copy; generatePacket((byte) MAVLINK_MSG_ID.GPS_RTCM_DATA, gps, sysid, compid); } // packet is perfectly aligned and larger than one packet, send termination packet if ((length % msglen) == 0 && (length > msglen)) { gps.len = 0; gps.data = new byte[msglen]; generatePacket((byte)MAVLINK_MSG_ID.GPS_RTCM_DATA, gps, sysid, compid); } inject_seq_no++; } else { mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t(); var msglen = 110; var len = (length%msglen) == 0 ? length/msglen : (length/msglen) + 1; for (int a = 0; a < len; a++) { gps.data = new byte[msglen]; int copy = Math.Min(length - a*msglen, msglen); Array.Copy(data, a*msglen, gps.data, 0, copy); gps.len = (byte) copy; gps.target_component = compid; gps.target_system = sysid; generatePacket((byte) MAVLINK_MSG_ID.GPS_INJECT_DATA, gps, sysid, compid); } } }