Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add LED-ring effect to show status of localization service. #477

Merged
merged 1 commit into from
Sep 27, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 48 additions & 0 deletions src/deck/drivers/src/ledring12.c
Original file line number Diff line number Diff line change
Expand Up @@ -707,6 +707,53 @@ static void rssiEffect(uint8_t buffer[][3], bool reset)
}
}

/**
* An effect that shows the status of the location service.
*
* Red means bad, green means good.
* Blinking means battery was low during flight.
*/
static void locSrvStatus(uint8_t buffer[][3], bool reset)
{
static int locSrvTickId = -1;
static int pmStateId = -1;

static int tic = 0;
static bool batteryEverLow = false;

// lazy initialization of the logging variables
if (locSrvTickId == -1) {
locSrvTickId = logGetVarId("locSrvZ", "tick");
pmStateId = logGetVarId("pm", "state");
}

// compute time since the last update in milliseconds
uint16_t time_since_last_update = xTaskGetTickCount() - logGetUint(locSrvTickId);
if (time_since_last_update > 30) {
time_since_last_update = 30;
}

int8_t pmstate = logGetInt(pmStateId);
if (pmstate == lowPower) {
batteryEverLow = true;
}

for (int i = 0; i < NBR_LEDS; i++) {
if (batteryEverLow && tic < 10) {
buffer[i][0] = 0;
buffer[i][1] = 0;
} else {
buffer[i][0] = LIMIT(LINSCALE(0, 30, 0, 100, time_since_last_update)); // Red (large time_since_last_update)
buffer[i][1] = LIMIT(LINSCALE(0, 30, 100, 0, time_since_last_update)); // Green (small time_since_last_update)
}
buffer[i][2] = 0;
}

if (++tic >= 20) {
tic = 0;
}
}

/**************** Effect list ***************/


Expand All @@ -728,6 +775,7 @@ Ledring12Effect effectsFct[] =
virtualMemEffect,
fadeColorEffect,
rssiEffect,
locSrvStatus,
};

/********** Ring init and switching **********/
Expand Down
10 changes: 9 additions & 1 deletion src/modules/src/crtp_localization_service.c
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ static float extPosStdDev = 0.01;
static float extQuatStdDev = 4.5e-3;
static bool isInit = false;
static uint8_t my_id;
static uint16_t tickOfLastPacket; // tick when last packet was received

static void locSrvCrtpCB(CRTPPacket* pk);
static void extPositionHandler(CRTPPacket* pk);
Expand Down Expand Up @@ -138,6 +139,7 @@ static void extPositionHandler(CRTPPacket* pk)
ext_pos.z = data->z;
ext_pos.stdDev = extPosStdDev;
estimatorEnqueuePosition(&ext_pos);
tickOfLastPacket = xTaskGetTickCount();
}

static void genericLocHandle(CRTPPacket* pk)
Expand Down Expand Up @@ -169,6 +171,7 @@ static void genericLocHandle(CRTPPacket* pk)
ext_pose.stdDevPos = extPosStdDev;
ext_pose.stdDevQuat = extQuatStdDev;
estimatorEnqueuePose(&ext_pose);
tickOfLastPacket = xTaskGetTickCount();
} else if (type == EXT_POSE_PACKED) {
uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem);
for (uint8_t i = 0; i < numItems; ++i) {
Expand All @@ -181,6 +184,7 @@ static void genericLocHandle(CRTPPacket* pk)
ext_pose.stdDevPos = extPosStdDev;
ext_pose.stdDevQuat = extQuatStdDev;
estimatorEnqueuePose(&ext_pose);
tickOfLastPacket = xTaskGetTickCount();
break;
}
}
Expand All @@ -198,7 +202,7 @@ static void extPositionPackedHandler(CRTPPacket* pk)
ext_pos.z = item->z / 1000.0f;
ext_pos.stdDev = extPosStdDev;
estimatorEnqueuePosition(&ext_pos);

tickOfLastPacket = xTaskGetTickCount();
break;
}
}
Expand Down Expand Up @@ -246,6 +250,10 @@ LOG_GROUP_START(ext_pos)
LOG_ADD(LOG_FLOAT, Z, &ext_pos.z)
LOG_GROUP_STOP(ext_pos)

LOG_GROUP_START(locSrvZ)
LOG_ADD(LOG_UINT16, tick, &tickOfLastPacket) // time when data was received last (ms/ticks)
LOG_GROUP_STOP(locSrvZ)

PARAM_GROUP_START(locSrv)
PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat)
PARAM_ADD(PARAM_FLOAT, extPosStdDev, &extPosStdDev)
Expand Down