static void
WriteWithChecksum(Port &port, const char *String)
{
port.Write(String);
char sTmp[8];
sprintf(sTmp, "%02X\r\n", ::NMEAChecksum(String));
port.Write(sTmp);
}
void
LX::CommandModeQuick(Port &port, OperationEnvironment &env)
{
port.Write(SYN);
env.Sleep(500);
port.Write(SYN);
env.Sleep(500);
port.Write(SYN);
env.Sleep(500);
}
bool
EWMicroRecorderDevice::Declare(const Declaration *decl)
{
// Must have at least two, max 12 waypoints
if (decl->size() < 2 || decl->size() > 12)
return false;
port->StopRxThread();
port->SetRxTimeout(500); // set RX timeout to 500[ms]
if (!TryConnect())
return false;
port->Write('\x18'); // start to upload file
port->Write(user_data);
EWMicroRecorderPrintf(port, _T("%-15s %s\r\n"),
_T("Pilot Name:"), decl->PilotName);
EWMicroRecorderPrintf(port, _T("%-15s %s\r\n"),
_T("Competition ID:"), decl->AircraftRego);
EWMicroRecorderPrintf(port, _T("%-15s %s\r\n"),
_T("Aircraft Type:"), decl->AircraftType);
port->Write("Description: Declaration\r\n");
for (unsigned i = 0; i < 11; i++) {
if (i+1>= decl->size()) {
EWMicroRecorderPrintf(port, _T("%-17s %s\r\n"),
_T("TP LatLon:"), _T("0000000N00000000E TURN POINT\r\n"));
} else {
const Waypoint &wp = decl->waypoints[i];
if (i == 0) {
EWMicroRecorderWriteWayPoint(port, wp, _T("Take Off LatLong:"));
EWMicroRecorderWriteWayPoint(port, wp, _T("Start LatLon:"));
} else if (i + 1 < decl->size()) {
EWMicroRecorderWriteWayPoint(port, wp, _T("TP LatLon:"));
}
}
}
const Waypoint &wp = decl->waypoints[decl->size() - 1];
EWMicroRecorderWriteWayPoint(port, wp, _T("Finish LatLon:"));
EWMicroRecorderWriteWayPoint(port, wp, _T("Land LatLon:"));
port->Write('\x03'); // finish sending user file
bool success = ExpectStringWait(port, "uploaded successfully");
port->Write("!!\r\n"); // go back to NMEA mode
port->SetRxTimeout(0); // clear timeout
port->StartRxThread(); // restart RX thread
return success;
}
void
DeviceDescriptor::ForwardLine(const char *line)
{
/* XXX make this method thread-safe; this method can be called from
any thread, and if the Port gets closed, bad things happen */
if (IsNMEAOut() && port != nullptr) {
Port *p = port;
p->Write(line);
p->Write("\r\n");
}
}
bool
CaiGpsNavDevice::Open(OperationEnvironment &env)
{
port->Write(CtrlC);
env.Sleep(50);
port->Write("NMEA\r");
// This is for a slightly different mode, that
// apparently outputs pressure info too...
//port->Write("PNP\r\n");
//port->Write("LOG 0\r\n");
return true;
}
bool
Volkslogger::SendCommand(Port &port, OperationEnvironment &env,
Command cmd, uint8_t param1, uint8_t param2)
{
static constexpr unsigned delay = 2;
/* flush buffers */
if (!port.FullFlush(env, 20, 100))
return false;
/* reset command interpreter */
if (!Reset(port, env, 6))
return false;
/* send command packet */
const uint8_t cmdarray[8] = {
(uint8_t)cmd, param1, param2,
0, 0, 0, 0, 0,
};
if (!port.Write(ENQ))
return false;
env.Sleep(delay);
if (!SendWithCRC(port, cmdarray, sizeof(cmdarray), env))
return false;
/* wait for confirmation */
return port.WaitRead(env, 4000) == Port::WaitResult::READY &&
port.GetChar() == 0;
}
bool
Volkslogger::WriteBulk(Port &port, OperationEnvironment &env,
const void *buffer, unsigned length)
{
const unsigned delay = 1;
env.SetProgressRange(length);
uint16_t crc16 = 0;
const uint8_t *p = (const uint8_t *)buffer, *end = p + length;
while (p < end) {
unsigned n = end - p;
if (n > 400)
n = 400;
n = port.Write(p, n);
if (n == 0)
return false;
crc16 = UpdateCRC16CCITT(p, n, crc16);
p += n;
env.SetProgressPosition(p - (const uint8_t *)buffer);
/* throttle sending a bit, or the Volkslogger's receive buffer
will overrun */
env.Sleep(delay * 100);
}
return port.Write(crc16 >> 8) && port.Write(crc16 & 0xff);
}
static void
EWMicroRecorderPrintf(Port &port, const TCHAR *fmt, ...)
{
TCHAR EWStr[128];
va_list ap;
va_start(ap, fmt);
_vstprintf(EWStr, fmt, ap);
va_end(ap);
#ifdef _UNICODE
char buffer[256];
if (::WideCharToMultiByte(CP_ACP, 0, EWStr, -1, buffer, sizeof(buffer),
NULL, NULL) <= 0)
return;
#else
char *buffer = EWStr;
#endif
char *p = strchr(buffer, ':');
if (p != NULL)
++p;
else
p = buffer;
for (; *p != 0; ++p)
if (!IsValidEWChar(*p))
*p = ' ';
port.Write(buffer);
}
static bool
TryConnect(Port &port, char *user_data, size_t max_user_data,
OperationEnvironment &env)
{
port.Flush();
port.Write('\x02'); // send IO Mode command
unsigned user_size = 0;
TimeoutClock timeout(8000);
while (true) {
int remaining = timeout.GetRemainingSigned();
if (remaining < 0)
return false;
if (port.WaitRead(env, remaining) != Port::WaitResult::READY)
return false;
int nbytes = port.Read(user_data + user_size, max_user_data - user_size);
if (nbytes < 0)
return false;
if (user_size == 0) {
const char *minus = (const char *)memchr(user_data, '-', nbytes);
if (minus == NULL)
continue;
user_size = user_data + nbytes - minus;
memmove(user_data, minus, user_size);
} else
user_size += nbytes;
char *end = (char *)memchr(user_data, '\x13', user_size);
if (end != NULL) {
*end = 0;
port.Write('\x16');
return true;
}
if (user_size >= max_user_data)
/* response too large */
return false;
}
return false;
}
bool
XCOM760Device::PutVolume(int Volume)
{
char szTmp[32];
sprintf(szTmp, "$RVOL=%d\r\n", Volume);
port->Write(szTmp);
return true;
}
bool
FLARM::SendEscaped(Port &port, const void *buffer, size_t length,
OperationEnvironment &env, unsigned timeout_ms)
{
assert(buffer != nullptr);
assert(length > 0);
const TimeoutClock timeout(timeout_ms);
// Send data byte-by-byte including escaping
const uint8_t *p = (const uint8_t *)buffer, *end = p + length;
while (true) {
const uint8_t *special = FindSpecial(p, end);
if (special > p) {
/* bulk write of "harmless" characters */
if (!port.FullWrite(p, special - p, env, timeout.GetRemainingOrZero()))
return false;
p = special;
}
if (p == end)
break;
// Check for bytes that need to be escaped and send
// the appropriate replacements
bool result;
if (*p == START_FRAME)
result = port.Write(ESCAPE) && port.Write(ESCAPE_START);
else if (*p == ESCAPE)
result = port.Write(ESCAPE) && port.Write(ESCAPE_ESCAPE);
else
// Otherwise just send the original byte
result = port.Write(*p);
if (!result)
return false;
p++;
}
return true;
}
bool
LX::SendPacket(Port &port, Command command,
const void *data, size_t length,
OperationEnvironment &env, unsigned timeout_ms)
{
return SendCommand(port, command) &&
port.FullWrite(data, length, env, timeout_ms) &&
port.Write(calc_crc(data, length, 0xff));
}
bool
LX::SendPacket(Port &port, Command command,
const void *data, size_t length,
OperationEnvironment &env,
std::chrono::steady_clock::duration timeout)
{
return SendCommand(port, command) &&
port.FullWrite(data, length, env, timeout) &&
port.Write(calc_crc(data, length, 0xff));
}
bool
XCOM760Device::PutActiveFrequency(RadioFrequency frequency)
{
char szTmp[32];
sprintf(szTmp, "$TXAF=%u.%03u\r\n",
frequency.GetKiloHertz() / 1000,
frequency.GetKiloHertz() % 1000);
port->Write(szTmp);
return true;
}
static bool
EWMicroRecorderWriteWaypoint(Port &port, const char *type,
const Waypoint &way_point)
{
return WriteLabel(port, type) &&
WriteGeoPoint(port, way_point.location) &&
port.Write(' ') &&
WriteCleanString(port, way_point.name.c_str(), 1000) &&
port.FullWrite("\r\n", 2, 500);
}
static bool
TryConnect(Port &port, char *user_data)
{
int retries=10;
while (--retries){
port.Write('\x02'); // send IO Mode command
unsigned user_size = 0;
bool started = false;
PeriodClock clock;
clock.update();
int i;
while ((i = port.GetChar()) != EOF && !clock.check(8000)) {
char ch = (char)i;
if (!started && ch == '-')
started = true;
if (started) {
if (ch == 0x13) {
port.Write('\x16');
user_data[user_size] = 0;
// found end of file
return true;
} else {
if (user_size < sizeof(user_data) - 1) {
user_data[user_size] = ch;
user_size++;
}
}
}
}
}
return false;
}
bool
EWMicroRecorderDevice::TryConnect()
{
int retries=10;
while (--retries){
port->Write('\x02'); // send IO Mode command
unsigned user_size = 0;
bool started = false;
int i;
while ((i = port->GetChar()) != _TEOF) {
char ch = (char)i;
if (!started) {
if (ch == _T('-')) {
started = true;
}
}
if (started) {
if (ch == 0x13) {
port->Write('\x16');
user_data[user_size] = 0;
// found end of file
return true;
} else {
if (user_size < sizeof(user_data) - 1) {
user_data[user_size] = ch;
user_size++;
}
}
}
}
}
return false;
}
bool
Volkslogger::Reset(Port &port, OperationEnvironment &env, unsigned n)
{
static constexpr auto delay = std::chrono::milliseconds(2);
while (n-- > 0) {
if (!port.Write(CAN))
return false;
env.Sleep(delay);
}
return true;
}
bool
Volkslogger::Reset(Port &port, OperationEnvironment &env, unsigned n)
{
static constexpr unsigned delay = 2;
while (n-- > 0) {
if (!port.Write(CAN))
return false;
env.Sleep(delay);
}
return true;
}
bool
WesterboerDevice::PutMacCready(fixed _mac_cready)
{
/* "0 .. 60 in 5-er Schritten" */
unsigned mac_cready = std::min(uround(_mac_cready * 10 / 5) * 5, 60u);
char buffer[64];
sprintf(buffer, "$PWES4,,%02u,,,,,,,", mac_cready);
AppendNMEAChecksum(buffer);
strcat(buffer, "\r\n");
port->Write(buffer);
return true;
}
bool
WesterboerDevice::PutBugs(fixed _bugs)
{
// Dirtyness from 0 until 20 %
unsigned bugs = 100 - ((unsigned)_bugs * 100);
char buffer[64];
sprintf(buffer, "$PWES4,,,,,%02u,,,,", bugs);
AppendNMEAChecksum(buffer);
strcat(buffer, "\r\n");
port->Write(buffer);
return true;
}
static bool
TryConnect(Port &port, char *user_data, size_t max_user_data,
OperationEnvironment &env)
{
port.Write('\x02'); // send IO Mode command
unsigned user_size = 0;
bool started = false;
PeriodClock clock;
clock.Update();
int i;
while ((i = port.GetChar()) != EOF && !clock.Check(8000) &&
!env.IsCancelled()) {
char ch = (char)i;
if (!started && ch == '-')
started = true;
if (started) {
if (ch == 0x13) {
port.Write('\x16');
user_data[user_size] = 0;
// found end of file
return true;
} else {
if (user_size < max_user_data - 1) {
user_data[user_size] = ch;
user_size++;
}
}
}
}
return false;
}
bool
EWDevice::TryConnect()
{
int retries=10;
while (--retries){
port->Write("##\r\n"); // send IO Mode command
if (port->ExpectString("IO Mode.\r"))
return true;
port->ExpectString("$$$"); // empty input buffer
}
return false;
}
bool
EWDevice::Declare(const struct Declaration &declaration,
OperationEnvironment &env)
{
lLastBaudrate = port->SetBaudrate(9600L); // change to IO Mode baudrate
port->SetRxTimeout(500); // set RX timeout to 500[ms]
bool success = DeclareInner(declaration, env);
port->Write("NMEA\r\n"); // switch to NMEA mode
port->SetBaudrate(lLastBaudrate); // restore baudrate
return success;
}
bool
PortWriteNMEA(Port &port, const char *line, OperationEnvironment &env)
{
assert(line != nullptr);
/* reasonable hard-coded timeout; do we need to make this a
parameter? */
const unsigned timeout_ms = 1000;
if (!port.Write('$') ||
!port.FullWrite(line, strlen(line), env, timeout_ms))
return false;
char checksum[16];
sprintf(checksum, "*%02X\r\n", NMEAChecksum(line));
return port.FullWrite(checksum, strlen(checksum), env, timeout_ms);
}
bool
LX::CommandMode(Port &port, OperationEnvironment &env)
{
/* switch to command mode, first attempt */
port.Write(SYN);
/* now flush all of the remaining input */
port.SetRxTimeout(10);
port.FullFlush(20);
/* the port is clean now; try the SYN/ACK procedure up to three
times */
return port.SetRxTimeout(500) &&
(Connect(port, env) || Connect(port, env) || Connect(port, env)) &&
/* ... and configure the timeout */
port.SetRxTimeout(5000);
}
bool
EWMicroRecorderDevice::Declare(const Declaration &declaration,
OperationEnvironment &env)
{
// Must have at least two, max 12 waypoints
if (declaration.size() < 2 || declaration.size() > 12)
return false;
/* during tests, the EW has taken up to one second to respond to
the command \x18 */
port->SetRxTimeout(2500);
bool success = DeclareInner(*port, declaration, env);
port->Write("!!\r\n"); // go back to NMEA mode
return success;
}
bool
Volkslogger::Handshake(Port &port, OperationEnvironment &env,
std::chrono::steady_clock::duration _timeout)
{
TimeoutClock timeout(_timeout);
while (true) { // Solange R's aussenden, bis ein L zurückkommt
if (!port.Write('R'))
return false;
auto remaining = timeout.GetRemainingSigned();
if (remaining.count() < 0)
return false;
if (remaining > std::chrono::milliseconds(500))
remaining = std::chrono::milliseconds(500);
Port::WaitResult result =
port.WaitForChar('L', env, remaining);
if (result == Port::WaitResult::READY)
break;
if (result != Port::WaitResult::TIMEOUT)
return false;
/* timeout, try again */
}
unsigned count = 1;
while (true) { // Auf 4 hintereinanderfolgende L's warten
const auto remaining = timeout.GetRemainingSigned();
if (remaining.count() < 0)
return false;
if (port.WaitForChar('L', env, remaining) != Port::WaitResult::READY)
return false;
count++;
if (count >= 4)
return true;
}
}
bool
Volkslogger::Handshake(Port &port, OperationEnvironment &env,
unsigned timeout_ms)
{
TimeoutClock timeout(timeout_ms);
while (true) { // Solange R's aussenden, bis ein L zurückkommt
if (!port.Write('R'))
return false;
int remaining = timeout.GetRemainingSigned();
if (remaining < 0)
return false;
if (remaining > 500)
remaining = 500;
Port::WaitResult result =
port.WaitForChar('L', env, remaining);
if (result == Port::WaitResult::READY)
break;
if (result != Port::WaitResult::TIMEOUT)
return false;
/* timeout, try again */
}
unsigned count = 1;
while (true) { // Auf 4 hintereinanderfolgende L's warten
int remaining = timeout.GetRemainingSigned();
if (remaining < 0)
return false;
if (port.WaitForChar('L', env, remaining) != Port::WaitResult::READY)
return false;
count++;
if (count >= 4)
return true;
}
}
int main(int argc, char **argv)
{
Args args(argc, argv, "PORT");
const DeviceConfig config = ParsePortArgs(args);
args.ExpectEnd();
InitialiseIOThread();
Port *port = OpenPort(config, nullptr, *(DataHandler *)nullptr);
if (port == nullptr) {
DeinitialiseIOThread();
fprintf(stderr, "Failed to open port\n");
return EXIT_FAILURE;
}
ConsoleOperationEnvironment env;
if (!port->WaitConnected(env)) {
delete port;
DeinitialiseIOThread();
fprintf(stderr, "Failed to connect the port\n");
return EXIT_FAILURE;
}
PeriodClock start_clock;
start_clock.Update();
PeriodClock pressure_clock;
PeriodClock battery_clock;
fixed pressure = fixed(101300);
unsigned battery_level = 11;
while (true) {
if (pressure_clock.CheckUpdate(48)) {
NarrowString<16> sentence;
int elapsed_ms = start_clock.Elapsed();
auto elapsed = fixed(elapsed_ms) / 1000;
auto vario = sin(elapsed / 3) * cos(elapsed / 10) *
cos(elapsed / 20 + fixed(2)) * 3;
auto pressure_vario = -vario * fixed(12.5);
auto delta_pressure = pressure_vario * 48 / 1000;
pressure += delta_pressure;
sentence = "_PRS ";
sentence.AppendFormat("%08X", uround(pressure));
sentence += "\n";
port->Write(sentence.c_str(), sentence.length());
}
if (battery_clock.CheckUpdate(11000)) {
NarrowString<16> sentence;
sentence = "_BAT ";
if (battery_level <= 10)
sentence.AppendFormat("%X", battery_level);
else
sentence += "*";
sentence += "\n";
port->Write(sentence.c_str(), sentence.length());
if (battery_level == 0)
battery_level = 11;
else
battery_level--;
}
}
}
