diff options
Diffstat (limited to 'c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c')
| -rw-r--r-- | c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c | 488 |
1 files changed, 0 insertions, 488 deletions
diff --git a/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c b/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c deleted file mode 100644 index 1faadcd5de..0000000000 --- a/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c +++ /dev/null @@ -1,488 +0,0 @@ -/*--------------------------------------------------------------------------------- - 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 -static 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; -} - - -static 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() -} - -static 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 -static 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() -static 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) -static 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; - (void) return_byte; /* avoid set but unused warning */ - 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; - (void) return_byte; /* avoid set but unused warning */ - 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; -} - - |