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@65:8ee40ed9295b, 2021-11-16 (annotated)
- Committer:
- JamieB
- Date:
- Tue Nov 16 16:08:26 2021 +0000
- Revision:
- 65:8ee40ed9295b
- Parent:
- 35:7ecf25d9c414
Temp commit - REMOVE;
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 | |
AndyA | 0:97661408d0f9 | 10 | extern const char* VIPSStatusMessages[]; |
AndyA | 35:7ecf25d9c414 | 11 | struct UserSettings_s; |
AndyA | 0:97661408d0f9 | 12 | |
AndyA | 35:7ecf25d9c414 | 13 | class VIPSSerial |
AndyA | 35:7ecf25d9c414 | 14 | { |
AndyA | 0:97661408d0f9 | 15 | |
AndyA | 35:7ecf25d9c414 | 16 | public: |
AndyA | 0:97661408d0f9 | 17 | |
AndyA | 1:dd1f7e162f91 | 18 | VIPSSerial(const PinName Tx, const PinName Rx); |
AndyA | 1:dd1f7e162f91 | 19 | void run(void); |
AndyA | 35:7ecf25d9c414 | 20 | bool setFilters(struct UserSettings_s *settings); |
AndyA | 0:97661408d0f9 | 21 | |
AndyA | 0:97661408d0f9 | 22 | // send all position outputs rather than just when requested. |
AndyA | 0:97661408d0f9 | 23 | void sendAllUpdated(bool enable); |
AndyA | 0:97661408d0f9 | 24 | |
AndyA | 0:97661408d0f9 | 25 | // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer. |
AndyA | 0:97661408d0f9 | 26 | position* sendPositionForTime(uint32_t timeValue); |
AndyA | 35:7ecf25d9c414 | 27 | position* getWaitingPostion() |
AndyA | 35:7ecf25d9c414 | 28 | { |
AndyA | 35:7ecf25d9c414 | 29 | position *ptr = outputPtr; |
AndyA | 35:7ecf25d9c414 | 30 | outputPtr=NULL; |
AndyA | 35:7ecf25d9c414 | 31 | return ptr; |
AndyA | 35:7ecf25d9c414 | 32 | } |
AndyA | 0:97661408d0f9 | 33 | |
AndyA | 1:dd1f7e162f91 | 34 | static void getCRC(void *data, int len, void *checksum); |
AndyA | 1:dd1f7e162f91 | 35 | |
AndyA | 3:14d241e29be3 | 36 | // void setOutMask(uint32_t outputMask) {_outputMask = outputMask;}; |
AndyA | 3:14d241e29be3 | 37 | |
AndyA | 35:7ecf25d9c414 | 38 | bool EnableSmoothing(bool enabled) |
AndyA | 35:7ecf25d9c414 | 39 | { |
AndyA | 35:7ecf25d9c414 | 40 | hyperSmoothEnabled = enabled; |
AndyA | 35:7ecf25d9c414 | 41 | return hyperSmoothEnabled; |
AndyA | 35:7ecf25d9c414 | 42 | }; |
JamieB | 65:8ee40ed9295b | 43 | bool ForceSmoothing(bool enabled) |
JamieB | 65:8ee40ed9295b | 44 | { |
JamieB | 65:8ee40ed9295b | 45 | forcedHyperSmooth = enabled; |
JamieB | 65:8ee40ed9295b | 46 | hyperSmoothEnabled = enabled; |
JamieB | 65:8ee40ed9295b | 47 | return hyperSmoothEnabled; |
JamieB | 65:8ee40ed9295b | 48 | }; |
JamieB | 65:8ee40ed9295b | 49 | bool ForceSmoothingEnabled(bool enabled) |
JamieB | 65:8ee40ed9295b | 50 | { |
JamieB | 65:8ee40ed9295b | 51 | return forcedHyperSmooth; |
JamieB | 65:8ee40ed9295b | 52 | }; |
AndyA | 35:7ecf25d9c414 | 53 | bool SmoothingEnabled(void) |
AndyA | 35:7ecf25d9c414 | 54 | { |
AndyA | 35:7ecf25d9c414 | 55 | return hyperSmoothEnabled; |
AndyA | 35:7ecf25d9c414 | 56 | }; |
AndyA | 16:a8d3a0dbe4bf | 57 | |
AndyA | 35:7ecf25d9c414 | 58 | int GetSmoothLevel(void) |
AndyA | 35:7ecf25d9c414 | 59 | { |
AndyA | 35:7ecf25d9c414 | 60 | return SmoothBy; |
AndyA | 35:7ecf25d9c414 | 61 | }; |
AndyA | 35:7ecf25d9c414 | 62 | bool SetSmoothLevel (const int newSmooth) |
AndyA | 35:7ecf25d9c414 | 63 | { |
AndyA | 35:7ecf25d9c414 | 64 | if (newSmooth == SmoothBy) return false; |
AndyA | 35:7ecf25d9c414 | 65 | SmoothBy = newSmooth; |
AndyA | 35:7ecf25d9c414 | 66 | SmoothRunning = false; |
AndyA | 35:7ecf25d9c414 | 67 | return true; |
AndyA | 35:7ecf25d9c414 | 68 | }; |
AndyA | 35:7ecf25d9c414 | 69 | void EnableBypass(bool enable) |
AndyA | 35:7ecf25d9c414 | 70 | { |
AndyA | 35:7ecf25d9c414 | 71 | BypassMode = enable; |
AndyA | 35:7ecf25d9c414 | 72 | }; |
AndyA | 16:a8d3a0dbe4bf | 73 | void bypassTx(char byte); |
AndyA | 18:ad407a2ed4c9 | 74 | void sendQueued(void); |
AndyA | 16:a8d3a0dbe4bf | 75 | |
AndyA | 0:97661408d0f9 | 76 | private: |
AndyA | 0:97661408d0f9 | 77 | |
AndyA | 0:97661408d0f9 | 78 | struct posAndTime_s { |
AndyA | 0:97661408d0f9 | 79 | uint32_t time; |
AndyA | 0:97661408d0f9 | 80 | position pos; |
AndyA | 0:97661408d0f9 | 81 | }; |
AndyA | 0:97661408d0f9 | 82 | |
AndyA | 16:a8d3a0dbe4bf | 83 | void smoothOutputPacket(position *posPtr); |
AndyA | 1:dd1f7e162f91 | 84 | void onSerialRx(void); |
AndyA | 0:97661408d0f9 | 85 | void processRxMessage(); |
AndyA | 0:97661408d0f9 | 86 | bool checkCRC(unsigned char* data); |
AndyA | 0:97661408d0f9 | 87 | void sendResponse(unsigned char function, unsigned char* data, int dataLen); |
AndyA | 18:ad407a2ed4c9 | 88 | void queueResponse(unsigned char function, unsigned char* data, int dataLen); |
AndyA | 0:97661408d0f9 | 89 | void sendAck(unsigned char function); |
AndyA | 0:97661408d0f9 | 90 | void sendNack(unsigned char function); |
AndyA | 0:97661408d0f9 | 91 | void sendVBOXTime(); |
AndyA | 0:97661408d0f9 | 92 | void parsePostionInput_legacy(); |
AndyA | 0:97661408d0f9 | 93 | void parsePostionInput_mocap(); |
AndyA | 0:97661408d0f9 | 94 | bool checkNewPacketRC(unsigned char* data); |
AndyA | 1:dd1f7e162f91 | 95 | RawSerial _port; |
AndyA | 0:97661408d0f9 | 96 | unsigned char messageInBuffer[128]; |
AndyA | 35:7ecf25d9c414 | 97 | unsigned char messageOutBuffer[16]; |
AndyA | 35:7ecf25d9c414 | 98 | #define posHistoryLen 3 |
AndyA | 0:97661408d0f9 | 99 | struct posAndTime_s lastPositions[posHistoryLen]; |
AndyA | 0:97661408d0f9 | 100 | int nextPosition; |
AndyA | 14:76083dc18b0d | 101 | struct posAndTime_s lastPos; // the most recent position received |
AndyA | 14:76083dc18b0d | 102 | struct posAndTime_s prevPos; // the most last but one position received |
AndyA | 0:97661408d0f9 | 103 | |
AndyA | 0:97661408d0f9 | 104 | position outputPosition; |
AndyA | 0:97661408d0f9 | 105 | position *outputPtr; |
AndyA | 0:97661408d0f9 | 106 | |
AndyA | 0:97661408d0f9 | 107 | int messagePrt; |
AndyA | 0:97661408d0f9 | 108 | int messageLength; |
AndyA | 0:97661408d0f9 | 109 | int statusMessage; |
AndyA | 0:97661408d0f9 | 110 | bool enableAllUpdates; |
AndyA | 0:97661408d0f9 | 111 | bool newFormatMsg; |
AndyA | 16:a8d3a0dbe4bf | 112 | bool hyperSmoothEnabled; |
AndyA | 0:97661408d0f9 | 113 | uint32_t pointCount; |
AndyA | 3:14d241e29be3 | 114 | uint32_t _outputMask; |
JamieB | 65:8ee40ed9295b | 115 | bool forcedHyperSmooth; |
AndyA | 0:97661408d0f9 | 116 | |
AndyA | 18:ad407a2ed4c9 | 117 | int queueLen; |
AndyA | 16:a8d3a0dbe4bf | 118 | int SmoothBy; |
AndyA | 35:7ecf25d9c414 | 119 | // total as a float we would start to see rounding errors at valuses of ~20m |
AndyA | 16:a8d3a0dbe4bf | 120 | double XSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 121 | double YSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 122 | double ZSmoothTotal; |
AndyA | 16:a8d3a0dbe4bf | 123 | bool SmoothRunning; |
AndyA | 16:a8d3a0dbe4bf | 124 | bool BypassMode; |
AndyA | 35:7ecf25d9c414 | 125 | LowPassFilter yFilter; |
AndyA | 35:7ecf25d9c414 | 126 | LowPassFilter xFilter; |
AndyA | 35:7ecf25d9c414 | 127 | LowPassFilter zFilter; |
AndyA | 35:7ecf25d9c414 | 128 | LowPassFilter rollFilter; |
AndyA | 35:7ecf25d9c414 | 129 | LowPassFilter pitchFilter; |
AndyA | 35:7ecf25d9c414 | 130 | LowPassFilter yawFilter; |
AndyA | 0:97661408d0f9 | 131 | }; |
AndyA | 0:97661408d0f9 | 132 | |
AndyA | 0:97661408d0f9 | 133 | #endif |