mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-30 06:34:38 +02:00
55 lines
1.7 KiB
C++
55 lines
1.7 KiB
C++
#include <iostream>
|
|
#include <iomanip>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <arpa/inet.h>
|
|
#include "packetTypes.h"
|
|
#include "util.h"
|
|
#include "net_wmm.h"
|
|
#include "TcpServer.h"
|
|
#include "InputReport.h"
|
|
|
|
using namespace std;
|
|
|
|
// Send an InputReport to a stream in a friendly format.
|
|
ostream &operator<<(ostream &os, const struct InputReportMessage &msg)
|
|
{
|
|
os << "msgId=" << msg.msgId;
|
|
os << " length=" << msg.length;
|
|
os << " timestamp=" << msg.timestamp;
|
|
os << " gimbalY=" << msg.gimbalY;
|
|
os << " gimbalRate=" << msg.gimbalRate;
|
|
os << " battery=" << msg.battery;
|
|
return os;
|
|
}
|
|
|
|
// Constructor - create and start the server that allows clients to connect
|
|
// and receive input reports
|
|
InputReport::InputReport(int port, int msg_rate)
|
|
: PacketHandler("127.0.0.1", port, IP_TOS_DEFAULT, PKT_ID_INPUT_REPORT),
|
|
tcpServer(port, "InputReport"), msgInterval_us(1000000 / msg_rate), nextMsgTime_us(0)
|
|
{
|
|
tcpServer.start();
|
|
}
|
|
|
|
// upHandler - called when an inpurt report message is received from the
|
|
// STM32. Prepend the timestamp and send it to all attached clients (if any).
|
|
// The timestamp is include in case it is useful for debug/analysis, and
|
|
// CLOCK_MONOTONIC is used because it seems more likely that message intervals
|
|
// are more interesting that absolute times.
|
|
int InputReport::upHandler(char serBuf[], int len)
|
|
{
|
|
uint64_t now_us;
|
|
now_us = clock_gettime_us(CLOCK_MONOTONIC);
|
|
if (now_us > nextMsgTime_us) {
|
|
InputReportMessage msg(now_us, serBuf);
|
|
tcpServer.send_clients(&msg, sizeof(msg));
|
|
// the following assumes CLOCK_MONOTONIC (no jumps)
|
|
if (nextMsgTime_us == 0)
|
|
nextMsgTime_us = now_us;
|
|
nextMsgTime_us += msgInterval_us;
|
|
}
|
|
return 0;
|
|
}
|