//
// Summary:
// Thread function for writing commander packets via BTH
// Writes as fast as it can in a tight loop
private async void CommanderSetpointThread(CancellationToken cancellationToken)
{
// Write commander packets as fast as possible in a loop until cancelled
while (!cancellationToken.IsCancellationRequested)
{
FlightControlAxes axes = await flightController.GetFlightControlAxes();
await bthCrtp.WriteCommanderPacket(
(float)(axes.roll * maxPitchRollRate),
(float)(axes.pitch * maxPitchRollRate),
(float)(axes.yaw * maxYawRate),
(ushort)(axes.thrust * maxThrustPercent * MaxThrust));
}
}
//
// Summary:
// Thread function for writing commander packets via BTH
// Writes as fast as it can in a tight loop
private async void CommanderSetpointThread(CancellationToken cancellationToken)
{
// Write commander packets as fast as possible in a loop until cancelled
while (!cancellationToken.IsCancellationRequested)
{
FlightControlAxes axes = await flightController.GetFlightControlAxes();
// Use the emulated CPPM commander type to allow arming in Betaflight
await bthCrtp.WriteCppmCommanderPacket(
(ushort)((axes.roll * 500) + 1500),
(ushort)((axes.pitch * 500) + 1500),
(ushort)((axes.yaw * 500) + 1500),
(ushort)((axes.thrust * 1000 * maxThrustPercent) + 1000),
(ushort)(axes.isSelfLevelEnabled ? 2000 : 1000),
(ushort)(axes.isArmed ? 2000 : 1000));
}
}
