protected override void ResetVariables()
{
base.ResetVariables();
currentNavigationDataStruct = new NavigationDataStruct();
currentNavigationDataHeaderStruct = new NavigationDataHeaderStruct();
currentNavigationData = new DroneData();
currentSequenceNumber = initialSequenceNumber;
}
public DroneData(NavigationDataStruct navigationDataStruct)
{
phi = navigationDataStruct.Phi / 1000.0;
psi = navigationDataStruct.Psi / 1000.0;
theta = navigationDataStruct.Theta / 1000.0;
vX = navigationDataStruct.VX;
vY = navigationDataStruct.VY;
vZ = navigationDataStruct.VZ;
altitude = navigationDataStruct.Altitude;
batteryLevel = (int)navigationDataStruct.BatteryLevel;
}
public DroneData(NavigationDataStruct navigationDataStruct)
{
phi = navigationDataStruct.Phi / 1000.0;
psi = navigationDataStruct.Psi / 1000.0;
theta = navigationDataStruct.Theta / 1000.0;
vX = navigationDataStruct.VX;
vY = navigationDataStruct.VY;
vZ = navigationDataStruct.VZ;
altitude = navigationDataStruct.Altitude;
batteryLevel = (int)navigationDataStruct.BatteryLevel;
}
public DroneData(NavigationDataStruct navigationDataStruct)
{
uint major = navigationDataStruct.ControlStatus >> 16;
//uint minor = navigationDataStruct.ControlStatus & 0xFFFF;
DroneState = (DroneState)major;
phi = navigationDataStruct.Phi / 1000.0;
psi = navigationDataStruct.Psi / 1000.0;
theta = navigationDataStruct.Theta / 1000.0;
vX = navigationDataStruct.VX;
vY = navigationDataStruct.VY;
vZ = navigationDataStruct.VZ;
altitude = navigationDataStruct.Altitude;
batteryLevel = (int)navigationDataStruct.BatteryLevel;
}
private void DetermineNavigationData(byte[] buffer, int position)
{
unsafe
{
fixed (byte* entry = &buffer[position])
{
currentNavigationDataStruct = *(NavigationDataStruct*)entry;
}
}
currentNavigationData = new DroneData(currentNavigationDataStruct);
}
public DroneData(NavigationDataStruct navigationDataStruct)
{
uint major = navigationDataStruct.ControlStatus >> 16;
//uint minor = navigationDataStruct.ControlStatus & 0xFFFF;
DroneState = (DroneState)major;
phi = navigationDataStruct.Phi / 1000.0;
psi = navigationDataStruct.Psi / 1000.0;
theta = navigationDataStruct.Theta / 1000.0;
vX = navigationDataStruct.VX;
vY = navigationDataStruct.VY;
vZ = navigationDataStruct.VZ;
altitude = navigationDataStruct.Altitude;
batteryLevel = (int)navigationDataStruct.BatteryLevel;
}
//[DllImport("gps_test.c", CallingConvention = CallingConvention.Cdecl)]
//public static extern double get_time();
//[DllImport("gps_test.c", CallingConvention = CallingConvention.Cdecl)]
//public static extern double get_lat();
//[DllImport("gps_test.c", CallingConvention = CallingConvention.Cdecl)]
//public static extern double get_long();
public DroneData(NavigationDataStruct navigationDataStruct)
{
phi = navigationDataStruct.Phi / 1000.0;
psi = navigationDataStruct.Psi / 1000.0;
theta = navigationDataStruct.Theta / 1000.0;
vX = navigationDataStruct.VX;
vY = navigationDataStruct.VY;
vZ = navigationDataStruct.VZ;
//time = get_time();
//lat = get_lat();
//longi = get_long();
altitude = navigationDataStruct.Altitude;
batteryLevel = (int)navigationDataStruct.BatteryLevel;
}
