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 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);
}
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);
}
