Mirrors/OpenSolo
1
0
Fork 0
mirror of https://github.com/OpenSolo/OpenSolo.git synced 2025-04-29 22:24:32 +02:00
Code Issues Packages Projects Releases Wiki Activity
OpenSolo/sololink/flightcode/stm32/stm32.cpp

791 lines
28 KiB
C++
Raw Blame History

#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <vector>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <poll.h>
#include <pthread.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <syslog.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include "util.h"
#include "mutex.h"
#include "RC.h"
#include "Telem.h"
#include "SysInfo.h"
#include "PairReq.h"
#include "PairRes.h"
#include "ParamStoredVals.h"
#include "ConfigStickAxes.h"
#include "SetTelemUnits.h"
#include "ConfigSweepTime.h"
#include "ButtonEventHandler.h"
#include "InputReport.h"
#include "ButtonFunctionCfg.h"
#include "SetShotInfo.h"
#include "Updater.h"
#include "LockoutState.h"
#include "AppConnected.h"
#include "SLIP.h"
#include "packetTypes.h"
#include "INIReader.h"
#include "net_wmm.h"
#ifdef INCLUDE_SERIAL_LOG
#include "SerialLog.h"
#endif // INCLUDE_SERIAL_LOG
using namespace std;
/***********************************************************************
Timeouts
***********************************************************************/
#define LOG_DT_US 10000000 // 10s
/***********************************************************************
Serial port name
***********************************************************************/
string serialPortName;
/***********************************************************************
Serial port receive buffer
***********************************************************************/
#define SERIAL_BUFSIZE 4096
#define MAX_SERIAL_MESSAGE_LEN 1024
/***********************************************************************
Thread IDs
***********************************************************************/
pthread_t upstream_ctx;
pthread_t downstream_ctx;
/***********************************************************************
Thread priorities
***********************************************************************/
#define UPSTREAM_PRIORITY 60
#define DOWNSTREAM_PRIORITY 56
/***********************************************************************
File descriptors
***********************************************************************/
int serial_fd;
#ifdef INCLUDE_SERIAL_LOG
/***********************************************************************
Serial port log
***********************************************************************/
SerialLog *serialLog;
unsigned serialLogSize;
unsigned serialLogTimeout_us;
#endif // INCLUDE_SERIAL_LOG
/***********************************************************************
The amount of data sent/received every cycle
***********************************************************************/
unsigned upBytes = 0;
unsigned upMsgs[PKT_ID_MAX + 1];
pthread_mutex_t mutex_upBytes;
/***********************************************************************
Handler objects
***********************************************************************/
RC *rc;
Telem *telem;
SysInfo *sysInfo;
PairReq *pairReq;
PairRes *pairRes;
ParamStoredVals *paramStoredVals;
ConfigStickAxes *configStickAxes;
SetTelemUnits *setTelemUnits;
ConfigSweepTime *configSweepTime;
ButtonEventHandler *btnEvt;
InputReport *inputReport;
ButtonFunctionCfg *btnFuncCfg;
SetShotInfo *setShotInfo;
Updater *updater;
LockoutState *lockoutState;
AppConnected *appConn;
/***********************************************************************
Debug control
***********************************************************************/
static uint32_t dbg_downHandler = 0;
/***********************************************************************
Function: void lock_thread_mem(uint32_t memsize)
Description: Locks memory in the thread so that stack faults don't cause
timing jitter.
***********************************************************************/
static void lock_thread_mem(uint32_t memsize)
{
uint8_t stackmem[memsize];
mlockall(MCL_CURRENT | MCL_FUTURE);
memset(stackmem, 0, sizeof(stackmem));
}
/***********************************************************************
Function: int serial_setup(int baudrate)
Description: The serial port initialization function. This function
initializes the serial port over which data is sent and
received from the STM32. A return of 0 indicates an error.
***********************************************************************/
int serial_setup(int baudrate)
{
struct termios options;
serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY); // Blocking read
if (serial_fd < 0) {
syslog(LOG_ERR, "unable to open serial port %s", serialPortName.c_str());
return 0;
}
// Configure port for 8N1 transmission
tcgetattr(serial_fd, &options); // Gets the current options for the port
switch (baudrate) {
case 57600:
cfsetspeed(&options, B57600);
break;
case 115200:
cfsetspeed(&options, B115200);
break;
case 1500000:
cfsetspeed(&options, B1500000);
break;
default:
syslog(LOG_ERR, "unsupported baudrate %d", baudrate);
return 0;
}
options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
options.c_cflag &= ~(CSIZE | PARENB);
options.c_cflag |= CS8;
options.c_cc[VMIN] = 1;
options.c_cc[VTIME] = 0;
tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW"
syslog(LOG_INFO, "opened serial port %s", serialPortName.c_str());
return 1;
}
static string msgToString(const char *msg, int msgLen)
{
stringstream s;
// dealing with the unknown; cap the number of bytes to log
int printLen = msgLen;
if (printLen > 8)
printLen = 8;
while (printLen-- > 0) {
s << setfill('0') << setw(2) << hex << unsigned(*msg);
if (printLen > 0)
s << " ";
msg++;
}
s << setfill(' ') << setw(0) << dec;
if (msgLen > 8)
s << "... (" << msgLen << " bytes long)";
return s.str();
}
/***********************************************************************
Function: void *upstream_task(void*)
Description: The upstream thread task. Checks for data available on the
serial and calls the relevant handler.
***********************************************************************/
const int POLLTIMEOUT_MS = 10000;
void *upstream_task(void *)
{
char buf[SERIAL_BUFSIZE];
char msg[MAX_SERIAL_MESSAGE_LEN];
struct pollfd fds[1];
int pollrc;
int i;
int msgLen = 0;
int readlen;
SLIPDecoder *slipDec = new SLIPDecoder(msg, sizeof(msg));
uint64_t now_us;
uint64_t unknown_us = 0;
unsigned unknown_skip_cnt = 0;
bool in_sync = false;
bool invalid_stick_inputs_logged = false;
fds[0].fd = serial_fd;
fds[0].events = POLLIN;
lock_thread_mem(32764);
while (true) {
// Wait for available data, block until we get some or timeout
pollrc = poll(fds, 1, POLLTIMEOUT_MS);
if (pollrc > 0 && fds[0].revents & POLLIN) {
// Check for any new incoming data, send over udp
readlen = read(serial_fd, buf, SERIAL_BUFSIZE);
if (readlen > 0) {
for (i = 0; i < readlen; ++i) {
// Decode slip packets. Returns:
// 0 : packet in progress
// -1 : error in bytes stream, e.g. invalid sync sequence
// >0 : packet received
msgLen = slipDec->addByte(&buf[i]);
if (msgLen <= 0) {
if (msgLen < 0) {
syslog(LOG_INFO, "slip error - discarding data");
in_sync = false;
}
continue;
}
if (!in_sync) {
// First packet after an error (or init) is no good,
// since we generally did not start after a previous
// packet's sync. But at this point, we *did* just
// receive a sync, so we drop the current packet and
// go get the next one.
syslog(LOG_INFO, "slip - initial sync");
in_sync = true;
continue;
}
// OK, got a message
#ifdef INCLUDE_SERIAL_LOG
serialLog->log_packet(msg, msgLen, SerialLog::PKTFLG_UP);
#endif // INCLUDE_SERIAL_LOG
// syslog(LOG_INFO, "packet %d: %d bytes", int(msg[0]), msgLen);
// Get the packet type from the first byte
// When passing the message, skip the first byte
// which is the packet type.
unsigned up = 0;
switch (msg[0]) {
case PKT_ID_DSM_CHANNELS:
up = rc->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_CALIBRATE:
break;
case PKT_ID_SYS_INFO:
up = sysInfo->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_MAVLINK:
up = telem->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_PAIR_CONFIRM:
up = pairReq->upHandler(&msg[1], msgLen - 1);
break;
// PKT_ID_PAIR_RESULT is never upstream
case PKT_ID_PARAM_STORED_VALS:
up = paramStoredVals->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_SHUTDOWN_REQUEST:
// Call the system shutdown command
syslog(LOG_INFO, "STM32 requested shutdown");
system("shutdown -h now");
sleep(30); // should get killed in here
exit(1); // init may restart us if we get here
break;
case PKT_ID_BUTTON_EVENT:
up = btnEvt->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_INPUT_REPORT:
up = inputReport->upHandler(&msg[1], msgLen - 1);
break;
case PKT_ID_INVALID_STICK_INPUTS:
// polluting log - print one then ignore
if (!invalid_stick_inputs_logged) {
string s = msgToString(msg, msgLen);
syslog(LOG_INFO, "received invalid stick inputs message: %s",
s.c_str());
invalid_stick_inputs_logged = true;
}
break;
case PKT_ID_NOP:
default:
// Print unknown messages, but max one per second
// Rate limit so if the STM32 firmware gets ahead
// and emits a new message, we don't soak the log.
now_us = clock_gettime_us(CLOCK_MONOTONIC);
if (now_us >= unknown_us) {
// okay to print it
string s = msgToString(msg, msgLen);
syslog(LOG_ERR, "unknown packet: %s", s.c_str());
if (unknown_skip_cnt != 0) {
syslog(LOG_ERR, "(%u more not printed)", unknown_skip_cnt);
unknown_skip_cnt = 0;
}
// don't print another for 1,000,000 usec
unknown_us = now_us + 1000000;
} else {
// too soon after the previous one; don't print
unknown_skip_cnt++;
}
break;
}
unsigned id = msg[0];
if (id > PKT_ID_MAX)
id = PKT_ID_MAX;
pthread_mutex_lock(&mutex_upBytes);
upMsgs[id]++; // upMsgs is [PKT_ID_MAX + 1]
upBytes += up;
pthread_mutex_unlock(&mutex_upBytes);
}
}
} else if (pollrc == 0) {
// timeout
syslog(LOG_INFO, "no data received from STM32 for %d msec", POLLTIMEOUT_MS);
}
} // while (true)
pthread_exit(NULL);
}
/**********************************************************************
Function: int start_upstream_thread(void)
Description: Starts the upstream thread
***********************************************************************/
int start_upstream_thread(void)
{
int ret = 0;
struct sched_param param;
// Start the upstream thread
pthread_create(&upstream_ctx, NULL, upstream_task, NULL);
pthread_setname_np(upstream_ctx, "stm32_up");
// upstream priority and schedule
param.sched_priority = UPSTREAM_PRIORITY;
if ((ret = pthread_setschedparam(upstream_ctx, SCHED_FIFO, &param)))
syslog(LOG_ERR, "error %d setting upstream thread priority", ret);
return ret;
}
/**********************************************************************
Function: void *downstream_task(void*)
Description: The downstream thread task. Selects() on the set of
fds and calls the relevant handler
***********************************************************************/
void *downstream_task(void *)
{
int nfds = 0;
int res = 0;
fd_set fds;
int bytesAvailable;
struct timeval timeout;
uint64_t log_last_us;
unsigned downBytes = 0;
lock_thread_mem(32764);
log_last_us = clock_gettime_us(CLOCK_MONOTONIC);
// Select() on the list of UDP FDs.
while (1) {
// Block until we get one
// Generate the list of fds
FD_ZERO(&fds);
FD_SET(telem->getfd(), &fds);
nfds = telem->getfd() + 1;
FD_SET(pairReq->getfd(), &fds);
if (pairReq->getfd() >= nfds)
nfds = pairReq->getfd() + 1;
FD_SET(pairRes->getfd(), &fds);
if (pairRes->getfd() >= nfds)
nfds = pairRes->getfd() + 1;
FD_SET(sysInfo->getfd(), &fds);
if (sysInfo->getfd() >= nfds)
nfds = sysInfo->getfd() + 1;
FD_SET(paramStoredVals->getfd(), &fds);
if (paramStoredVals->getfd() >= nfds)
nfds = paramStoredVals->getfd() + 1;
FD_SET(configStickAxes->getfd(), &fds);
if (configStickAxes->getfd() >= nfds)
nfds = configStickAxes->getfd() + 1;
FD_SET(setTelemUnits->getfd(), &fds);
if (setTelemUnits->getfd() >= nfds)
nfds = setTelemUnits->getfd() + 1;
FD_SET(configSweepTime->getfd(), &fds);
if (configSweepTime->getfd() >= nfds)
nfds = configSweepTime->getfd() + 1;
FD_SET(btnFuncCfg->getfd(), &fds);
if (btnFuncCfg->getfd() >= nfds)
nfds = btnFuncCfg->getfd() + 1;
FD_SET(setShotInfo->getfd(), &fds);
if (setShotInfo->getfd() >= nfds)
nfds = setShotInfo->getfd() + 1;
FD_SET(updater->getfd(), &fds);
if (updater->getfd() >= nfds)
nfds = updater->getfd() + 1;
FD_SET(lockoutState->getfd(), &fds);
if (lockoutState->getfd() >= nfds)
nfds = lockoutState->getfd() + 1;
FD_SET(appConn->getfd(), &fds);
if (appConn->getfd() >= nfds)
nfds = appConn->getfd() + 1;
// select timeout 0.1 sec
// (note that select can update timeout)
// need to time out, so if downlink stops we still see stats
timeout.tv_sec = 0;
timeout.tv_usec = 100000;
res = select(nfds, &fds, NULL, NULL, &timeout);
if (res > 0) {
// Run the appropriate downstream handler
if (FD_ISSET(telem->getfd(), &fds)) {
// record the amount of data we download
ioctl(telem->getfd(), FIONREAD, &bytesAvailable);
// Run the handler
telem->downHandler(serial_fd);
// Record the amount of data we got down
downBytes += bytesAvailable;
}
if (FD_ISSET(pairReq->getfd(), &fds)) {
// Don't increment the amount of data we send here
syslog(LOG_INFO, "pair request going down");
pairReq->downHandler(serial_fd, dbg_downHandler);
}
if (FD_ISSET(pairRes->getfd(), &fds)) {
// Don't increment the amount of data we send here
syslog(LOG_INFO, "pair result going down");
pairRes->downHandler(serial_fd, dbg_downHandler);
}
if (FD_ISSET(sysInfo->getfd(), &fds)) {
// Don't increment the amount of data we send here
sysInfo->downHandler(serial_fd, dbg_downHandler);
}
if (FD_ISSET(paramStoredVals->getfd(), &fds))
paramStoredVals->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(configStickAxes->getfd(), &fds))
configStickAxes->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(setTelemUnits->getfd(), &fds))
setTelemUnits->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(configSweepTime->getfd(), &fds))
configSweepTime->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(btnFuncCfg->getfd(), &fds))
btnFuncCfg->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(setShotInfo->getfd(), &fds))
setShotInfo->downHandler(serial_fd, dbg_downHandler);
if (FD_ISSET(updater->getfd(), &fds)) {
// Don't increment the amount of data we send here
syslog(LOG_INFO, "updater message going down");
updater->downHandler(serial_fd, dbg_downHandler);
}
if (FD_ISSET(lockoutState->getfd(), &fds)) {
// Don't increment the amount of data we send here
syslog(LOG_INFO, "lockout state message going down");
lockoutState->downHandler(serial_fd, dbg_downHandler);
}
if (FD_ISSET(appConn->getfd(), &fds)) {
// Don't increment the amount of data we send here
syslog(LOG_INFO, "app connected message going down");
appConn->downHandler(serial_fd, dbg_downHandler);
}
}
// check for time to log status logging
uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
uint64_t delta = now_us - log_last_us;
if (delta >= LOG_DT_US) {
unsigned newUpBytes;
unsigned newUpMsgs[PKT_ID_MAX + 1];
pthread_mutex_lock(&mutex_upBytes);
newUpBytes = upBytes;
upBytes = 0;
memcpy(newUpMsgs, upMsgs, sizeof(newUpMsgs));
memset(upMsgs, 0, sizeof(upMsgs));
pthread_mutex_unlock(&mutex_upBytes);
log_last_us = now_us;
// normally need about 20 chars tops
char msg[200];
int x = sizeof(msg) - 1;
char *p = msg;
memset(msg, 0, sizeof(msg));
for (int i = 0; i <= PKT_ID_MAX; i++) {
if (newUpMsgs[i] != 0) {
int k = snprintf(p, x, "%d:%u ", i, newUpMsgs[i]);
if (k >= x)
break;
x -= k;
p += k;
}
}
syslog(LOG_INFO, "up %s%u B %0.2f KB/s dn %u B %0.2f KB/s", msg, newUpBytes,
newUpBytes / 1024.0 / (delta / 1e6), downBytes,
downBytes / 1024.0 / (delta / 1e6));
downBytes = 0;
}
}
pthread_exit(NULL);
}
/**********************************************************************
Function: int start_downstream_thread(void)
Description: Starts the downstream thread
***********************************************************************/
int start_downstream_thread(void)
{
int ret = 0;
struct sched_param param;
// Start the downstream thread
pthread_create(&downstream_ctx, NULL, downstream_task, NULL);
pthread_setname_np(downstream_ctx, "stm32_down");
// Set the priority and scheduler
memset(&param, 0, sizeof(param));
param.sched_priority = DOWNSTREAM_PRIORITY;
if ((ret = pthread_setschedparam(downstream_ctx, SCHED_FIFO, &param)))
syslog(LOG_ERR, "error %d setting downstream thread priority", ret);
return ret;
}
/**********************************************************************
Function: bool readSoloIP(string filename, string *ip)
Description: Reads the Solo's IP address from the solo Address File
***********************************************************************/
bool readSoloIP(string filename, string *ip)
{
ifstream fd;
// Open the address file, return false if its not been created yet
fd.open(filename.c_str(), ios::in);
if (!fd)
return false;
ip->clear();
fd >> *ip;
fd.close();
return true;
}
/**********************************************************************
Function: int main(void)
Description: The main function. Initializes and runs the serial and
UDP threads.
***********************************************************************/
int main(void)
{
int rcDestPort;
int rcUpTos;
int mavDestPort;
int sysDestPort;
int pairReqDestPort;
int pairResDestPort;
int paramStoredValsDestPort;
int configStickAxesDestPort;
int setTelemUnitsDestPort;
string setTelemUnitsSetting;
int telemLogGap;
int telemLogDelayMax;
string telemLogDelayFile;
int configSweepTimeDestPort;
int btnEvtDestPort;
int inputReportDestPort;
int btnFuncCfgDestPort;
int setShotInfoDestPort;
int updaterDestPort;
int lockoutStateDestPort;
int appConnDestPort;
int baudrate;
string soloIPAddr;
bool haveSoloIP = false;
openlog("stm32", LOG_NDELAY, LOG_LOCAL0);
syslog(LOG_INFO, "main: starting: built " __DATE__ " " __TIME__);
/* Parse the sololink.conf file for serial port, source IPs and ports */
INIReader reader("/etc/sololink.conf");
if (reader.ParseError() < 0) {
syslog(LOG_ERR, "main: can't load /etc/sololink.conf");
return -1;
}
/* Get all parameter info */
serialPortName = reader.Get("solo", "stm32Dev", "/dev/ttymxc1");
rcDestPort = reader.GetInteger("solo", "rcDestPort", 5005);
rcUpTos = reader.GetInteger("solo", "rcUpTos", 0xff);
mavDestPort = reader.GetInteger("solo", "mavDestPort", 5015);
sysDestPort = reader.GetInteger("solo", "sysDestPort", 5012);
pairReqDestPort = reader.GetInteger("solo", "pairReqDestPort", 5013);
pairResDestPort = reader.GetInteger("solo", "pairResDestPort", 5014);
paramStoredValsDestPort = reader.GetInteger("solo", "paramStoredValsDestPort", 5011);
configStickAxesDestPort = reader.GetInteger("solo", "configStickAxesDestPort", 5010);
setTelemUnitsDestPort = reader.GetInteger("solo", "setTelemUnitsDestPort", 5024);
setTelemUnitsSetting = reader.Get("solo", "uiUnits", "metric");
telemLogGap = reader.GetInteger("solo", "telemLogGap", 1000000);
syslog(LOG_INFO, "logging telemetry gaps >= %d msec", (telemLogGap + 500) / 1000);
telemLogDelayMax =
reader.GetInteger("solo", "telemLogDelayMax", 100000); // 100,000 is ~20 minutes, 6.4 MB
telemLogDelayFile = reader.Get("solo", "telemLogDelayFile", "");
if (telemLogDelayFile != "")
syslog(LOG_INFO, "main: logging packet delays to %s (%d records per file)",
telemLogDelayFile.c_str(), telemLogDelayMax);
configSweepTimeDestPort = reader.GetInteger("solo", "configSweepTimeDestPort", 5022);
btnEvtDestPort = reader.GetInteger("solo", "buttonEventDestPort", 5016);
inputReportDestPort = reader.GetInteger("solo", "inputReportDestPort", 5021);
btnFuncCfgDestPort = reader.GetInteger("solo", "buttonFunctionConfigDestPort", 5017);
setShotInfoDestPort = reader.GetInteger("solo", "setShotInfoDestPort", 5018);
updaterDestPort = reader.GetInteger("solo", "updaterDestPort", 5019);
lockoutStateDestPort = reader.GetInteger("solo", "lockoutStateDestPort", 5020);
appConnDestPort = reader.GetInteger("solo", "appConnDestPort", 5026);
baudrate = reader.GetInteger("solo", "stm32Baud", 115200);
dbg_downHandler = reader.GetInteger("solo", "dbg_downHandler", 0);
if (dbg_downHandler != 0)
syslog(LOG_INFO, "main: dbg_downHandler=0x%08x", dbg_downHandler);
#ifdef INCLUDE_SERIAL_LOG
// serial log
serialLogSize = reader.GetInteger("solo", "serialLogSize", 50000);
serialLogTimeout_us = reader.GetInteger("solo", "serialLogTimeout_us", 1000000);
serialLog = new SerialLog(serialLogSize, serialLogTimeout_us);
#endif // INCLUDE_SERIAL_LOG
// shared variable mutex
if (mutex_init(&mutex_upBytes) != 0) {
syslog(LOG_ERR, "main: can't initialize mutex");
return -1;
}
// initialize all the handler objects
// The RC destination address gets updated by the Solo IP address file. We don't
// send any data to the Solo until we've got that address
rc = new RC("", rcDestPort, rcUpTos);
telem = new Telem("127.0.0.1", mavDestPort, telemLogGap, telemLogDelayMax, telemLogDelayFile);
sysInfo = new SysInfo("127.0.0.1", sysDestPort);
pairReq = new PairReq("127.0.0.1", pairReqDestPort);
pairRes = new PairRes("127.0.0.1", pairResDestPort);
paramStoredVals = new ParamStoredVals(paramStoredValsDestPort);
configStickAxes = new ConfigStickAxes(configStickAxesDestPort);
setTelemUnits = new SetTelemUnits(setTelemUnitsDestPort);
configSweepTime = new ConfigSweepTime(configSweepTimeDestPort);
btnEvt = new ButtonEventHandler(btnEvtDestPort);
inputReport = new InputReport(inputReportDestPort, 1);
btnFuncCfg = new ButtonFunctionCfg(btnFuncCfgDestPort);
setShotInfo = new SetShotInfo(setShotInfoDestPort);
updater = new Updater("127.0.0.1", updaterDestPort);
lockoutState = new LockoutState(lockoutStateDestPort);
appConn = new AppConnected("127.0.0.1", appConnDestPort);
/* Start the serial port */
if (!serial_setup(baudrate)) {
syslog(LOG_ERR, "main: unable to initialize serial port");
return -1;
}
/* Start the upstream and downstream threads */
start_downstream_thread();
start_upstream_thread();
// Send a ping to get the STM32 information in the log
sysInfo->ping();
// Set the telemetry display units
setTelemUnits->set(setTelemUnitsSetting);
// The main while() loop just becomes a thread monitor. If any thread dies,
// exit and let inittab restart us
// We also check to see if the solo IP address has been written yet
while (true) {
if (pthread_kill(upstream_ctx, 0) != 0 || pthread_kill(downstream_ctx, 0) != 0) {
syslog(LOG_ERR, "a thread is not running, exiting");
exit(0);
}
// Check the Solo IP address file
if (!haveSoloIP) {
haveSoloIP = readSoloIP("/var/run/solo.ip", &soloIPAddr);
if (haveSoloIP) {
syslog(LOG_INFO, "got solo ip: %s", soloIPAddr.c_str());
rc->setSoloIP(&soloIPAddr);
}
}
// We also "ping" the STM32 occasionally to make sure its still talking to us.
sysInfo->ping();
// Long sleep
sleep(1);
}
return -1;
}
Reference in New Issue View Git Blame Copy Permalink