I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
VIPSSerialProtocol.h@72:da2f1df6a50d, 2022-02-10 (annotated)
- Committer:
- JamieB
- Date:
- Thu Feb 10 09:24:29 2022 +0000
- Revision:
- 72:da2f1df6a50d
- Parent:
- 69:47f800793d00
- Child:
- 82:ee6eed2a51bd
Added Interpolation offset option, added a re-setting timeout to VIPS RX in passthrough mode so TCP messages are sent as one message, rather than smaller chunks
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AndyA | 0:97661408d0f9 | 1 | #ifndef __VIPSSERIALPROTOCOL_H__ |
AndyA | 0:97661408d0f9 | 2 | #define __VIPSSERIALPROTOCOL_H__ |
AndyA | 0:97661408d0f9 | 3 | |
AndyA | 0:97661408d0f9 | 4 | #include "mbed.h" |
AndyA | 0:97661408d0f9 | 5 | #include "position.h" |
AndyA | 0:97661408d0f9 | 6 | #include "BufferedSerial.h" |
AndyA | 35:7ecf25d9c414 | 7 | #include "LTCApp.h" |
AndyA | 35:7ecf25d9c414 | 8 | #include "LowPassFilter.h" |
AndyA | 0:97661408d0f9 | 9 | |
JamieB | 72:da2f1df6a50d | 10 | #define MaxBuffSize 272 //The Get Unit Info LPC command (G N) returns 266 bytes |
JamieB | 72:da2f1df6a50d | 11 | |
JamieB | 72:da2f1df6a50d | 12 | |
JamieB | 69:47f800793d00 | 13 | |
AndyA | 0:97661408d0f9 | 14 | extern const char* VIPSStatusMessages[]; |
AndyA | 35:7ecf25d9c414 | 15 | struct UserSettings_s; |
AndyA | 0:97661408d0f9 | 16 | |
AndyA | 35:7ecf25d9c414 | 17 | class VIPSSerial |
AndyA | 35:7ecf25d9c414 | 18 | { |
AndyA | 0:97661408d0f9 | 19 | |
AndyA | 35:7ecf25d9c414 | 20 | public: |
AndyA | 0:97661408d0f9 | 21 | |
AndyA | 1:dd1f7e162f91 | 22 | VIPSSerial(const PinName Tx, const PinName Rx); |
AndyA | 1:dd1f7e162f91 | 23 | void run(void); |
AndyA | 35:7ecf25d9c414 | 24 | bool setFilters(struct UserSettings_s *settings); |
AndyA | 0:97661408d0f9 | 25 | |
AndyA | 0:97661408d0f9 | 26 | // send all position outputs rather than just when requested. |
AndyA | 0:97661408d0f9 | 27 | void sendAllUpdated(bool enable); |
AndyA | 0:97661408d0f9 | 28 | |
AndyA | 0:97661408d0f9 | 29 | // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer. |
AndyA | 0:97661408d0f9 | 30 | position* sendPositionForTime(uint32_t timeValue); |
AndyA | 35:7ecf25d9c414 | 31 | position* getWaitingPostion() |
AndyA | 35:7ecf25d9c414 | 32 | { |
AndyA | 35:7ecf25d9c414 | 33 | position *ptr = outputPtr; |
AndyA | 35:7ecf25d9c414 | 34 | outputPtr=NULL; |
AndyA | 35:7ecf25d9c414 | 35 | return ptr; |
AndyA | 35:7ecf25d9c414 | 36 | } |
AndyA | 0:97661408d0f9 | 37 | |
AndyA | 1:dd1f7e162f91 | 38 | static void getCRC(void *data, int len, void *checksum); |
AndyA | 1:dd1f7e162f91 | 39 | |
AndyA | 3:14d241e29be3 | 40 | // void setOutMask(uint32_t outputMask) {_outputMask = outputMask;}; |
AndyA | 3:14d241e29be3 | 41 | |
AndyA | 35:7ecf25d9c414 | 42 | bool EnableSmoothing(bool enabled) |
AndyA | 35:7ecf25d9c414 | 43 | { |
AndyA | 35:7ecf25d9c414 | 44 | hyperSmoothEnabled = enabled; |
AndyA | 35:7ecf25d9c414 | 45 | return hyperSmoothEnabled; |
AndyA | 35:7ecf25d9c414 | 46 | }; |
AndyA | 35:7ecf25d9c414 | 47 | bool SmoothingEnabled(void) |
AndyA | 35:7ecf25d9c414 | 48 | { |
AndyA | 35:7ecf25d9c414 | 49 | return hyperSmoothEnabled; |
AndyA | 35:7ecf25d9c414 | 50 | }; |
JamieB | 66:066b16c6c34f | 51 | bool ForceSmoothing(bool enabled) |
JamieB | 66:066b16c6c34f | 52 | { |
JamieB | 66:066b16c6c34f | 53 | forcedHyperSmooth = enabled; |
JamieB | 66:066b16c6c34f | 54 | hyperSmoothEnabled = enabled; |
JamieB | 66:066b16c6c34f | 55 | return hyperSmoothEnabled; |
JamieB | 66:066b16c6c34f | 56 | }; |
JamieB | 66:066b16c6c34f | 57 | bool ForceSmoothingEnabled(bool enabled) |
JamieB | 66:066b16c6c34f | 58 | { |
JamieB | 66:066b16c6c34f | 59 | return forcedHyperSmooth; |
JamieB | 66:066b16c6c34f | 60 | }; |
AndyA | 35:7ecf25d9c414 | 61 | int GetSmoothLevel(void) |
AndyA | 35:7ecf25d9c414 | 62 | { |
AndyA | 35:7ecf25d9c414 | 63 | return SmoothBy; |
AndyA | 35:7ecf25d9c414 | 64 | }; |
AndyA | 35:7ecf25d9c414 | 65 | bool SetSmoothLevel (const int newSmooth) |
AndyA | 35:7ecf25d9c414 | 66 | { |
AndyA | 35:7ecf25d9c414 | 67 | if (newSmooth == SmoothBy) return false; |
AndyA | 35:7ecf25d9c414 | 68 | SmoothBy = newSmooth; |
AndyA | 35:7ecf25d9c414 | 69 | SmoothRunning = false; |
AndyA | 35:7ecf25d9c414 | 70 | return true; |
AndyA | 35:7ecf25d9c414 | 71 | }; |
AndyA | 35:7ecf25d9c414 | 72 | void EnableBypass(bool enable) |
AndyA | 35:7ecf25d9c414 | 73 | { |
AndyA | 35:7ecf25d9c414 | 74 | BypassMode = enable; |
AndyA | 35:7ecf25d9c414 | 75 | }; |
AndyA | 16:a8d3a0dbe4bf | 76 | void bypassTx(char byte); |
AndyA | 18:ad407a2ed4c9 | 77 | void sendQueued(void); |
JamieB | 69:47f800793d00 | 78 | bool EnableDirectTX(bool enabled) |
JamieB | 69:47f800793d00 | 79 | { |
JamieB | 69:47f800793d00 | 80 | directTx = enabled; |
JamieB | 69:47f800793d00 | 81 | return directTx; |
JamieB | 69:47f800793d00 | 82 | }; |
JamieB | 69:47f800793d00 | 83 | void sendDirectTX(unsigned char* data, int dataLen); |
JamieB | 69:47f800793d00 | 84 | int getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend); |
JamieB | 69:47f800793d00 | 85 | void sendQuiet(void); |
AndyA | 16:a8d3a0dbe4bf | 86 | |
AndyA | 0:97661408d0f9 | 87 | private: |
AndyA | 0:97661408d0f9 | 88 | |
AndyA | 0:97661408d0f9 | 89 | struct posAndTime_s { |
AndyA | 0:97661408d0f9 | 90 | uint32_t time; |
AndyA | 0:97661408d0f9 | 91 | position pos; |
AndyA | 0:97661408d0f9 | 92 | }; |
AndyA | 0:97661408d0f9 | 93 | |
AndyA | 16:a8d3a0dbe4bf | 94 | void smoothOutputPacket(position *posPtr); |
AndyA | 1:dd1f7e162f91 | 95 | void onSerialRx(void); |
AndyA | 0:97661408d0f9 | 96 | void processRxMessage(); |
AndyA | 0:97661408d0f9 | 97 | bool checkCRC(unsigned char* data); |
AndyA | 0:97661408d0f9 | 98 | void sendResponse(unsigned char function, unsigned char* data, int dataLen); |
AndyA | 18:ad407a2ed4c9 | 99 | void queueResponse(unsigned char function, unsigned char* data, int dataLen); |
AndyA | 0:97661408d0f9 | 100 | void sendAck(unsigned char function); |
AndyA | 0:97661408d0f9 | 101 | void sendNack(unsigned char function); |
AndyA | 0:97661408d0f9 | 102 | void sendVBOXTime(); |
AndyA | 0:97661408d0f9 | 103 | void parsePostionInput_legacy(); |
AndyA | 0:97661408d0f9 | 104 | void parsePostionInput_mocap(); |
JamieB | 72:da2f1df6a50d | 105 | void onTxTimeout(); |
AndyA | 0:97661408d0f9 | 106 | bool checkNewPacketRC(unsigned char* data); |
AndyA | 1:dd1f7e162f91 | 107 | RawSerial _port; |
AndyA | 0:97661408d0f9 | 108 | unsigned char messageInBuffer[128]; |
AndyA | 35:7ecf25d9c414 | 109 | unsigned char messageOutBuffer[16]; |
JamieB | 69:47f800793d00 | 110 | unsigned char TXBuffer1[MaxBuffSize]; |
JamieB | 69:47f800793d00 | 111 | unsigned char TXBuffer2[MaxBuffSize]; |
JamieB | 69:47f800793d00 | 112 | unsigned char *txBuf; |
JamieB | 69:47f800793d00 | 113 | int waitingBytes; |
AndyA | 35:7ecf25d9c414 | 114 | #define posHistoryLen 3 |
AndyA | 0:97661408d0f9 | 115 | struct posAndTime_s lastPositions[posHistoryLen]; |
AndyA | 0:97661408d0f9 | 116 | int nextPosition; |
AndyA | 14:76083dc18b0d | 117 | struct posAndTime_s lastPos; // the most recent position received |
AndyA | 14:76083dc18b0d | 118 | struct posAndTime_s prevPos; // the most last but one position received |
AndyA | 0:97661408d0f9 | 119 | |
AndyA | 0:97661408d0f9 | 120 | position outputPosition; |
AndyA | 0:97661408d0f9 | 121 | position *outputPtr; |
AndyA | 0:97661408d0f9 | 122 | |
AndyA | 0:97661408d0f9 | 123 | int messagePrt; |
AndyA | 0:97661408d0f9 | 124 | int messageLength; |
AndyA | 0:97661408d0f9 | 125 | int statusMessage; |
AndyA | 0:97661408d0f9 | 126 | bool enableAllUpdates; |
AndyA | 0:97661408d0f9 | 127 | bool newFormatMsg; |
AndyA | 16:a8d3a0dbe4bf | 128 | bool hyperSmoothEnabled; |
JamieB | 66:066b16c6c34f | 129 | bool forcedHyperSmooth; |
JamieB | 69:47f800793d00 | 130 | bool directTx; |
AndyA | 0:97661408d0f9 | 131 | uint32_t pointCount; |
AndyA | 3:14d241e29be3 | 132 | uint32_t _outputMask; |
AndyA | 0:97661408d0f9 | 133 | |
AndyA | 18:ad407a2ed4c9 | 134 | int queueLen; |
AndyA | 16:a8d3a0dbe4bf | 135 | int SmoothBy; |
AndyA | 35:7ecf25d9c414 | 136 | // total as a float we would start to see rounding errors at valuses of ~20m |
AndyA | 16:a8d3a0dbe4bf | 137 | double XSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 138 | double YSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 139 | double ZSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 140 | bool SmoothRunning; |
AndyA | 16:a8d3a0dbe4bf | 141 | bool BypassMode; |
AndyA | 35:7ecf25d9c414 | 142 | LowPassFilter yFilter; |
AndyA | 35:7ecf25d9c414 | 143 | LowPassFilter xFilter; |
AndyA | 35:7ecf25d9c414 | 144 | LowPassFilter zFilter; |
AndyA | 35:7ecf25d9c414 | 145 | LowPassFilter rollFilter; |
AndyA | 35:7ecf25d9c414 | 146 | LowPassFilter pitchFilter; |
AndyA | 35:7ecf25d9c414 | 147 | LowPassFilter yawFilter; |
JamieB | 72:da2f1df6a50d | 148 | |
JamieB | 72:da2f1df6a50d | 149 | Timeout TxTimeout; |
JamieB | 72:da2f1df6a50d | 150 | bool TransmitFinished; |
JamieB | 72:da2f1df6a50d | 151 | //VIPSSerial* self; |
AndyA | 0:97661408d0f9 | 152 | }; |
AndyA | 0:97661408d0f9 | 153 | |
AndyA | 0:97661408d0f9 | 154 | #endif |