view main/robots/odr-imu/imu.cpp @ 251:87864f3a89e4 main

IMU module code.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Sep 2015 14:49:45 -0700
parents 1fd327ac6ad8
children
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 );

static float g_yaw_adjustment = 0.0;

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

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

    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 );
}

// ----------------------------------------------------------------------------------------
// 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;
}

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

bool imu_process_pending()
{
    if ( ! recv_imu_packet() )
    {
        return false;
    }

    if ( um7_packet_address() == um7_addr_dreg_euler_phi_theta )
    {
        return true;
    }
    else if ( um7_packet_address() == um7_addr_dreg_health )
    {
        return true;
    }

    return false; // message, but not one we care about
}

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

bool imu_did_receive_imu_angles()
{
    return ( um7_packet_address() == um7_addr_dreg_euler_phi_theta );
}

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

bool imu_did_receive_imu_health()
{
    return ( um7_packet_address() == um7_addr_dreg_health );
}

// ----------------------------------------------------------------------------------------
//  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 imu_angle_roll()
{
    float roll = um7_packet_value_s16( 2 );
    roll /= 91.02222;
    roll *= -1.0;

    return roll;
}

float imu_angle_pitch()
{
    float pitch = um7_packet_value_s16( 0 );
    pitch /= 91.02222;
    pitch *= -1.0;
    
    return pitch;
}

float imu_angle_yaw()
{
    float yaw = um7_packet_value_s16( 4 );
    yaw /= 91.02222;
    yaw += g_yaw_adjustment; // magnetic declination correction
    if ( yaw > 360.0 ) yaw -= 360.0;
    if ( yaw < 0.0 ) yaw += 360.0;
    yaw -= 180.0;

    return yaw;
}

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

uint32_t imu_health_info()
{
    return um7_packet_value_u32( 0 );
}

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