view main/robots/odr-imu/status.cpp @ 248:1fd327ac6ad8 main

Checkpoint for the IMU node firmware.
author Bob Cook <bob@bobcookdev.com>
date Mon, 24 Aug 2015 20:51:16 -0700
parents d1e26bffa503
children 87864f3a89e4
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2015 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Status functions for the Outdoor Robot IMU node.
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
//  There is an implicit assumption our "heartbeat" is 2 Hz (twice per second).
//
// ----------------------------------------------------------------------------------------

#include "func.h"
#include "leds.h"

#include <avr/io.h>

#include "packages/common/can/can_nodes.h"

// ----------------------------------------------------------------------------------------
//  "beats" is 500ms (approximately)

static volatile uint8_t g_beats_without_mgr;
static volatile uint8_t g_beats_without_imu;

static volatile uint8_t g_status_flags;

static const uint8_t status_flag_emergency_active = ( 1 << 0 );

// ----------------------------------------------------------------------------------------

void status_init()
{
    // initalize the heartbeat watchdog assuming no contact yet
    g_beats_without_mgr = 100;
    g_beats_without_imu = 100;
}

// ----------------------------------------------------------------------------------------

void status_update()
{
    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
    if ( g_beats_without_imu < 200 ) { ++g_beats_without_imu; }
}

// ----------------------------------------------------------------------------------------

void status_got_heartbeat( uint8_t source )
{
    switch ( source )
    {
        case can_node_odr_manager:
            g_beats_without_mgr = 0;
            break;

//        case can_node_odr_sonar_front:
//            break;
    }
}

// ----------------------------------------------------------------------------------------

void status_got_imu_packet()
{
    g_beats_without_imu = 0;
}

// ----------------------------------------------------------------------------------------

void status_set_emergency()
{
    g_status_flags |= status_flag_emergency_active;
}

// ----------------------------------------------------------------------------------------

void status_got_emergency( uint8_t source )
{
    g_status_flags |= status_flag_emergency_active;
}

// ----------------------------------------------------------------------------------------

void status_got_all_clear( uint8_t source )
{
    if ( source == can_node_odr_manager )
    {
        g_status_flags &= ~status_flag_emergency_active;
    }
}

// ----------------------------------------------------------------------------------------

bool status_is_emergency()
{
    // more than two seconds == down
    if ( g_beats_without_mgr > 5 )
    {
        return true;
    }

    // more than two seconds == down
    if ( g_beats_without_imu > 5 )
    {
        return true;
    }

    return ( g_status_flags & status_flag_emergency_active ) > 0;
}

// ----------------------------------------------------------------------------------------

bool status_is_imu_active()
{
    // more than two seconds == down
    return ( g_beats_without_imu < 5 );
}

// ----------------------------------------------------------------------------------------