Skip to content

Commit

Permalink
#382 Re-worked the lighthouse sensor processing to support multiple s…
Browse files Browse the repository at this point in the history 
…ensors
  • Loading branch information
@krichardsson
krichardsson committed Dec 5, 2018
1 parent 2c63770 commit d4e45bc
Show file tree
Hide file tree
Showing 4 changed files with 221 additions and 79 deletions.
45 changes: 24 additions & 21 deletions src/deck/drivers/src/lighthouse.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
#include "estimator_kalman.h"


static float angles[2][2];
static pulseProcessorResult_t angles[PULSE_PROCESSOR_N_SENSORS];

typedef union frame_u {
struct {
Expand Down Expand Up @@ -88,7 +88,6 @@ static void lighthouseTask(void *param)
static frame_t frame;
static pulseProcessor_t ppState = {};

float angle;
int basestation;
int axis;

Expand Down Expand Up @@ -123,23 +122,27 @@ static void lighthouseTask(void *param)
continue;
}

if (frame.sensor == 0) {
// DEBUG_PRINT("Reading %08X:%04X\n", frame.timestamp, frame.width);
if (pulseProcessorProcessPulse(&ppState, frame.timestamp, frame.width, &angle, &basestation, &axis)) {
angles[basestation][axis] = angle;

if (basestation == 1 && axis == 1) {
lighthouseGeometryGetPosition(baseStationsGeometry, (void*)angles, position, &delta);

ext_pos.x = position[0];
ext_pos.y = -position[2];
ext_pos.z = position[1];
ext_pos.stdDev = 0.01;
estimatorKalmanEnqueuePosition(&ext_pos);
if (pulseProcessorProcessPulse(&ppState, frame.sensor, frame.timestamp, frame.width, angles, &basestation, &axis)) {
if (basestation == 1 && axis == 1) {
for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
// Only use sensor 0 for now
if (sensor == 0) {
if (angles[sensor].validCount == 4) {
lighthouseGeometryGetPosition(baseStationsGeometry, (void*)angles[sensor].angles, position, &delta);

ext_pos.x = position[0];
ext_pos.y = -position[2];
ext_pos.z = position[1];
ext_pos.stdDev = 0.01;
estimatorKalmanEnqueuePosition(&ext_pos);

// DEBUG_PRINT("%i %f %f %f\n", sensor, (double)ext_pos.x, (double)ext_pos.y, (double)ext_pos.z);
}
}

angles[sensor].validCount = 0;
}
}
} else {
DEBUG_PRINT("Receiving from wrong sensor?!?!?!\n");
}

synchronized = getFrame(&frame);
Expand Down Expand Up @@ -174,10 +177,10 @@ static const DeckDriver lighthouse_deck = {
DECK_DRIVER(lighthouse_deck);

LOG_GROUP_START(lighthouse)
LOG_ADD(LOG_FLOAT, angle0x, &angles[0][0])
LOG_ADD(LOG_FLOAT, angle0y, &angles[1][0])
LOG_ADD(LOG_FLOAT, angle1x, &angles[0][1])
LOG_ADD(LOG_FLOAT, angle1y, &angles[1][1])
LOG_ADD(LOG_FLOAT, angle0x, &angles[0].angles[0][0])
LOG_ADD(LOG_FLOAT, angle0y, &angles[0].angles[1][0])
LOG_ADD(LOG_FLOAT, angle1x, &angles[0].angles[0][1])
LOG_ADD(LOG_FLOAT, angle1y, &angles[0].angles[1][1])
LOG_ADD(LOG_FLOAT, x, &position[0])
LOG_ADD(LOG_FLOAT, y, &position[1])
LOG_ADD(LOG_FLOAT, z, &position[2])
Expand Down
30 changes: 23 additions & 7 deletions src/utils/interface/lighthouse/pulse_processor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <stdint.h>
#include <stdlib.h>

#define PULSE_PROCESSOR_N_SENSORS 8
#define PULSE_PROCESSOR_N_SENSORS 4
#define PULSE_PROCESSOR_HISTORY_LENGTH 8
#define TIMESTAMP_BITWIDTH 29
#define TIMESTAMP_MAX ((1<<TIMESTAMP_BITWIDTH)-1)
Expand All @@ -17,6 +17,17 @@ typedef struct {
int width;
} pulseProcessorPulse_t;

typedef enum {
sweepDirection_j = 0,
sweepDirection_k = 1
} SweepDirection;

typedef enum {
sweepStorageStateWaiting = 0,
sweepStorageStateValid,
sweepStorageStateError,
} SweepStorageState_t;

typedef struct pulseProcessor_s {
bool synchronized; // At true if we are currently syncthonized (ie. we have seen one short sweep)

Expand All @@ -31,23 +42,28 @@ typedef struct pulseProcessor_s {
int nSyncPulses; // Number of sync pulses accumulated

// Sync pulse timestamps
uint32_t currentSync; // Sync currently used for sweep phase measurment
uint32_t currentSync; // Sync currently used for sweep phase measurement
uint32_t currentSync0; // Sync0 of the current frame
uint32_t currentSync1; // Sync1 of the current frame

// Base station and axis of the current frame
int currentBs;
int currentAxis;
int currentBaseStation;
SweepDirection currentAxis;

// Sweep timestamps
struct {
uint32_t timestamp;
bool valid;
bool error;
SweepStorageState_t state;
} sweeps[PULSE_PROCESSOR_N_SENSORS];
bool sweepDataStored;

} pulseProcessor_t;

typedef struct {
float angles[2][2];
int validCount;
} pulseProcessorResult_t;

// If returns true, the angle, base station and direction are written
bool pulseProcessorProcessPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis);
bool pulseProcessorProcessPulse(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis);

175 changes: 125 additions & 50 deletions src/utils/src/lighthouse/pulse_processor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,27 +17,25 @@
#define SYNC_DIVIDER 500
#define SYNC_MAX_SEPARATION 25000 // More than 400us (400us is 19200)
#define SYNC_SEPARATION 19200
#define SENSOR_MAX_DISPERTION 10
#define SENSOR_MAX_DISPERTION 20
#define MAX_FRAME_LENGTH_NOISE 400

// Utility functions and macros
#define TS_DIFF(X, Y) ((X-Y)&((1<<TIMESTAMP_BITWIDTH)-1))
// #define TS_DIFF(X, Y) ((X-Y)&((1<<TIMESTAMP_BITWIDTH)-1))
static uint32_t TS_DIFF(uint32_t x, uint32_t y) {
const uint32_t bitmask = (1 << TIMESTAMP_BITWIDTH) - 1;
return (x - y) & bitmask;
}



TESTABLE_STATIC bool findSyncTime(const pulseProcessorPulse_t pulseHistory[], uint32_t *foundSyncTime);
TESTABLE_STATIC bool getSystemSyncTime(const uint32_t syncTimes[], size_t nSyncTimes, uint32_t *syncTime);



static void resetPulseHistory(pulseProcessor_t *state) {
memset(state->pulseHistoryPtr, 0, sizeof(state->pulseHistoryPtr));
}

static void resetSynchronization(pulseProcessor_t *state)
{
state->synchronized = false;
}

static void synchronize(pulseProcessor_t *state, int sensor, uint32_t timestamp, uint32_t width)
{
state->pulseHistory[sensor][state->pulseHistoryPtr[sensor]].timestamp = timestamp;
Expand Down Expand Up @@ -68,13 +66,13 @@ static void synchronize(pulseProcessor_t *state, int sensor, uint32_t timestamp,

static bool isSweep(pulseProcessor_t *state, unsigned int timestamp, int width)
{
int delta = TS_DIFF(timestamp, state->lastSync);
uint32_t delta = TS_DIFF(timestamp, state->lastSync);
return ((delta > SYNC_MAX_SEPARATION) && (delta < (FRAME_LENGTH - (2*SYNC_MAX_SEPARATION)))) || (width < SWEEP_MAX_WIDTH);
}

TESTABLE_STATIC bool isSync(pulseProcessor_t *state, unsigned int timestamp, int width)
{
int delta = TS_DIFF(timestamp, state->currentSync0);
uint32_t delta = TS_DIFF(timestamp, state->currentSync0);
int deltaModulo = delta % FRAME_LENGTH;

// We expect a modulo close to 0, detect and handle wrapping around FRAME_LENGTH
Expand All @@ -88,70 +86,147 @@ TESTABLE_STATIC bool isSync(pulseProcessor_t *state, unsigned int timestamp, int
return false;
}

static bool getAxis(int width)
{
return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0;
static int getBaseStationId(pulseProcessor_t *state, unsigned int timestamp) {
int baseStation = 1;

uint32_t delta = TS_DIFF(timestamp, state->currentSync0);
if (delta > SYNC_MAX_SEPARATION) {
baseStation = 0;
}

return baseStation;
}

static SweepDirection getAxis(int width) {
SweepDirection result = sweepDirection_j;

if ((((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0) {
result = sweepDirection_k;
}

return result;
}

static bool isSweepActiveThisFrame(int width) {
return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x04) == 0;
}

static void storeSweepData(pulseProcessor_t *state, int sensor, unsigned int timestamp) {
if (state->sweeps[sensor].state == sweepStorageStateWaiting) {
state->sweeps[sensor].timestamp = timestamp;
state->sweeps[sensor].state = sweepStorageStateValid;
} else {
state->sweeps[sensor].state = sweepStorageStateError;
}

state->sweepDataStored = true;
}

static void resetSweepData(pulseProcessor_t *state) {
for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
state->sweeps[sensor].state = sweepStorageStateWaiting;
}
state->sweepDataStored = false;
}

static bool getSkip(int width)
static void resetSynchronization(pulseProcessor_t *state)
{
return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x04) != 0;
resetSweepData(state);
state->synchronized = false;
}

static bool processPreviousFrame(pulseProcessor_t *state, pulseProcessorResult_t result[], int *baseStation, int *axis) {
bool anglesMeasured = false;

static bool processWhenSynchronized(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis) {
bool angleMeasured = false;
if (state->sweepDataStored) {
for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
if (state->sweeps[sensor].state == sweepStorageStateValid) {
int delta = TS_DIFF(state->sweeps[sensor].timestamp, state->currentSync);
if (delta < FRAME_LENGTH) {
float angle = (delta - SWEEP_CENTER)*(float)M_PI/FRAME_LENGTH;

if (isSweep(state, timestamp, width)) {
int delta = TS_DIFF(timestamp, state->currentSync);
*baseStation = state->currentBaseStation;
*axis = state->currentAxis;

if (delta < FRAME_LENGTH) {
*angle = (delta - SWEEP_CENTER)*(float)M_PI/FRAME_LENGTH;
*baseStation = state->currentBs;
*axis = state->currentAxis;
angleMeasured = true;
}
result[sensor].angles[state->currentBaseStation][state->currentAxis] = angle;
result[sensor].validCount++;

state->currentSync = 0;
} else if (isSync(state, timestamp, width)) {
if (TS_DIFF(timestamp, state->lastSync) > SYNC_MAX_SEPARATION) {
// This is sync0
if (!getSkip(width)) {
state->currentBs = 0;
state->currentAxis = getAxis(width);
state->currentSync = timestamp;
state->currentSync0 = timestamp;
anglesMeasured = true;
}
}
}

resetSweepData(state);
}

return anglesMeasured;
}

static void storeSyncData(pulseProcessor_t *state, unsigned int timestamp, unsigned int width) {
int baseStation = getBaseStationId(state, timestamp);
if (0 == baseStation) {
state->currentSync0 = timestamp;
} else {
state->currentSync1 = timestamp;
}

state->lastSync = timestamp;

if (isSweepActiveThisFrame(width)) {
state->currentBaseStation = baseStation;
state->currentAxis = getAxis(width);
state->currentSync = timestamp;
}
}

TESTABLE_STATIC bool isNewSync(uint32_t timestamp, uint32_t lastSync) {
const uint32_t min = (1 << TIMESTAMP_BITWIDTH) - SENSOR_MAX_DISPERTION;
const uint32_t max = SENSOR_MAX_DISPERTION;

uint32_t diff = TS_DIFF(timestamp, lastSync);
return (diff > max) && (diff < min);
}

static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis) {
bool anglesMeasured = false;

if (isNewSync(timestamp, state->lastSync)) {
if (isSync(state, timestamp, width)) {
anglesMeasured = processPreviousFrame(state, angles, baseStation, axis);
storeSyncData(state, timestamp, width);
} else {
// this is sync1
if (!getSkip(width)) {
state->currentBs = 1;
state->currentAxis = getAxis(width);
state->currentSync = timestamp;
}
// Expected a sync but something is wrong, re-synchronize.
resetSynchronization(state);
}
}

return anglesMeasured;
}

static bool processWhenSynchronized(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis) {
bool anglesMeasured = false;

state->lastSync = timestamp;
if (isSweep(state, timestamp, width)) {
storeSweepData(state, sensor, timestamp);
} else {
// If the pulse is not sync nor sweep: we are not syncronized anymore!
resetSynchronization(state);
anglesMeasured = processSync(state, timestamp, width, angles, baseStation, axis);
}

return angleMeasured;
return anglesMeasured;
}


bool pulseProcessorProcessPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis)
bool pulseProcessorProcessPulse(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis)
{
bool angleMeasured = false;
bool anglesMeasured = false;

if (!state->synchronized) {
synchronize(state, 0, timestamp, width);
synchronize(state, sensor, timestamp, width);
} else {
angleMeasured = processWhenSynchronized(state, timestamp, width, angle, baseStation, axis);
anglesMeasured = processWhenSynchronized(state, sensor, timestamp, width, angles, baseStation, axis);
}

return angleMeasured;
return anglesMeasured;
}


Expand Down
Loading

0 comments on commit d4e45bc

Please sign in to comment.