view main/robots/odr-display/status.cpp @ 235:8af48f7c2f34 main

Initial commit for the odr-display node.
author Bob Cook <bob@bobcookdev.com>
date Sat, 12 Jul 2014 13:20:14 -0700
parents
children 43f43ebe35df
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2011-2014 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Status display functions for the Outdoor Robot user interface board.
//
//  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 <avr/io.h>

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

#include "leds.h"

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

static volatile uint8_t g_beats_without_mgr;
static volatile uint8_t g_beats_without_motion_ctl;
static volatile uint8_t g_beats_without_sonarfront;
static volatile uint8_t g_beats_without_gps;

static volatile uint8_t g_status_flags;

static const uint8_t status_flag_emergency_active    = ( 1 << 0 );
static const uint8_t status_flag_sonar_front_enabled = ( 1 << 1 );
static const uint8_t status_flag_gps_has_fix         = ( 1 << 2 );

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

void status_init()
{
    // initalize the heartbeat watchdogs assuming no contact yet
    
    g_beats_without_mgr = 100;
    g_beats_without_sonarfront = 100;
    g_beats_without_motion_ctl = 100;
    g_beats_without_gps = 100;
}

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

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

        case can_node_odr_motion:
            g_beats_without_motion_ctl = 0;
            break;

        case can_node_odr_sonar_front:
            break;

        case can_node_sensor_gps:
            g_beats_without_gps = 0;
            break;
    }
}

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

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;

    switch ( source )
    {
        case can_node_odr_manager:
            break;

        case can_node_odr_motion:
            break;

        case can_node_odr_sonar_front:
            break;
    }
}

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

void status_got_all_clear( uint8_t source )
{
    switch ( source )
    {
        case can_node_odr_manager:
            g_status_flags &= ~status_flag_emergency_active;
            break;

        case can_node_odr_motion:
            break;

        case can_node_odr_sonar_front:
            break;
    }
}

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

void status_update()
{
    //--    Increment all of the beat counters.

    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
    if ( g_beats_without_sonarfront < 200 ) { ++g_beats_without_sonarfront; }
    if ( g_beats_without_motion_ctl < 200 ) { ++g_beats_without_motion_ctl; }
    if ( g_beats_without_gps < 200 ) { ++g_beats_without_gps; }

#if 0
    //--    Has the ESTOP condition transitioned? Note we ignore this check if the
    //      override switch has been set.

    if ( g_status_flags & status_flag_estop_triggered )
    {
        if ( ! did_estop_timeout() )
        {
            // switching from ESTOP triggered to not triggered
            g_status_flags &= ~status_flag_estop_triggered;
            canmsg_send_all_clear();
        }
        else
        {
            // keep sending the emergency until the ESTOP is not triggered
            canmsg_send_emergency();
        }
    }
    else
    {
        if ( did_estop_timeout() )
        {
            // switching from ESTOP not triggered to triggered
            g_status_flags |= status_flag_estop_triggered;
            canmsg_send_emergency();
        }
        else if ( status_is_emergency() )
        {
            // keep sending the all-clear until we are out of the emergency state
            canmsg_send_all_clear();
        }
    }
#endif
}

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

void status_got_mgr_update()
{
    g_beats_without_mgr = 0;
}

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

void status_got_sonar_front_enabled()
{
    g_beats_without_sonarfront = 0;
    g_status_flags |= status_flag_sonar_front_enabled;
}

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

void status_got_sonar_front_disabled()
{
    g_beats_without_sonarfront = 0;
    g_status_flags &= ~status_flag_sonar_front_enabled;
}

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

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

    return ( g_status_flags & status_flag_emergency_active ) > 0;
}

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

bool status_is_mgr_up()
{
    // more than five seconds == down
    return ( g_beats_without_mgr < 10 );
}

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

bool status_is_motion_ctl_alive()
{
    // more than four seconds == down
    return ( g_beats_without_motion_ctl < 8 );
}

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

bool status_is_sonar_front_up()
{
    // more than four seconds == down
    return ( g_beats_without_sonarfront < 8 );
}

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

bool status_is_sonar_front_enabled()
{
    return ( g_status_flags & status_flag_sonar_front_enabled ) > 0;
}

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

bool status_is_gps_up()
{
    // more than four seconds == down
    return ( g_beats_without_gps < 8 );
}

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

bool status_does_gps_have_fix()
{
    // more than four seconds == down
    return ( g_status_flags & status_flag_gps_has_fix ) > 0;
}

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

void status_got_gps_fix( bool has_fix )
{
    if ( has_fix )
    {
        g_status_flags |= status_flag_gps_has_fix;
    }
    else
    {
        g_status_flags &= ~status_flag_gps_has_fix;
    }
}

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