status.c

Go to the documentation of this file.

/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#include "sensor_fusion.h"      // Sensor fusion structures and functions
#include "board.h"              
#include "driver_sensors.h"     // hardware-specific drivers
#include "status.h"             // Header for this .c file
 
// bit-field definitions
#define N   0x00                // No color
#define R   0x04                // Red LED
#define G   0x02                // Green LED
#define B   0x01                // Blue LED
 
// set RGB LED as a function of bit-field values
void ssSetLeds(int8_t RGB)
{
    if (RGB & R) {
        LED_RED_ON();
    }else{
        LED_RED_OFF();
    }
    if (RGB & G) {
        LED_GREEN_ON();
    }else {
        LED_GREEN_OFF();
    }
    if (RGB & B) {
        LED_BLUE_ON();
    }else {
        LED_BLUE_OFF();
    }
}
 
// Do an immediate status update
void ssSetStatusNow(StatusSubsystem *pStatus, fusion_status_t status)
{
    pStatus->status = status;
    pStatus->next = status;
 
    uint8_t blink = false;
    uint8_t RGB = N;
 
    // This is where we actually change the visual indicator
    switch (status)
    {
        case INITIALIZING:      // solid GREEN
            RGB = G;
            break;
        case NORMAL:            // blinking GREEN
            RGB = G;
            blink = true;
            break;
        case LOWPOWER:          // blinking YELLOW
            RGB = R|G;
            blink = true;
            break;
        case SOFT_FAULT:        // solid RED (usually momentary)
        case HARD_FAULT:        // solid RED
            RGB = R;
            break;
        default:                // default = off;
            RGB = N;
    }
 
    if ((!blink) | (status != pStatus->previous))
    {
        ssSetLeds(RGB);
        pStatus->toggle = true;
    }
    else
    {
        if (pStatus->toggle)
        {
            ssSetLeds(N);
        }
        else
        {
            ssSetLeds(RGB);
        }
 
        pStatus->toggle = !pStatus->toggle;
    }
    while (status == HARD_FAULT) ; // Never return on hard fault
    // while (status == SOFT_FAULT) ; // DEBUG ONLY Never return on soft fault
}
 
// Unit test for status sub-system
void ssTest(StatusSubsystem *pStatus)
{
    switch (pStatus->status)
    {
        case OFF:
            ssSetStatusNow(pStatus, INITIALIZING);
            break;
        case INITIALIZING:
            ssSetStatusNow(pStatus, LOWPOWER);
            break;
        case LOWPOWER:
            ssSetStatusNow(pStatus, NORMAL);
            break;
        case NORMAL:
            ssSetStatusNow(pStatus, RECEIVING_WIRED);
            break;
        case RECEIVING_WIRED:
            ssSetStatusNow(pStatus, RECEIVING_WIRELESS);
            break;
        case RECEIVING_WIRELESS:
            ssSetStatusNow(pStatus, SOFT_FAULT);
            break;
        case SOFT_FAULT:
            ssSetStatusNow(pStatus, HARD_FAULT);
            break;
        case HARD_FAULT:
            ssSetStatusNow(pStatus, OFF);
            break;
    }
}
 
// undefine these just in case some other library needs them
#undef N
#undef R
#undef G
#undef B
 
 
// queue up a status change (which will take place at the next updateStatus)
void ssQueueStatus(StatusSubsystem *pStatus, fusion_status_t status)
{
    pStatus->next = status;
}
 
// promote any previously queued status update
void ssUpdateStatus(StatusSubsystem *pStatus)
{
    pStatus->previous = pStatus->status;
    ssSetStatusNow(pStatus, pStatus->next);
}
 
// make an immediate update to the system status
void ssSetStatus(StatusSubsystem *pStatus, fusion_status_t status)
{
    pStatus->next = status;
    ssUpdateStatus(pStatus);
}
 
// return the system status
fusion_status_t ssGetStatus(StatusSubsystem *pStatus)
{
    return pStatus->status;
}
 
void initializeStatusSubsystem(StatusSubsystem *pStatus)
{
    pStatus->set = ssSetStatus;
    pStatus->get = ssGetStatus;
    pStatus->queue = ssQueueStatus;
    pStatus->update = ssUpdateStatus;
    pStatus->test = ssTest;
    pStatus->previous = OFF;
    pStatus->set(pStatus, OFF);
    pStatus->queue(pStatus, OFF);
    pStatus->toggle = false;
 
    /* set initial values */
    LED_RED_INIT(LOGIC_LED_OFF);
    LED_GREEN_INIT(LOGIC_LED_OFF);
    LED_BLUE_INIT(LOGIC_LED_OFF);
}