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