diff --git a/src/deck/drivers/interface/locodeck.h b/src/deck/drivers/interface/locodeck.h index 344c4b1957..deb6249e3c 100644 --- a/src/deck/drivers/interface/locodeck.h +++ b/src/deck/drivers/interface/locodeck.h @@ -61,7 +61,7 @@ typedef struct { const locoAddress_t anchorAddress[LOCODECK_NR_OF_ANCHORS]; point_t anchorPosition[LOCODECK_NR_OF_ANCHORS]; - bool anchorPositionOk; + bool combinedAnchorPositionOk; float distance[LOCODECK_NR_OF_ANCHORS]; float pressures[LOCODECK_NR_OF_ANCHORS]; @@ -69,6 +69,8 @@ typedef struct { volatile uint16_t rangingState; } lpsAlgoOptions_t; +point_t* locodeckGetAnchorPosition(uint8_t anchor); + // Callback for one uwb algorithm typedef struct uwbAlgorithm_s { void (*init)(dwDevice_t *dev, lpsAlgoOptions_t* options); diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c index a5b89eb0f7..67b725eabf 100644 --- a/src/deck/drivers/src/locodeck.c +++ b/src/deck/drivers/src/locodeck.c @@ -69,7 +69,7 @@ // The anchor position can be set using parameters // As an option you can set a static position in this file and set -// anchorPositionOk to enable sending the anchor rangings to the Kalman filter +// combinedAnchorPositionOk to enable sending the anchor rangings to the Kalman filter static lpsAlgoOptions_t algoOptions = { .tagAddress = 0xbccf000000000008, @@ -84,21 +84,27 @@ static lpsAlgoOptions_t algoOptions = { .antennaDelay = (ANTENNA_OFFSET*499.2e6*128)/299792458.0, // In radio tick .rangingFailedThreshold = 6, - .anchorPositionOk = false, + .combinedAnchorPositionOk = false, // To set a static anchor position from startup, uncomment and modify the // following code: - // .anchorPosition = { - // {x: 0.99, y: 1.49, z: 1.80}, - // {x: 0.99, y: 3.29, z: 1.80}, - // {x: 4.67, y: 2.54, z: 1.80}, - // {x: 0.59, y: 2.27, z: 0.20}, - // {x: 4.70, y: 3.38, z: 0.20}, - // {x: 4.70, y: 1.14, z: 0.20}, - // }, - // .anchorPositionOk = true, +// .anchorPosition = { +// {timestamp: 4711, x: 0.99, y: 1.49, z: 1.80}, +// {x: 0.99, y: 3.29, z: 1.80}, +// {x: 4.67, y: 2.54, z: 1.80}, +// {timestamp: 17, x: 0.59, y: 2.27, z: 0.20}, +// {x: 4.70, y: 3.38, z: 0.20}, +// {timestamp: 13, x: 4.70, y: 1.14, z: 0.20}, +// }, +// +// .combinedAnchorPositionOk = true, }; +point_t* locodeckGetAnchorPosition(uint8_t anchor) +{ + return &algoOptions.anchorPosition[anchor]; +} + #if LPS_TDOA_ENABLE static uwbAlgorithm_t *algorithm = &uwbTdoaTagAlgorithm; #else @@ -423,5 +429,5 @@ PARAM_ADD(PARAM_FLOAT, anchor7x, &algoOptions.anchorPosition[7].x) PARAM_ADD(PARAM_FLOAT, anchor7y, &algoOptions.anchorPosition[7].y) PARAM_ADD(PARAM_FLOAT, anchor7z, &algoOptions.anchorPosition[7].z) #endif -PARAM_ADD(PARAM_UINT8, enable, &algoOptions.anchorPositionOk) +PARAM_ADD(PARAM_UINT8, enable, &algoOptions.combinedAnchorPositionOk) PARAM_GROUP_STOP(anchorpos) diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c index f9547df7c9..a64e921f4a 100644 --- a/src/deck/drivers/src/lpsTwrTag.c +++ b/src/deck/drivers/src/lpsTwrTag.c @@ -170,7 +170,7 @@ static uint32_t rxcallback(dwDevice_t *dev) { rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor]; - if (options->anchorPositionOk && (diff < (OUTLIER_TH*stddev))) { + if (options->combinedAnchorPositionOk && (diff < (OUTLIER_TH*stddev))) { distanceMeasurement_t dist; dist.distance = options->distance[current_anchor]; dist.x = options->anchorPosition[current_anchor].x; diff --git a/src/deck/drivers/src/lpsTwrTdmaTag.c b/src/deck/drivers/src/lpsTwrTdmaTag.c index 1c8a8bdb6a..85d78f2980 100644 --- a/src/deck/drivers/src/lpsTwrTdmaTag.c +++ b/src/deck/drivers/src/lpsTwrTdmaTag.c @@ -221,7 +221,7 @@ static uint32_t rxcallback(dwDevice_t *dev) { rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor]; - if (options->anchorPositionOk && (diff < (OUTLIER_TH*stddev))) { + if (options->combinedAnchorPositionOk && (diff < (OUTLIER_TH*stddev))) { distanceMeasurement_t dist; dist.distance = options->distance[current_anchor]; dist.x = options->anchorPosition[current_anchor].x; diff --git a/src/modules/src/mem.c b/src/modules/src/mem.c index 3523f77032..d3b3fc56a4 100644 --- a/src/modules/src/mem.c +++ b/src/modules/src/mem.c @@ -42,6 +42,7 @@ #include "eeprom.h" #ifdef PLATFORM_CF2 #include "ledring12.h" +#include "locodeck.h" #endif @@ -72,30 +73,36 @@ // of one wire ids that depends on the decks that are attached #define EEPROM_ID 0x00 #define LEDMEM_ID 0x01 +#define LOCO_ID 0x02 #ifdef PLATFORM_CF1 #define OW_FIRST_ID 0x00 uint8_t ledringmem[1]; #else - #define OW_FIRST_ID 0x02 + #define OW_FIRST_ID 0x03 #endif +#define STATUS_OK 0 #define MEM_TYPE_EEPROM 0x00 #define MEM_TYPE_OW 0x01 #define MEM_TYPE_LED12 0x10 +#define MEM_TYPE_LOCO 0x11 +#define MEM_LOCO_INFO 0x0000 +#define MEM_LOCO_ANCHOR_BASE 0x1000 +#define MEM_LOCO_ANCHOR_PAGE_SIZE 0x0100 +#define MEM_LOCO_PAGE_LEN (3 * sizeof(float) + 1) //Private functions static void memTask(void * prm); static void memSettingsProcess(int command); static void memWriteProcess(void); static void memReadProcess(void); +static uint8_t handleLocoMemRead(uint32_t memAddr, uint8_t readLen, uint8_t* dest); static void createNbrResponse(CRTPPacket* p); static void createInfoResponse(CRTPPacket* p, uint8_t memId); -static void createEepromInfoResponse(CRTPPacket* p); -static void createLedInfoResponse(CRTPPacket* p); -static void createOwInfoResponse(CRTPPacket* p); +static void createInfoResponseBody(CRTPPacket* p, uint8_t type, uint32_t memSize, const uint8_t data[8]); static bool isInit = false; @@ -105,7 +112,7 @@ static const OwSerialNum eepromSerialNum = { .data = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, EEPROM_I2C_ADDR} }; -static uint32_t memSize; +static const uint8_t noData[8] = {0, 0, 0, 0, 0, 0, 0, 0}; static CRTPPacket p; void memInit(void) @@ -191,62 +198,42 @@ void createInfoResponse(CRTPPacket* p, uint8_t memId) switch(memId) { case EEPROM_ID: - createEepromInfoResponse(p); + createInfoResponseBody(p, MEM_TYPE_EEPROM, EEPROM_SIZE, eepromSerialNum.data); break; case LEDMEM_ID: - createLedInfoResponse(p); + createInfoResponseBody(p, MEM_TYPE_LED12, sizeof(ledringmem), noData); + break; + case LOCO_ID: + createInfoResponseBody(p, MEM_TYPE_LOCO, MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * LOCODECK_NR_OF_ANCHORS, noData); break; default: if (owGetinfo(memId - OW_FIRST_ID, &serialNbr)) { - createOwInfoResponse(p); + createInfoResponseBody(p, MEM_TYPE_OW, OW_MAX_SIZE, serialNbr.data); } break; } } -void createEepromInfoResponse(CRTPPacket* p) +void createInfoResponseBody(CRTPPacket* p, uint8_t type, uint32_t memSize, const uint8_t data[8]) { - p->data[2] = MEM_TYPE_EEPROM; + p->data[2] = type; p->size += 1; - // Size of the memory - memSize = EEPROM_SIZE; - memcpy(&p->data[3], &memSize, 4); - p->size += 4; - memcpy(&p->data[7], eepromSerialNum.data, 8); - p->size += 8; -} -void createLedInfoResponse(CRTPPacket* p) -{ - p->data[2] = MEM_TYPE_LED12; - p->size += 1; - // Size of the memory - memSize = sizeof(ledringmem); memcpy(&p->data[3], &memSize, 4); p->size += 4; - memcpy(&p->data[7], eepromSerialNum.data, 8); //TODO - p->size += 8; -} -void createOwInfoResponse(CRTPPacket* p) -{ - p->data[2] = MEM_TYPE_OW; - p->size += 1; - // Size of the memory TODO: Define length type - memSize = OW_MAX_SIZE; - memcpy(&p->data[3], &memSize, 4); - p->size += 4; - memcpy(&p->data[7], serialNbr.data, 8); + memcpy(&p->data[7], data, 8); p->size += 8; } + void memReadProcess() { uint8_t memId = p.data[0]; uint8_t readLen = p.data[5]; uint32_t memAddr; - uint8_t status = 0; + uint8_t status = STATUS_OK; memcpy(&memAddr, &p.data[1], 4); @@ -260,7 +247,7 @@ void memReadProcess() { if (memAddr + readLen <= EEPROM_SIZE && eepromReadBuffer(&p.data[6], memAddr, readLen)) - status = 0; + status = STATUS_OK; else status = EIO; } @@ -270,18 +257,22 @@ void memReadProcess() { if (memAddr + readLen <= sizeof(ledringmem) && memcpy(&p.data[6], &(ledringmem[memAddr]), readLen)) - status = 0; + status = STATUS_OK; else status = EIO; } break; + case LOCO_ID: + status = handleLocoMemRead(memAddr, readLen, &p.data[6]); + break; + default: { memId = memId - OW_FIRST_ID; if (memAddr + readLen <= OW_MAX_SIZE && owRead(memId, memAddr, readLen, &p.data[6])) - status = 0; + status = STATUS_OK; else status = EIO; } @@ -299,7 +290,7 @@ void memReadProcess() #endif p.data[5] = status; - if (status == 0) + if (status == STATUS_OK) p.size = 6 + readLen; else p.size = 6; @@ -308,12 +299,51 @@ void memReadProcess() crtpSendPacket(&p); } +uint8_t handleLocoMemRead(uint32_t memAddr, uint8_t readLen, uint8_t* dest) { + uint8_t status = EIO; + + if (MEM_LOCO_INFO == memAddr) + { + if (1 == readLen) + { + *dest = LOCODECK_NR_OF_ANCHORS; + status = STATUS_OK; + } + } + else + { + if (memAddr >= MEM_LOCO_ANCHOR_BASE) + { + uint32_t pageAddress = memAddr - MEM_LOCO_ANCHOR_BASE; + if ((pageAddress % MEM_LOCO_ANCHOR_PAGE_SIZE) == 0) + { + uint32_t page = pageAddress / MEM_LOCO_ANCHOR_PAGE_SIZE; + if (page < LOCODECK_NR_OF_ANCHORS && MEM_LOCO_PAGE_LEN == readLen) + { + point_t* position = locodeckGetAnchorPosition(page); + float* destAsFloat = (float*)dest; + destAsFloat[0] = position->x; + destAsFloat[1] = position->y; + destAsFloat[2] = position->z; + + bool hasBeenSet = (position->timestamp != 0); + dest[sizeof(float) * 3] = hasBeenSet; + + status = STATUS_OK; + } + } + } + } + + return status; +} + void memWriteProcess() { uint8_t memId = p.data[0]; uint8_t writeLen; uint32_t memAddr; - uint8_t status = 0; + uint8_t status = STATUS_OK; memcpy(&memAddr, &p.data[1], 4); writeLen = p.size - 5; @@ -328,7 +358,7 @@ void memWriteProcess() { if (memAddr + writeLen <= EEPROM_SIZE && eepromWriteBuffer(&p.data[5], memAddr, writeLen)) - status = 0; + status = STATUS_OK; else status = EIO; } @@ -348,12 +378,17 @@ void memWriteProcess() } break; + case LOCO_ID: + // Not supported + status = EIO; + break; + default: { memId = memId - OW_FIRST_ID; if (memAddr + writeLen <= OW_MAX_SIZE && owWrite(memId, memAddr, writeLen, &p.data[5])) - status = 0; + status = STATUS_OK; else status = EIO; } diff --git a/test/deck/drivers/src/TestLpsTwrTag.c b/test/deck/drivers/src/TestLpsTwrTag.c index b23007b7f1..3164f69895 100644 --- a/test/deck/drivers/src/TestLpsTwrTag.c +++ b/test/deck/drivers/src/TestLpsTwrTag.c @@ -47,7 +47,7 @@ static lpsAlgoOptions_t defaultOptions = { }, .antennaDelay = 30000, .rangingFailedThreshold = 6, - .anchorPositionOk = false + .combinedAnchorPositionOk = false };