view main/robots/odr-imu/imu.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
children 87864f3a89e4
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2015 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Functions for the Outdoor Robot IMU node to communicate with the UM7 module.
//
//  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.
//
// ----------------------------------------------------------------------------------------

#include <avr/eeprom.h>

#include "project_defs.h"

#include "func.h"

#include "packages/avr/device/spinwait.h"
#include "packages/avr/device/uart.h"

#include "packages/common/chrobotics/um7.h"
#include "packages/common/chrobotics/um7_data.h"

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

static float* eeprom_yaw_adjustment_address = reinterpret_cast< float* >( 0x04 );

float g_yaw_adjustment = 0.0;

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

void imu_init()
{
    uart0_init();
    uart0_set_baudrate( 115200 );

    g_yaw_adjustment = eeprom_read_float( eeprom_yaw_adjustment_address );
}

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

void imu_zero_gyros()
{
    uint8_t um7_packet[ um7_min_packet_size ];
    uint8_t pkt_len = um7_create_packet( um7_packet, um7_cmd_zero_gyros, NULL, 0 );
    uart0_write( um7_packet, pkt_len );
}

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

void imu_set_yaw_adjustment( float adj )
{
    g_yaw_adjustment = adj;
    eeprom_update_float( eeprom_yaw_adjustment_address, adj );
}

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

static void recv_euler_phi_theta()
{
    //--    The sensor is physically 180 degrees from its true direction for yaw, and 
    //      the pitch and roll are reversed. Fix this in software.
    //
    //      For yaw we want to output the range of [-180,180] rather than [0,360]

    float pitch = um7_packet_value_s16( 0 ) / 91.02222;
    float roll  = um7_packet_value_s16( 2 ) / 91.02222;
    float yaw   = um7_packet_value_s16( 4 ) / 91.02222;

    roll  *= -1.0;
    pitch *= -1.0;

    yaw += g_yaw_adjustment; // magnetic declination correction
    if ( yaw > 360.0 ) yaw -= 360.0;
    if ( yaw < 0.0 ) yaw += 360.0;
    yaw -= 180.0;

    canmsg_send_imu_pitch( pitch );
    canmsg_send_imu_roll( roll );
    canmsg_send_imu_yaw( yaw );
}

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

static void recv_health()
{
    canmsg_send_imu_health( um7_packet_value_u32( 0 ) );
}

// ----------------------------------------------------------------------------------------
// return true if a full packet is found

static bool recv_imu_packet()
{
    for ( uint8_t i = 0; i < 16; ++i )
    {
        if ( ! uart0_is_char_available() )
        {
            return false;
        }

        if ( um7_parse( uart0_read() ) )
        {
            status_got_imu_packet();
            return true;
        }
    }

    return false;
}

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

void imu_process_pending()
{
    if ( ! recv_imu_packet() )
    {
        return;
    }

    if ( um7_packet_address() == um7_addr_dreg_euler_phi_theta )
    {
        recv_euler_phi_theta();
    }
    else if ( um7_packet_address() == um7_addr_dreg_health )
    {
        recv_health();
    }
}

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