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 | 976 |
1 files changed, 488 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 index 7a31e1cfbc..830329fe11 100644 --- a/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c +++ b/c/src/lib/libbsp/arm/nds/libnds/source/arm9/ndsmotion.c @@ -1,488 +1,488 @@ -/*---------------------------------------------------------------------------------
- $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() {
- // 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() {
- // 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(){
- 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(){
- // 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() {
- 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() {
- // 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() {
- 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(){
- 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(){
- unsigned char return_byte;
- return_byte = V_SRAM[16];
- swiDelay(WAIT_CYCLES);
-}
-
-// enable analog input number 2 (ain_2)
-void motion_enable_ain_2(){
- 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(){
- 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(){
- 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;
-}
-
-
+/*--------------------------------------------------------------------------------- + $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() { + // 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() { + // 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(){ + 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(){ + // 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() { + 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() { + // 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() { + 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(){ + 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(){ + unsigned char return_byte; + return_byte = V_SRAM[16]; + swiDelay(WAIT_CYCLES); +} + +// enable analog input number 2 (ain_2) +void motion_enable_ain_2(){ + 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(){ + 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(){ + 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; +} + + |