static void Main(string[] args)
{
uint zano;
Library.Initialise("Example-GetAltitude.1.0.0");
zano = Library.Obtain("192.168.0.1", 10001);
bool isConnected = false;
Frame frame = new Frame();
Library.Connect(zano);
while (true)
{
if (isConnected)
{
Frames.FlightAltitudeGet(zano);
}
Library.Tick();
while (Library.Receive(ref frame) == 1)
{
switch (frame.Reference)
{
case Symbols.kFlightAltitudeGet:
{
int altitude = frame.Get(Symbols.kAltitude);
Console.WriteLine(String.Format("Altitude is {0} mm", altitude));
}
break;
}
}
Library.Status status;
while ((status = Library.GetStatus()) != Library.Status.None)
{
switch (status)
{
case Library.Status.DeviceDisconnectionDisconnected:
case Library.Status.DeviceConnectionNotConnected:
case Library.Status.DeviceConnectionHeldStill:
{
isConnected = false;
Library.Connect(zano);
}
break;
case Library.Status.DeviceConnectionConnected:
{
isConnected = true;
}
break;
}
}
Thread.Sleep(10);
}
}
public static void Tick()
{
if (Initialised && CanTick)
{
A.Trigger("tick");
LibZano.Library.GetFirmwareVersion(Handle, ref FirmwareVersion);
if (Library.Time() >= BatteryTimer)
{
BatteryTimer = Library.Time() + 1000;
Console.WriteLine("Booted = {0}, Connected = {1}", IsBooted, IsConnected);
if (IsBooted && IsConnected)
{
Frames.SensorsBatteryGet(Handle);
}
}
Library.Tick();
Frame frame = new Frame();
while (Library.Receive(ref frame) == 1)
{
switch (frame.Type)
{
case Frame.kSendFrame:
{
String evt;
if (listens.TryGetValue(frame.Reference, out evt))
{
T.Trigger(evt, frame);
}
if (frame.Reference == Symbols.kSensorsBatteryGet)
{
Z.LastBattery = frame.Get(Symbols.kCharge);
Console.WriteLine("Battery = {0}", Z.LastBattery);
A.Trigger("battery", Z.LastBattery);
}
}
break;
case Frame.kConnectFrame:
{
Console.WriteLine("Connect Frame");
}
break;
case Frame.kDisconnectFrame:
{
Console.WriteLine("Disconnect Frame");
}
break;
case Frame.kFlyStateFrame:
{
Console.WriteLine("Fly State Frame");
}
break;
case Frame.kFirmware:
{
Console.WriteLine("Firmware Frame");
}
break;
case Frame.kServerConnectivity:
{
Console.WriteLine("Server Connectivity Frame");
}
break;
}
}
Library.Status status;
while ((status = Library.GetStatus()) != Library.Status.None)
{
Console.WriteLine("Status = {0}", status);
switch (status)
{
case Library.Status.None:
break;
case Library.Status.FlashingStarted:
Internal.Connect.Trigger("firmware_upgrade_start");
A.Trigger("firmware_upgrade_start");
break;
case Library.Status.FlashingProcessSucessfull:
Internal.Connect.Trigger("firmware_upgrade_start_complete");
A.Trigger("firmware_upgrade_start_complete");
break;
case Library.Status.FlashingProcessFailed:
Internal.Connect.Trigger("firmware_upgrade_start_failed");
A.Trigger("firmware_upgrade_start_failed");
break;
case Library.Status.FlashingProcessInProgress:
Internal.Connect.Trigger("firmware_upgrade_in_progress");
A.Trigger("firmware_upgrade_in_progress");
break;
case Library.Status.ConfigurationStarting:
break;
case Library.Status.ConfigurationApplied:
break;
case Library.Status.ConfigurationNotNeeded:
break;
case Library.Status.ConfigurationDoesNotExist:
break;
case Library.Status.HelloRequested:
break;
case Library.Status.HelloSuccess:
A.Trigger("has_internet");
T.Trigger("has_internet");
break;
case Library.Status.HelloFailed:
A.Trigger("no_internet");
T.Trigger("no_internet");
break;
case Library.Status.HaveServerConnection:
A.Trigger("have_server_connection");
T.Trigger("have_server_connection");
break;
case Library.Status.FirmwareDownloadingRequested:
break;
case Library.Status.FirmwareDownloadedNew:
break;
case Library.Status.FirmwareDownloadedUpToDate:
break;
case Library.Status.FirmwareDownloadedNetworkError:
break;
case Library.Status.ConfigurationRequest:
break;
case Library.Status.ConfigurationNetworkError:
break;
case Library.Status.ConfigurationAuthenticationError:
break;
case Library.Status.ConfigurationNoConfigurationAssociatedWithAccount:
break;
case Library.Status.ConfigurationDownloaded:
A.Trigger("configuration_downloaded");
T.Trigger("configuration_downloaded");
break;
case Library.Status.DeviceConnectionAttempt:
break;
case Library.Status.DeviceConnectionGotConnection:
break;
case Library.Status.DeviceConnectionBooted:
Internal.Connect.Trigger("booted");
A.Trigger("booted");
break;
case Library.Status.DeviceConnectionInRecovery:
break;
case Library.Status.DeviceConnectionHeldStill:
Internal.Connect.Trigger("not_still");
A.Trigger("not_still");
break;
case Library.Status.DeviceConnectionBootFailure:
break;
case Library.Status.DeviceConnectionNeedToUpdate:
Internal.Connect.Trigger("need_to_update");
A.Trigger("need_to_update");
break;
case Library.Status.DeviceConnectionConnected:
Library.GetSerialNumber(Handle, ref SerialNumber);
Internal.Connect.Trigger("connected");
A.Trigger("connected");
BatteryTimer = Library.Time() + 1000;
LastBattery = 0;
break;
case Library.Status.DeviceConnectionNotConnected:
LastBattery = 0;
Internal.Connect.Trigger("not_connected");
A.Trigger("not_connected");
break;
case Library.Status.DeviceConnectionBadUpdate:
Internal.Connect.Trigger("bad_update");
A.Trigger("bad_update");
break;
case Library.Status.DeviceConnectionDroneNotAssociatedWithAccount:
Internal.Connect.Trigger("drone_not_associated");
A.Trigger("drone_not_associated");
break;
case Library.Status.DeviceConnectionPeekConnected:
A.Trigger("peek_connected");
break;
case Library.Status.DeviceDisconnectionDisconnected:
LastBattery = 0;
T.Trigger("disconnected");
A.Trigger("disconnected");
break;
case Library.Status.ServerConnectivityLoginSuccess:
A.Trigger("logged_in");
T.Trigger("logged_in");
break;
case Library.Status.ServerConnectivityLoginFailure:
A.Trigger("not_logged_in");
T.Trigger("not_logged_in");
break;
case Library.Status.FirmwareConfigurationPersistanceSaveRequested:
A.Trigger("config_server_save_started");
T.Trigger("config_server_save_started");
break;
case Library.Status.FirmwareConfigurationPersistanceSaveCompleted:
A.Trigger("config_server_save_complete");
T.Trigger("config_server_save_complete");
break;
case Library.Status.FirmwareConfigurationPersistanceSaveFailure:
A.Trigger("config_server_save_failed");
T.Trigger("config_server_save_failed");
break;
}
}
}
}
public static void CalculatePressure(Frame f, PressureReading r)
{
//G.AccelerationNowX[index] = f.Get(Symbols.kX);
double altitude = f.Get(Symbols.kAltitude);
//G.FilteredAccelerationX[index] = U.LowPass(G.FilteredAccelerationX[index], G.AccelerationNowX[index], K.D);
r.Altitude = U.LowPass(r.Altitude, altitude, K.D);
//if (G.Readings[index] == 0)
if (r.Readings == 0)
{
//G.FilteredAccelerationX[index] = G.AccelerationNowX[index];
r.Altitude = altitude;
}
//double hpX = G.AccelerationNowX[index] - G.FilteredAccelerationX[index];
double hpAltitude = altitude - r.Altitude;
//double D = G.Readings[index] - K.DiscardUnder;
double D = 35.0f;
//if (hpX > G.FilteredAccelerationMaxX[index])
// G.FilteredAccelerationMaxX[index] = hpX;
if (hpAltitude > r.MaxAltitude)
r.MaxAltitude = hpAltitude;
//if (hpX < G.FilteredAccelerationMinX[index])
// G.FilteredAccelerationMinX[index] = hpX;
if (hpAltitude < r.MinAltitude)
r.MinAltitude = hpAltitude;
//if (G.Readings[index] >= K.DiscardUnder || G.MotorIndex == 4)
// if (r.Readings >= K.DiscardUnder || r == G.WaitReading)
// {
//G.FilteredAccelerationMaxX[index] *= (D / (D+1.0));
r.MaxAltitude *= (D / (D + 1.0));
//G.FilteredAccelerationMinX[index] *= (D / (D+1.0));
r.MinAltitude *= (D / (D + 1.0));
// }
//if (G.Readings[index] >= K.DiscardUnder)
// if (r.Readings >= K.DiscardUnder)
// {
//double rangeX = G.FilteredAccelerationMaxX[index] - G.FilteredAccelerationMinX[index];
double rangeAltitude = r.MaxAltitude - r.MinAltitude;
//G.RangeX[index] = U.LowPass(G.RangeX[index], rangeX, 10.0);
r.RangeAltitude = U.LowPass(r.RangeAltitude, rangeAltitude, 10.0);
//
// //G.Rating[index] = 5.0 - ((((G.RangeX[index] + G.RangeY[index])*0.5)-200.0)/70.0);
// r.RawRating = 5.0 - ((((r.RangeX + r.RangeY) * 0.5) - 200.0) / 70.0);
//
// //if (G.Rating[index] > 5.0)
// if (r.RawRating > 5.0)
// //G.Rating[index] = 5.0;
// r.RawRating = 5.0;
// //else if (G.Rating[index] < 0.0)
// else if (r.RawRating < 0.0)
// //G.Rating[index] = 0.0;
// r.RawRating = 0.0;
//
// ///// G.AllRatings[index] += G.Rating[index];
// //G.AvgRatings[index] = U.LowPass(G.AvgRatings[index], G.Rating[index], 10.0); //G.AllRatings[index]/G.Readings[index];
// //r.Rating = U.LowPass(r.Rating, r.RawRating, 10.0);
// r.Rating = U.LowPass(r.Rating, r.RawRating, 20.0);
// }
}
/*
public static void ResetReadings(int index)
{
G.AccelerationNowX[index] = 0;
G.AccelerationNowY[index] = 0;
G.FilteredAccelerationX[index] = 0;
G.FilteredAccelerationY[index] = 0;
G.FilteredAccelerationMinX[index] = 0;
G.FilteredAccelerationMinY[index] = 0;
G.FilteredAccelerationMaxX[index] = 0;
G.FilteredAccelerationMaxY[index] = 0;
G.RangeX[index] = 0;
G.RangeY[index] = 0;
G.Rating[index] = 0;
G.AllRatings[index] = 0;
G.AvgRatings[index] = 0;
G.Readings[index] = 0;
}
*/
public static void CalculateVibration(Frame f, MotorReading r)
{
//G.AccelerationNowX[index] = f.Get(Symbols.kX);
double x = f.Get(Symbols.kX);
//G.AccelerationNowY[index] = f.Get(Symbols.kY);
double y = f.Get(Symbols.kY);
//G.FilteredAccelerationX[index] = U.LowPass(G.FilteredAccelerationX[index], G.AccelerationNowX[index], K.D);
r.X = U.LowPass(r.X, x, K.D);
//G.FilteredAccelerationY[index] = U.LowPass(G.FilteredAccelerationY[index], G.AccelerationNowY[index], K.D);
r.Y = U.LowPass(r.Y, y, K.D);
//if (G.Readings[index] == 0)
if (r.Readings == 0)
{
//G.FilteredAccelerationX[index] = G.AccelerationNowX[index];
r.X = x;
//G.FilteredAccelerationY[index] = G.AccelerationNowY[index];
r.Y = y;
}
//double hpX = G.AccelerationNowX[index] - G.FilteredAccelerationX[index];
double hpX = x - r.X;
//double hpY = G.AccelerationNowY[index] - G.FilteredAccelerationY[index];
double hpY = y - r.Y;
//double D = G.Readings[index] - K.DiscardUnder;
double D = r.Readings - K.DiscardUnder;
if (D < 0)
{
D = 0;
}
//if (D > 30.0)
if (D > 45.0)
{
// D = 30.0;
D = 45.0;
}
//if (hpX > G.FilteredAccelerationMaxX[index])
// G.FilteredAccelerationMaxX[index] = hpX;
if (hpX > r.MaxX)
r.MaxX = hpX;
//if (hpY > G.FilteredAccelerationMaxY[index])
// G.FilteredAccelerationMaxY[index] = hpY;
if (hpY > r.MaxY)
r.MaxY = hpY;
//if (hpX < G.FilteredAccelerationMinX[index])
// G.FilteredAccelerationMinX[index] = hpX;
if (hpX < r.MinX)
r.MinX = hpX;
//if (hpY < G.FilteredAccelerationMinY[index])
// G.FilteredAccelerationMinY[index] = hpY;
if (hpY < r.MinY)
r.MinY = hpY;
//if (G.Readings[index] >= K.DiscardUnder || G.MotorIndex == 4)
if (r.Readings >= K.DiscardUnder || r == G.WaitReading)
{
//G.FilteredAccelerationMaxX[index] *= (D / (D+1.0));
r.MaxX *= (D/(D + 1.0));
//G.FilteredAccelerationMaxY[index] *= (D / (D+1.0));
r.MaxY *= (D/(D + 1.0));
//G.FilteredAccelerationMinX[index] *= (D / (D+1.0));
r.MinX *= (D/(D + 1.0));
//G.FilteredAccelerationMinY[index] *= (D / (D+1.0));
r.MinY *= (D/(D + 1.0));
}
//if (G.Readings[index] >= K.DiscardUnder)
if (r.Readings >= K.DiscardUnder)
{
//double rangeX = G.FilteredAccelerationMaxX[index] - G.FilteredAccelerationMinX[index];
double rangeX = r.MaxX - r.MinX;
//double rangeY = G.FilteredAccelerationMaxY[index] - G.FilteredAccelerationMinY[index];
double rangeY = r.MaxY - r.MinY;
//G.RangeX[index] = U.LowPass(G.RangeX[index], rangeX, 10.0);
r.RangeX = U.LowPass(r.RangeX, rangeX, 10.0);
//G.RangeY[index] = U.LowPass(G.RangeY[index], rangeY, 10.0);
r.RangeY = U.LowPass(r.RangeX, rangeY, 10.0);
//G.Rating[index] = 5.0 - ((((G.RangeX[index] + G.RangeY[index])*0.5)-200.0)/70.0);
r.RawRating = 5.0 - ((((r.RangeX + r.RangeY)*0.5)-200.0)/70.0);
//if (G.Rating[index] > 5.0)
if (r.RawRating > 5.0)
//G.Rating[index] = 5.0;
r.RawRating = 5.0;
//else if (G.Rating[index] < 0.0)
else if (r.RawRating < 0.0)
//G.Rating[index] = 0.0;
r.RawRating = 0.0;
///// G.AllRatings[index] += G.Rating[index];
//G.AvgRatings[index] = U.LowPass(G.AvgRatings[index], G.Rating[index], 10.0); //G.AllRatings[index]/G.Readings[index];
//r.Rating = U.LowPass(r.Rating, r.RawRating, 10.0);
r.Rating = U.LowPass(r.Rating, r.RawRating, 20.0);
}
}
