public void CheckMavLinkMessageSerialization() { MavLinkMessage sampleMessage = createSampleMessage(); MAVLink.mavlink_heartbeat_t sampleStruct = (MAVLink.mavlink_heartbeat_t)sampleMessage.data_struct; JsonSerializerSettings settings = new JsonSerializerSettings { TypeNameHandling = TypeNameHandling.All }; String serialized = JsonConvert.SerializeObject(sampleMessage, settings); MavLinkMessage deserialized = JsonConvert.DeserializeObject <MavLinkMessage>(serialized, settings); MAVLink.mavlink_heartbeat_t dstruct = (MAVLink.mavlink_heartbeat_t)deserialized.data_struct; Assert.AreEqual(sampleMessage.compid, deserialized.compid); Assert.AreEqual(sampleMessage.sysid, deserialized.sysid); Assert.AreEqual(sampleMessage.seq, deserialized.seq); Assert.AreEqual(sampleMessage.messid, deserialized.messid); Assert.AreEqual(sampleStruct.autopilot, dstruct.autopilot); Assert.AreEqual(sampleStruct.base_mode, dstruct.base_mode); Assert.AreEqual(sampleStruct.custom_mode, dstruct.custom_mode); Assert.AreEqual(sampleStruct.mavlink_version, dstruct.mavlink_version); Assert.AreEqual(sampleStruct.system_status, dstruct.system_status); Assert.AreEqual(sampleStruct.type, dstruct.type); }
public MessageContainerBase(MavLinkMessage message) { // pass in null to this constructor and map fields in inherited constructor for ~20x speed increase // see Heartbeat.cs for example if (null != message) { this.message = message; if (message.messid.Equals(MessageID)) { try { MethodInfo method = typeof(MessageContainerBase).GetMethod("copyMessageValues", (BindingFlags.NonPublic | BindingFlags.Instance)); MethodInfo generic = method.MakeGenericMethod(getStructType()); generic.Invoke(this, null); } catch (Exception e) { // database logging is crazy slow in events logger.Trace("Unable to parse data for {1} object, with exception message {0}", e.Message, MessageID); } } else { // database logging is crazy slow in events logger.Trace("Tried to initialize {1} object with message of type {0}", (MAVLink.MAVLINK_MSG_ID)message.messid, MessageID); } } else { // database logging is crazy slow in events logger.Trace("Tried to initialize {1} object with null message", MessageID); } }
public void CheckTerrainReportObject() { MAVLink.mavlink_terrain_report_t data = new MAVLink.mavlink_terrain_report_t(); data.current_height = 1; data.lat = 2; data.loaded = 3; data.lon = 4; data.pending = 5; data.spacing = 6; data.terrain_height = 7; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT, data); TerrainReport obj = new TerrainReport(message); Assert.AreEqual(data.current_height, obj.current_height); Assert.AreEqual(data.lat, obj.lat); Assert.AreEqual(data.loaded, obj.loaded); Assert.AreEqual(data.lon, obj.lon); Assert.AreEqual(data.pending, obj.pending); Assert.AreEqual(data.spacing, obj.spacing); Assert.AreEqual(data.terrain_height, obj.terrain_height); TerrainReportDTO dto = DTOFactory.createTerrainReportDTO(obj); Assert.AreEqual(dto.current_height, obj.current_height); Assert.AreEqual(dto.lat, obj.lat); Assert.AreEqual(dto.loaded, obj.loaded); Assert.AreEqual(dto.lon, obj.lon); Assert.AreEqual(dto.pending, obj.pending); Assert.AreEqual(dto.spacing, obj.spacing); Assert.AreEqual(dto.terrain_height, obj.terrain_height); }
public void CheckHeartbeatObject() { MavLinkMessage message = createSampleMessage(); MAVLink.mavlink_heartbeat_t sampleStruct = (MAVLink.mavlink_heartbeat_t)message.data_struct; Heartbeat heartbeatContainer = new Heartbeat(message); Assert.AreEqual((MAVLink.MAV_AUTOPILOT)sampleStruct.autopilot, heartbeatContainer.autopilot); //Assert.AreEqual((MAVLink.MAV_MODE_FLAG)sampleStruct.base_mode, heartbeatContainer.base_mode); Assert.AreEqual(sampleStruct.custom_mode, heartbeatContainer.custom_mode); Assert.AreEqual(sampleStruct.mavlink_version, heartbeatContainer.mavlink_version); Assert.AreEqual((MAVLink.MAV_STATE)sampleStruct.system_status, heartbeatContainer.system_status); Assert.AreEqual((MAVLink.MAV_TYPE)sampleStruct.type, heartbeatContainer.type); HeartbeatDTO dto = DTOFactory.createHeartbeatDTO(heartbeatContainer); Assert.AreEqual(dto.autopilot, heartbeatContainer.autopilot.ToString()); Assert.AreEqual(dto.base_mode.Count, heartbeatContainer.base_mode.Count); Assert.AreEqual(dto.custom_mode, heartbeatContainer.custom_mode); Assert.AreEqual(dto.mavlink_version, heartbeatContainer.mavlink_version); Assert.AreEqual(dto.system_status, heartbeatContainer.system_status.ToString()); Assert.AreEqual(dto.type, heartbeatContainer.type.ToString()); String json = JsonConvert.SerializeObject(dto); }
public void CheckVfrHudObject() { MAVLink.mavlink_vfr_hud_t data = new MAVLink.mavlink_vfr_hud_t(); data.airspeed = 1; data.alt = 2; data.climb = 3; data.groundspeed = 4; data.heading = 5; data.throttle = 6; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.VFR_HUD, data); VfrHud obj = new VfrHud(message); Assert.AreEqual(data.airspeed, obj.airspeed); Assert.AreEqual(data.alt, obj.alt); Assert.AreEqual(data.climb, obj.climb); Assert.AreEqual(data.groundspeed, obj.groundspeed); Assert.AreEqual(data.heading, obj.heading); Assert.AreEqual(data.throttle, obj.throttle); VfrHudDTO dto = DTOFactory.createVfrHudDTO(obj); Assert.AreEqual(dto.airspeed, obj.airspeed); Assert.AreEqual(dto.alt, obj.alt); Assert.AreEqual(dto.climb, obj.climb); Assert.AreEqual(dto.groundspeed, obj.groundspeed); Assert.AreEqual(dto.heading, obj.heading); Assert.AreEqual(dto.throttle, obj.throttle); }
public SystemStatus(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { MAVLink.mavlink_sys_status_t raw_data = (MAVLink.mavlink_sys_status_t)message.data_struct; this.voltage_battery = raw_data.voltage_battery; this.current_battery = raw_data.current_battery; this.battery_remaining = raw_data.battery_remaining; this.drop_rate_comm = raw_data.drop_rate_comm; this.errors_comm = raw_data.errors_comm; this.errors_count1 = raw_data.errors_count1; this.errors_count2 = raw_data.errors_count2; this.errors_count3 = raw_data.errors_count3; this.errors_count4 = raw_data.errors_count4; // parse bit masks into lists IEnumerable <MAVLink.MAV_SYS_STATUS_SENSOR> values = EnumValues.GetValues <MAVLink.MAV_SYS_STATUS_SENSOR>(); foreach (MAVLink.MAV_SYS_STATUS_SENSOR sensor in values) { uint sensorMask = (uint)sensor; if ((sensorMask & raw_data.onboard_control_sensors_enabled) == sensorMask) { sensorsEnabled.Add(sensor); } if ((sensorMask & raw_data.onboard_control_sensors_health) == sensorMask) { sensorsHealth.Add(sensor); } if ((sensorMask & raw_data.onboard_control_sensors_present) == sensorMask) { sensorsPresent.Add(sensor); } } } }
public void CheckAttitudeObject() { MAVLink.mavlink_attitude_t data = new MAVLink.mavlink_attitude_t(); data.pitch = 1; data.pitchspeed = 2; data.roll = 3; data.rollspeed = 4; data.time_boot_ms = 5; data.yaw = 6; data.yawspeed = 7; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.ATTITUDE, data); Attitude obj = new Attitude(message); Assert.AreEqual(data.pitch, obj.pitch); Assert.AreEqual(data.pitchspeed, obj.pitchspeed); Assert.AreEqual(data.roll, obj.roll); Assert.AreEqual(data.rollspeed, obj.rollspeed); Assert.AreEqual(data.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(data.yaw, obj.yaw); Assert.AreEqual(data.yawspeed, obj.yawspeed); AttitudeDTO dto = DTOFactory.createAttitudeDTO(obj); Assert.AreEqual(dto.pitch, obj.pitch); Assert.AreEqual(dto.pitchspeed, obj.pitchspeed); Assert.AreEqual(dto.roll, obj.roll); Assert.AreEqual(dto.rollspeed, obj.rollspeed); Assert.AreEqual(dto.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(dto.yaw, obj.yaw); Assert.AreEqual(dto.yawspeed, obj.yawspeed); }
public void CheckParameterValueObject() { MAVLink.mavlink_param_value_t data = new MAVLink.mavlink_param_value_t(); data.param_count = 1; data.param_id = Encoding.ASCII.GetBytes("foo"); data.param_index = 3; data.param_type = 4; data.param_value = 5; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.PARAM_VALUE, data); ParamValue obj = new ParamValue(message); Assert.AreEqual(obj.param_count, data.param_count); Assert.AreEqual(Encoding.ASCII.GetBytes(obj.param_id)[0], data.param_id[0]); Assert.AreEqual(obj.param_index, data.param_index); Assert.AreEqual((int)obj.param_type, data.param_type); Assert.AreEqual(obj.param_value, data.param_value); ParamValueDTO dto = DTOFactory.createParamValueDTO(obj); Assert.AreEqual(dto.param_count, obj.param_count); Assert.AreEqual(dto.param_id, obj.param_id); Assert.AreEqual(dto.param_index, obj.param_index); Assert.AreEqual(dto.param_type, obj.param_type.ToString()); Assert.AreEqual(dto.param_value, obj.param_value); }
void processMessage(MavLinkMessage message) { try { if (null == message) { logger.Error("Failed to parse MavLinkMessage from JSON in events callback"); return; } // store message in currentState Dictionary this.currentState[(MAVLink.MAVLINK_MSG_ID)message.messid] = message; // commands wait for their ack events and fetch them from these queues if (message.messid.Equals(MAVLink.MAVLINK_MSG_ID.COMMAND_ACK)) { CommandAck cmdack = new CommandAck(message); if (!this.commandAckStacks.ContainsKey(cmdack.command)) { this.commandAckStacks[cmdack.command] = new Stack <CommandAck>(); } lock (commandAckStacks[cmdack.command]) { this.commandAckStacks[cmdack.command].Push(cmdack); } } if (message.messid.Equals(MAVLink.MAVLINK_MSG_ID.HEARTBEAT)) { logger.Trace("Heartbeat received on port {0} {1}", connection.portName(), JsonConvert.SerializeObject(message)); } if (null == parameters) { parameters = new Dictionary <String, ParamValue>(); this.connection.sendParamsListRequest(); } if ((message.messid.Equals(MAVLink.MAVLINK_MSG_ID.PARAM_VALUE)) && (null != parameters)) { lock (parameters) { // set this value in the global parameter set ParamValue param = new ParamValue(message); parameterReceived(param); // set this object in case there is a thread waiting on a param_value 'ack' message on a param set request if (!parameterSetAckObj.ContainsKey(param.param_id)) { parameterSetAckObj.Add(param.param_id, param); } } } } catch (Exception e) { logger.Error("Failure in parsing message, exception with message {0}", e.Message); } }
public MissionCurrent(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_mission_current_t data = (MAVLink.mavlink_mission_current_t)message.data_struct; this.seq = data.seq; } }
public SystemTime(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { MAVLink.mavlink_system_time_t data = (MAVLink.mavlink_system_time_t)message.data_struct; this.time_boot_ms = data.time_boot_ms; this.time_unix_sec = data.time_unix_usec; } }
T getCurrentMessage <T>(MAVLink.MAVLINK_MSG_ID id) where T : MessageContainerBase { if (!this.currentState.ContainsKey(id)) { return(null); } MavLinkMessage message = this.currentState[id]; return((T)Activator.CreateInstance(typeof(T), new object[] { message })); }
MavLinkMessage createSampleMessage(MAVLink.MAVLINK_MSG_ID id, object data_struct) { MavLinkMessage sampleMessage = new MavLinkMessage(); sampleMessage.messid = id; sampleMessage.seq = 128; sampleMessage.sysid = 12; sampleMessage.compid = 12; sampleMessage.data_struct = data_struct; return(sampleMessage); }
public ScaledPressure(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { MAVLink.mavlink_scaled_pressure_t data = (MAVLink.mavlink_scaled_pressure_t)message.data_struct; this.press_abs = data.press_abs; this.press_diff = data.press_diff; this.temperature = data.temperature; this.time_boot_ms = data.time_boot_ms; } }
public VfrHud(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_vfr_hud_t data = (MAVLink.mavlink_vfr_hud_t)message.data_struct; this.airspeed = data.airspeed; this.alt = data.alt; this.climb = data.climb; this.groundspeed = data.groundspeed; this.heading = data.heading; this.throttle = data.throttle; } }
public ParamValue(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_param_value_t data = (MAVLink.mavlink_param_value_t)message.data_struct; this.param_count = data.param_count; this.param_id = System.Text.ASCIIEncoding.ASCII.GetString(data.param_id); this.param_id = this.param_id.Replace("\0", string.Empty); this.param_index = data.param_index; this.param_type = (MAVLink.MAV_PARAM_TYPE)data.param_type; this.param_value = data.param_value; } }
public Attitude(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { MAVLink.mavlink_attitude_t data = (MAVLink.mavlink_attitude_t)message.data_struct; this.pitch = data.pitch; this.pitchspeed = data.pitchspeed; this.roll = data.roll; this.rollspeed = data.rollspeed; this.time_boot_ms = data.time_boot_ms; this.yaw = data.yaw; this.yawspeed = data.yawspeed; } }
MavLinkMessage createSampleMessage() { MAVLink.mavlink_heartbeat_t sampleStruct = new MAVLink.mavlink_heartbeat_t(); sampleStruct.autopilot = 1; sampleStruct.base_mode = 2; sampleStruct.custom_mode = 3; sampleStruct.mavlink_version = 4; sampleStruct.system_status = 5; sampleStruct.type = 6; MavLinkMessage sampleMessage = createSampleMessage(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, sampleStruct); return(sampleMessage); }
void SendMessage(MAVLink.MAVLINK_MSG_ID id, object mavlinkPayload) { var channel = ConnectionPanel.Channel; if (channel != null) { MavLinkMessage message = new MavLinkMessage(); message.ComponentId = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; message.SystemId = 255; message.MsgId = id; message.TypedPayload = mavlinkPayload; channel.SendMessage(message); } }
public TerrainReport(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_terrain_report_t data = (MAVLink.mavlink_terrain_report_t)message.data_struct; this.current_height = data.current_height; this.lat = data.lat; this.loaded = data.loaded; this.lon = data.lon; this.pending = data.pending; this.spacing = data.spacing; this.terrain_height = data.terrain_height; } }
public void CheckMissionCurrentObject() { MAVLink.mavlink_mission_current_t data = new MAVLink.mavlink_mission_current_t(); data.seq = 7; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT, data); MissionCurrent obj = new MissionCurrent(message); Assert.AreEqual(data.seq, obj.seq); MissionCurrentDTO dto = DTOFactory.createMissionCurrentDTO(obj); Assert.AreEqual(dto.seq, obj.seq); }
public NavControllerOutput(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_nav_controller_output_t data = (MAVLink.mavlink_nav_controller_output_t)message.data_struct; this.alt_error = data.alt_error; this.aspd_error = data.aspd_error; this.nav_bearing = data.nav_bearing; this.nav_pitch = data.nav_pitch; this.nav_roll = data.nav_roll; this.xtrack_error = data.xtrack_error; this.wp_dist = data.wp_dist; this.target_bearing = data.target_bearing; } }
public Heartbeat(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { // this code is faster... run it and pass null to base message for speed increase // base message uses reflection MAVLink.mavlink_heartbeat_t raw_data = (MAVLink.mavlink_heartbeat_t)message.data_struct; type = (MAVLink.MAV_TYPE)raw_data.type; autopilot = (MAVLink.MAV_AUTOPILOT)raw_data.autopilot; custom_mode = raw_data.custom_mode; base_mode = Utilities.BitwiseOperations.parseBitValues <MAVLink.MAV_MODE_FLAG>(raw_data.base_mode); system_status = (MAVLink.MAV_STATE)raw_data.system_status; mavlink_version = (int)raw_data.mavlink_version; } }
public RawImu(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { MAVLink.mavlink_raw_imu_t raw_data = (MAVLink.mavlink_raw_imu_t)message.data_struct; this.xacc = raw_data.xacc; this.yacc = raw_data.yacc; this.zacc = raw_data.zacc; this.xgyro = raw_data.xgyro; this.ygyro = raw_data.ygyro; this.zgyro = raw_data.zgyro; this.xmag = raw_data.xmag; this.ymag = raw_data.ymag; this.zmag = raw_data.zmag; } }
public GlobalPositionInt(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_global_position_int_t data = (MAVLink.mavlink_global_position_int_t)message.data_struct; this.alt = data.alt; this.hdg = data.hdg; this.lat = data.lat; this.lon = data.lon; this.relative_alt = data.relative_alt; this.time_boot_ms = data.time_boot_ms; this.vx = data.vx; this.vy = data.vy; this.vz = data.vz; } }
public void CheckSystemStatusObject() { MAVLink.mavlink_sys_status_t statusStruct = new MAVLink.mavlink_sys_status_t(); statusStruct.voltage_battery = 1; statusStruct.current_battery = 2; statusStruct.battery_remaining = 3; statusStruct.drop_rate_comm = 4; statusStruct.errors_comm = 5; statusStruct.errors_count1 = 6; statusStruct.errors_count2 = 7; statusStruct.errors_count3 = 8; statusStruct.errors_count4 = 9; MavLinkMessage message = new MavLinkMessage(); message.compid = 1; message.messid = MAVLink.MAVLINK_MSG_ID.SYS_STATUS; message.seq = 1; message.sysid = 1; message.data_struct = statusStruct; SystemStatus systemStatus = new SystemStatus(message); Assert.AreEqual(statusStruct.voltage_battery, systemStatus.voltage_battery); Assert.AreEqual(statusStruct.current_battery, systemStatus.current_battery); Assert.AreEqual(statusStruct.battery_remaining, systemStatus.battery_remaining); Assert.AreEqual(statusStruct.drop_rate_comm, systemStatus.drop_rate_comm); Assert.AreEqual(statusStruct.errors_comm, systemStatus.errors_comm); Assert.AreEqual(statusStruct.errors_count1, systemStatus.errors_count1); Assert.AreEqual(statusStruct.errors_count2, systemStatus.errors_count2); Assert.AreEqual(statusStruct.errors_count3, systemStatus.errors_count3); Assert.AreEqual(statusStruct.errors_count4, systemStatus.errors_count4); SystemStatusDTO dto = DTOFactory.createSystemStatusDTO(systemStatus); Assert.AreEqual(dto.voltage_battery, systemStatus.voltage_battery); Assert.AreEqual(dto.current_battery, systemStatus.current_battery); Assert.AreEqual(dto.battery_remaining, systemStatus.battery_remaining); Assert.AreEqual(dto.drop_rate_comm, systemStatus.drop_rate_comm); Assert.AreEqual(dto.errors_comm, systemStatus.errors_comm); Assert.AreEqual(dto.errors_count1, systemStatus.errors_count1); Assert.AreEqual(dto.errors_count2, systemStatus.errors_count2); Assert.AreEqual(dto.errors_count3, systemStatus.errors_count3); Assert.AreEqual(dto.errors_count4, systemStatus.errors_count4); String json = JsonConvert.SerializeObject(dto); }
private void RequestLogData(ushort logFileId, uint ofs, uint count) { MavLinkMessage msg = new MavLinkMessage(); msg.ComponentId = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; msg.SystemId = 255; msg.MsgId = MAVLINK_MSG_ID.LOG_REQUEST_DATA; msg.TypedPayload = new mavlink_log_request_data_t() { id = logFileId, ofs = ofs, count = count, target_system = 1, target_component = 1 }; channel.SendMessage(msg); }
public ServoOutputRaw(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_servo_output_raw_t data = (MAVLink.mavlink_servo_output_raw_t)message.data_struct; this.port = data.port; this.servo1_raw = data.servo1_raw; this.servo2_raw = data.servo2_raw; this.servo3_raw = data.servo3_raw; this.servo4_raw = data.servo4_raw; this.servo5_raw = data.servo5_raw; this.servo6_raw = data.servo6_raw; this.servo7_raw = data.servo7_raw; this.servo8_raw = data.servo8_raw; this.time_usec = data.time_usec; } }
public ScaledImu2(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_scaled_imu2_t data = (MAVLink.mavlink_scaled_imu2_t)message.data_struct; this.time_boot_ms = data.time_boot_ms; this.xacc = data.xacc; this.xgyro = data.xgyro; this.xmag = data.xmag; this.yacc = data.yacc; this.ygyro = data.ygyro; this.ymag = data.ymag; this.zacc = data.zacc; this.zgyro = data.zgyro; this.zmag = data.zmag; } }
public void CheckRcChannelsRawObject() { MAVLink.mavlink_rc_channels_raw_t data = new MAVLink.mavlink_rc_channels_raw_t(); data.chan1_raw = 1; data.chan2_raw = 2; data.chan3_raw = 3; data.chan4_raw = 4; data.chan5_raw = 5; data.chan6_raw = 6; data.chan7_raw = 7; data.chan8_raw = 8; data.port = 9; data.rssi = 10; data.time_boot_ms = 11; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_RAW, data); RcChannelsRaw obj = new RcChannelsRaw(message); Assert.AreEqual(data.chan1_raw, obj.chan1_raw); Assert.AreEqual(data.chan2_raw, obj.chan2_raw); Assert.AreEqual(data.chan3_raw, obj.chan3_raw); Assert.AreEqual(data.chan4_raw, obj.chan4_raw); Assert.AreEqual(data.chan5_raw, obj.chan5_raw); Assert.AreEqual(data.chan6_raw, obj.chan6_raw); Assert.AreEqual(data.chan7_raw, obj.chan7_raw); Assert.AreEqual(data.chan8_raw, obj.chan8_raw); Assert.AreEqual(data.port, obj.port); Assert.AreEqual(data.rssi, obj.rssi); Assert.AreEqual(data.time_boot_ms, obj.time_boot_ms); RcChannelsRawDTO dto = DTOFactory.createRcChannelsRawDTO(obj); Assert.AreEqual(dto.chan1_raw, obj.chan1_raw); Assert.AreEqual(dto.chan2_raw, obj.chan2_raw); Assert.AreEqual(dto.chan3_raw, obj.chan3_raw); Assert.AreEqual(dto.chan4_raw, obj.chan4_raw); Assert.AreEqual(dto.chan5_raw, obj.chan5_raw); Assert.AreEqual(dto.chan6_raw, obj.chan6_raw); Assert.AreEqual(dto.chan7_raw, obj.chan7_raw); Assert.AreEqual(dto.chan8_raw, obj.chan8_raw); Assert.AreEqual(dto.port, obj.port); Assert.AreEqual(dto.rssi, obj.rssi); Assert.AreEqual(dto.time_boot_ms, obj.time_boot_ms); }
private void OnMavlinkMessageReceived(object sender, MavLinkMessage e) { if (currentFlightLog != null && !pauseRecording) { currentFlightLog.AddMessage(e); } switch (e.MsgId) { case MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION: { var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload; var q = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4); UiDispatcher.RunOnUIThread(() => { ModelViewer.ModelAttitude = initialAttitude * q; }); break; } case MAVLink.MAVLINK_MSG_ID.ATTITUDE: { var payload = (MAVLink.mavlink_attitude_t)e.TypedPayload; Quaternion y = new Quaternion(new Vector3D(0, 0, 1), -payload.yaw * 180 / Math.PI); Quaternion x = new Quaternion(new Vector3D(1, 0, 0), payload.pitch * 180 / Math.PI); Quaternion z = new Quaternion(new Vector3D(0, 1, 0), payload.roll * 180 / Math.PI); UiDispatcher.RunOnUIThread(() => { ModelViewer.ModelAttitude = initialAttitude * (y * x * z); }); break; } case MAVLink.MAVLINK_MSG_ID.HIL_STATE_QUATERNION: { var payload = (MAVLink.mavlink_hil_state_quaternion_t)e.TypedPayload; Quaternion q = new Quaternion(payload.attitude_quaternion[0], payload.attitude_quaternion[1], payload.attitude_quaternion[2], payload.attitude_quaternion[3]); UiDispatcher.RunOnUIThread(() => { ModelViewer.ModelAttitude = initialAttitude * q; }); break; } case MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT: { var payload = (MAVLink.mavlink_global_position_int_t)e.TypedPayload; UiDispatcher.RunOnUIThread(() => { MapLocation((double)payload.lat / 1e7, (double)payload.lon / 1e7); }); break; } case MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE: if (showImageStream) { var p = (MAVLink.mavlink_data_transmission_handshake_t)e.TypedPayload; incoming_image.size = p.size; incoming_image.packets = p.packets; incoming_image.payload = p.payload; incoming_image.quality = p.jpg_quality; incoming_image.type = p.type; incoming_image.width = p.width; incoming_image.height = p.height; incoming_image.start = Environment.TickCount; incoming_image.packetsArrived = 0; incoming_image.data = new byte[incoming_image.size]; } break; case MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA: if (showImageStream) { var img = (MAVLink.mavlink_encapsulated_data_t)e.TypedPayload; int seq = img.seqnr; uint pos = (uint)seq * (uint)incoming_image.payload; // Check if we have a valid transaction if (incoming_image.packets == 0 || incoming_image.size == 0) { // not expecting an image? incoming_image.packetsArrived = 0; break; } uint available = (uint)incoming_image.payload; if (pos + available > incoming_image.size) { available = incoming_image.size - pos; } Array.Copy(img.data, 0, incoming_image.data, pos, available); progress.ShowProgress(0, incoming_image.size, pos + available); ++incoming_image.packetsArrived; //Debug.WriteLine("packet {0} of {1}, position {2} of {3}", incoming_image.packetsArrived, incoming_image.packets, // pos + available, incoming_image.size); // emit signal if all packets arrived if (pos + available >= incoming_image.size) { // Restart state machine incoming_image.packets = 0; incoming_image.packetsArrived = 0; byte[] saved = incoming_image.data; incoming_image.data = null; UiDispatcher.RunOnUIThread(() => { progress.ShowProgress(0, 0, 0); ShowImage(saved); }); } } break; } }
void OnGetList() { if (channel != null) { MavLinkMessage msg = new MavLinkMessage(); msg.ComponentId = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; msg.SystemId = 255; msg.MsgId = MAVLINK_MSG_ID.LOG_REQUEST_LIST; msg.TypedPayload = new mavlink_log_request_list_t() { start = 0, end = ushort.MaxValue, target_system = 1, target_component = 1 }; channel.SendMessage(msg); delayedActions.StartDelayedAction("GetList", OnCheckList, TimeSpan.FromMilliseconds(1000)); } }
private void OnMessageReceived(object sender, MavLinkMessage e) { if (e.MsgId == MAVLINK_MSG_ID.LOG_ENTRY) { if (e.TypedPayload is mavlink_log_entry_t) { List<LogEntryModel> list = new List<LogEntryModel>(); mavlink_log_entry_t entry = (mavlink_log_entry_t)e.TypedPayload; UiDispatcher.RunOnUIThread(() => { logList.Add(new LogEntryModel(entry)); delayedActions.CancelDelayedAction("GetList"); }); } } else if (e.MsgId == MAVLINK_MSG_ID.LOG_DATA) { if (e.TypedPayload is mavlink_log_data_t) { var logdata = (mavlink_log_data_t)e.TypedPayload; if (logdata.id == selectedEntry.id) { lock (data) { lastChunkTime = Environment.TickCount; totalDownloaded += logdata.count; data.Add(logdata); } if (holes.Count > 0) { FetchNextHole(logdata); } } } } }
private void OnMessageReceived(object sender, MavLinkMessage e) { if (proxyAddress != null) { // send it to the proxy... byte[] packed = e.Pack(); proxyPort.Write(packed, packed.Length); } if (e.SystemId != 1 && e.SystemId != 10) { } Console.Write(e.TypedPayload.GetType().Name); if (e.TypedPayload is MAVLink.mavlink_statustext_t) { MAVLink.mavlink_statustext_t s = (MAVLink.mavlink_statustext_t)e.TypedPayload; string msg = BytesToString(s.text); Console.WriteLine(": severity=" + s.severity + ", text=" + msg); } else if (e.TypedPayload is MAVLink.mavlink_named_value_int_t) { MAVLink.mavlink_named_value_int_t nv = (MAVLink.mavlink_named_value_int_t)e.TypedPayload; string name = BytesToString(nv.name); Console.WriteLine(": " + nv.name + "=" + nv.value); } else if (e.TypedPayload is MAVLink.mavlink_named_value_float_t) { MAVLink.mavlink_named_value_float_t nv = (MAVLink.mavlink_named_value_float_t)e.TypedPayload; string name = BytesToString(nv.name); Console.WriteLine(": " + nv.name + "=" + nv.value); } else if (e.TypedPayload != null) { Console.WriteLine(": " + e.ToString()); } else { Console.WriteLine("UnknownMessageType: " + e.ToString()); } }
private void OnProxyMessageReceived(object sender, MavLinkMessage e) { // message from the proxy has to go back the other way. byte[] packed = e.Pack(); port.Write(packed, packed.Length); }