source: rtems/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c @ 9a85541

/*---------------------------------------------------------------------------------
        $Id$

        DS Motion Card/DS Motion Pak functionality
        
        Copyright (C) 2007
                Michael Noland (joat)
                Jason Rogers (dovoto)
                Dave Murphy (WinterMute)
                Keith Epstein (KeithE)

        This software is provided 'as-is', without any express or implied
        warranty.  In no event will the authors be held liable for any
        damages arising from the use of this software.

        Permission is granted to anyone to use this software for any
        purpose, including commercial applications, and to alter it and
        redistribute it freely, subject to the following restrictions:

        1.      The origin of this software must not be misrepresented; you
                must not claim that you wrote the original software. If you use
                this software in a product, an acknowledgment in the product
                documentation would be appreciated but is not required.
        2.      Altered source versions must be plainly marked as such, and
                must not be misrepresented as being the original software.
        3.      This notice may not be removed or altered from any source
                distribution.

---------------------------------------------------------------------------------*/

#include <nds/card.h>
#include <nds/system.h>
#include <nds/memory.h>
#include <nds/bios.h>
#include <nds/arm9/ndsmotion.h>

#define WAIT_CYCLES 185

#define CARD_WaitBusy()   while (CARD_CR1 & /*BUSY*/0x80);

// enables SPI bus at 4.19 MHz
#define SPI_On() CARD_CR1 = /*E*/0x8000 | /*SEL*/0x2000 | /*MODE*/0x40 | 0;

// disables SPI bus
#define SPI_Off() CARD_CR1 = 0;

// Volatile GBA bus SRAM for reading from DS Motion Pak
#define V_SRAM ((volatile unsigned char*)0x0A000000)


int card_type = -1;

//these are the default calibration values for sensitivity and offset
MotionCalibration calibration = {2048, 2048, 2048, 1680, 819, 819, 819, 825};

// sends and receives 1 byte on the SPI bus
unsigned char motion_spi(unsigned char in_byte){

        unsigned char out_byte;
        CARD_EEPDATA = in_byte; // send the output byte to the SPI bus
        CARD_WaitBusy(); // wait for transmission to complete
        out_byte=CARD_EEPDATA; // read the input byte from the SPI bus
        return out_byte;
}


void motion_MK6_sensor_mode(void) {
        // send some commands on the SPI bus
        SPI_On()
        motion_spi(0xFE);
        SPI_Off()
        SPI_On()
        motion_spi(0xFD);
        SPI_Off()
        SPI_On()
        motion_spi(0xFB);
        SPI_Off()
        SPI_On()
        motion_spi(0xF8);
        SPI_Off()
}

void motion_MK6_EEPROM_mode(void) {
        // send some commands on the SPI bus
        SPI_On()
        motion_spi(0xFE);
        SPI_Off()
        SPI_On()
        motion_spi(0xFD);
        SPI_Off()
        SPI_On()
        motion_spi(0xFB);
        SPI_Off()
        SPI_On()
        motion_spi(0xF9);
        SPI_Off()
}

// checks whether a DS Motion Pak is plugged in
int motion_pak_is_inserted(void){
    int motion_pak = 0;
        unsigned char return_byte = V_SRAM[10]; // read first byte of DS Motion Pak check
        swiDelay(WAIT_CYCLES);
        return_byte = V_SRAM[0];
        swiDelay(WAIT_CYCLES);
        if (return_byte==0xF0) { // DS Motion Pak returns 0xF0
                return_byte = V_SRAM[0]; // read second byte of DS Motion Pak check
                swiDelay(WAIT_CYCLES);
                if(return_byte==0x0F) { // DS Motion Pak returns 0x0F
                        motion_pak = 1;
                }
        }
    return motion_pak;
}

// checks whether a DS Motion Card is plugged in
// this only works after motion_init()
// it will return false if it is run before motion_init()
int motion_card_is_inserted(void){
        // send 0x03 to read from DS Motion Card control register
        SPI_On()
        motion_spi(0x03); // command to read from control register
        // if the control register is 0x04 then the enable was successful
        if( motion_spi(0x00) == 0x04)
        {
                SPI_Off()
                return 1;
        }
        SPI_Off();
        return 0;
}

// turn on the DS Motion Sensor (DS Motion Pak or DS Motion Card)
// Requires knowing which type is present (can be found by using motion_init)
int motion_enable(int card_type) {
        switch (card_type)
        {
                case 1: // DS Motion Pak - automatically enabled on powerup
                        // check to see whether Motion Pak is alive
                        return motion_pak_is_inserted();
                        break;
                case 2: // DS Motion Card
                        // send 0x04, 0x04 to enable
                        SPI_On()
                        motion_spi(0x04); // command to write to control register
                        motion_spi(0x04); // enable
                        SPI_Off()
                        // check to see whether Motion Card is alive
                        return motion_card_is_inserted();
                        break;
                case 3: // MK6 - same command as DS Motion Card
                        // send 0x04, 0x04 to enable
                        SPI_On()
                        motion_spi(0x04); // command to write to control register
                        motion_spi(0x04); // enable
                        SPI_Off()
                        // check to see whether Motion Card is alive
                        return motion_card_is_inserted();
                        break;
                default: // if input parameter is not recognized, return 0
                        return 0;
                        break;
        }
}

// Initialize the DS Motion Sensor
// Determines which DS Motion Sensor is present
// Turns it on
// Does not require knowing which type is present
int motion_init(void) {
        sysSetBusOwners(true, true);
        // first, check for the DS Motion Pak - type 1
        if( motion_pak_is_inserted() == 1 )
    { 
        card_type = 1;
        return 1;
        }// next, check for DS Motion Card - type 2
        if( motion_enable(2) == 1 )
    {
         card_type = 2;
         return 2;
        }
    
    motion_MK6_sensor_mode(); // send command to switch MK6 to sensor mode
        
    if( motion_enable(3) == 1 )
    { 
        card_type = 3;
        return 3;
        }// if neither cases are true, then return 0 to indicate no DS Motion Sensor
        return 0;
}

// Deinitialize the DS Motion Sensor
// In the case of a DS Motion Pak, do nothing - there is nothing to de-init
// In the case of a DS Motion Card, turns off the accelerometer
// In the case of an MK6, turns off accelerometer and switches out of sensor mode into EEPROM mode
void motion_deinit(void) {
        // DS Motion Card - turn off accelerometer
        SPI_On()
        motion_spi(0x04); // command to write to control register
        motion_spi(0x00); // turn it off
        SPI_Off()
        // MK6 - switch to EEPROM mode
        motion_MK6_EEPROM_mode(); // switch MK6 to EEPROM mode
}

// read the X acceleration
signed int motion_read_x(void) {
        unsigned char High_byte = 0;
        unsigned char Low_byte = 0;
        signed int output = 0;
        switch(card_type)
        {
                case 1: // DS Motion Pak
                        High_byte = V_SRAM[2]; // Command to load X High onto bus
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        High_byte = V_SRAM[0]; // get the high byte
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        Low_byte = V_SRAM[0]; // get the low byte
                        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
                        output = (signed int)( (High_byte<<8 | Low_byte)>>4);
                        return output;
                        break;
                case 2: // DS Motion Card
                        SPI_On()
                        motion_spi(0x00); // command to convert X axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;
                case 3: // MK6 - same command as DS Motion Card
                        SPI_On()
                        motion_spi(0x00); // command to convert X axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;                  
                default:
                        return 0;
                        break;
        }
}

// read the Y acceleration
signed int motion_read_y(void) {
        unsigned char High_byte = 0;
        unsigned char Low_byte = 0;
        signed int output = 0;
        switch (card_type)
        {
                case 1: // DS Motion Pak
                        High_byte = V_SRAM[4]; // Command to load Y High onto bus
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        High_byte = V_SRAM[0]; // get the high byte
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        Low_byte = V_SRAM[0]; // get the low byte
                        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
                        output = (signed int)( (High_byte<<8 | Low_byte)>>4);
                        return output;
                        break;
                case 2: // DS Motion Card
                        SPI_On()
                        motion_spi(0x02); // command to convert Y axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;
                case 3: // MK6 - same command as DS Motion Card
                        SPI_On()
                        motion_spi(0x02); // command to convert Y axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;                          
                default:
                        return 0;
                        break;
        }
}

// read the Z acceleration
signed int motion_read_z(void) {
        unsigned char High_byte = 0;
        unsigned char Low_byte = 0;
        signed int output = 0;
        switch (card_type)
        {
                case 1: // DS Motion Pak
                        High_byte = V_SRAM[6]; // Command to load Z High onto bus
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        High_byte = V_SRAM[0]; // get the high byte
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        Low_byte = V_SRAM[0]; // get the low byte
                        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
                        output = (signed int)( (High_byte<<8 | Low_byte)>>4);
                        return output;
                        break;
                case 2: // DS Motion Card
                        SPI_On()
                        motion_spi(0x01); // command to convert Z axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;
                case 3: // MK6 - same command as DS Motion Card
                        SPI_On()
                        motion_spi(0x01); // command to convert Z axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;                          
                default:
                        return 0;
                        break;
        }
}

// read the Z rotation (gyro)
signed int motion_read_gyro(void) {
        unsigned char High_byte = 0;
        unsigned char Low_byte = 0;
        signed int output = 0;
        switch (card_type)
        {
                case 1: // DS Motion Pak
                        High_byte = V_SRAM[8]; // Command to load Gyro High onto bus
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        High_byte = V_SRAM[0]; // get the high byte
                        swiDelay(WAIT_CYCLES); // wait for data ready
                        Low_byte = V_SRAM[0]; // get the low byte
                        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
                        output = (signed int)( (High_byte<<8 | Low_byte)>>4);
                        return output;
                        break;
                case 2: // DS Motion Card
                        SPI_On()
                        motion_spi(0x07); // command to convert Gyro axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;
                case 3: // MK6 - same command as DS Motion Card
                        SPI_On()
                        motion_spi(0x07); // command to convert Gyro axis
                        swiDelay(625); // wait at least 40 microseconds for the A-D conversion
                        output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
                        SPI_Off()
                        return output;
                        break;                          
                default:
                        return 0;
                        break;
        }
}

//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_x(void){
        int accel = motion_read_x();
        return (accel - calibration.xoff) * 1000 / calibration.xsens;
}

//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_y(void){
        int accel = motion_read_y();
        return (accel - calibration.yoff) * 1000 / calibration.ysens;
}
//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_z(void){
        int accel = motion_read_z();
        return (accel - calibration.zoff) * 1000 / calibration.zsens;
}

//converts raw rotation value to degrees per second
int motion_rotation(void){
        int rotation = motion_read_gyro();
        return (rotation - calibration.goff) * 1000 / calibration.gsens;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_x(int sens){
        calibration.xsens = sens - calibration.xoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_y(int sens){
        calibration.ysens = sens - calibration.yoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_z(int sens){
        calibration.zsens = sens - calibration.zoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 825
void motion_set_sens_gyro(int sens){
        calibration.gsens = sens;
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_x(void){
        calibration.xoff = motion_read_x();
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_y(void){
        calibration.yoff = motion_read_y();
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_z(void){
        calibration.zoff = motion_read_z();
}

//this should be called when the axis is under no acceleration
//default is 1680
void motion_set_offs_gyro(void){
        calibration.goff = motion_read_gyro();
}

MotionCalibration* motion_get_calibration(void){
        return &calibration;
}

void motion_set_calibration(MotionCalibration* cal){
        calibration.xsens = cal->xsens;
        calibration.ysens = cal->ysens;
        calibration.zsens = cal->zsens;
        calibration.gsens = cal->gsens;
        calibration.xoff = cal->xoff;
        calibration.yoff = cal->yoff;
        calibration.zoff = cal->zoff;
        calibration.goff = cal->goff;
}

// enable analog input number 1 (ain_1)
void motion_enable_ain_1(void){
        unsigned char return_byte;
    return_byte = V_SRAM[16];
        swiDelay(WAIT_CYCLES);
}

// enable analog input number 2 (ain_2)
void motion_enable_ain_2(void){
        unsigned char return_byte;
    return_byte = V_SRAM[18];
        swiDelay(WAIT_CYCLES);
}

// read from the analog input number 1 - requires enabling ain_1 first
int motion_read_ain_1(void){
        unsigned char High_byte = V_SRAM[12]; // Command to load AIN_1 High onto bus
        swiDelay(WAIT_CYCLES); // wait for data ready
        High_byte = V_SRAM[0]; // get the high byte
        swiDelay(WAIT_CYCLES); // wait for data ready
        unsigned char Low_byte = V_SRAM[0]; // get the low byte
        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
        signed int output = (signed int)( (High_byte<<8 | Low_byte)>>4);
        return output;
}

// read from the analog input number 2 - requires enabling ain_2 first
int motion_read_ain_2(void){
        unsigned char High_byte = V_SRAM[14]; // Command to load AIN_1 High onto bus
        swiDelay(WAIT_CYCLES); // wait for data ready
        High_byte = V_SRAM[0]; // get the high byte
        swiDelay(WAIT_CYCLES); // wait for data ready
        unsigned char Low_byte = V_SRAM[0]; // get the low byte
        swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
        signed int output = (signed int)( (High_byte<<8 | Low_byte)>>4);
        return output;
}