Skip to content

Commit

Permalink
Added Loco Anchor Position memory subsystem. Closes #198
Browse files Browse the repository at this point in the history 
Anchor positions can now be transferred to a client using the memory sub system. The old Param way of setting anchor positions still in place for backwards compatibility.
  • Loading branch information
@krichardsson
krichardsson committed Feb 15, 2017
1 parent 65dd816 commit 87b60b2
Show file tree
Hide file tree
Showing 6 changed files with 102 additions and 59 deletions.
4 changes: 3 additions & 1 deletion src/deck/drivers/interface/locodeck.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,14 +61,16 @@ 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];
int failedRanging[LOCODECK_NR_OF_ANCHORS];
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);
Expand Down
30 changes: 18 additions & 12 deletions src/deck/drivers/src/locodeck.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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
Expand Down Expand Up @@ -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)
2 changes: 1 addition & 1 deletion src/deck/drivers/src/lpsTwrTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/deck/drivers/src/lpsTwrTdmaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
121 changes: 78 additions & 43 deletions src/modules/src/mem.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "eeprom.h"
#ifdef PLATFORM_CF2
#include "ledring12.h"
#include "locodeck.h"
#endif


Expand Down Expand Up @@ -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;

Expand All @@ -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)
Expand Down Expand Up @@ -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);

Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion test/deck/drivers/src/TestLpsTwrTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ static lpsAlgoOptions_t defaultOptions = {
},
.antennaDelay = 30000,
.rangingFailedThreshold = 6,
.anchorPositionOk = false
.combinedAnchorPositionOk = false
};


Expand Down

0 comments on commit 87b60b2

Please sign in to comment.