the MK6 motion source code

/*———————————————————————————
   $Id: ndsmotion.c,v 1.4 2007/05/22 18:05:26 dovoto Exp $

   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.

   $Log: ndsmotion.c,v $
   Revision 1.4  2007/05/22 18:05:26  dovoto
   updated to support multiple ds motion cards.  motion_enable is now motion_init…see ds_motion example

   
   Revision 1.4  2007/05/18 9:35:01  KeithE
   Added support for DS Motion Pak and MK6
   Added motion_init() to determine which type of motion sensor is present (0 if none)
   motion_init() NEEDS TO BE RUN BEFORE USING THE MOTION SENSOR FUNCTIONS
   Added input parameter to motion_read_ functions that contains the motion sensor type
   Use motion_init() to check whether a DS Motion Sensor is inserted
   
   Revision 1.3  2007/01/10 16:47:30  dovoto
   Added calibtation settings to ds motion code
   
   Revision 1.2  2007/01/09 06:23:43  dovoto
   Fixed logging header for ndsmotion.h and ndsmotion.c
   

———————————————————————————*/

#include <nds.h>
#include "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: ndsmotion.h,v 1.6 2007/05/22 18:05:26 dovoto Exp $

   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.

   $Log: ndsmotion.h,v $
   Revision 1.6  2007/05/22 18:05:26  dovoto
   updated to support multiple ds motion cards.  motion_enable is now motion_init…see ds_motion example

   
   Revision 1.5  2007/05/18 9:36:45  KeithE
   Added support for DS Motion Pak and MK6
   Added motion_init() to determine which type of motion sensor is present (0 if none)
   motion_init() NEEDS TO BE RUN BEFORE USING THE MOTION SENSOR FUNCTIONS
   Added input parameter to motion_read_ functions that contains the motion sensor type
   Use motion_init() to check whether a DS Motion Sensor is inserted
   
   Revision 1.4  2007/01/10 16:58:16  dovoto
   Fixed a type in motion_set_sens_z()
   
   Revision 1.3  2007/01/10 16:47:30  dovoto
   Added calibtation settings to ds motion code
   
   Revision 1.2  2007/01/09 06:23:43  dovoto
   Fixed logging header for ndsmotion.h and ndsmotion.c
   
   Revision 1.1  2007/01/09 06:11:54  dovoto
   Added DS motion card functionality to libnds!
   

———————————————————————————*/

/*! \file motion.h
\brief interface code for the ds motion card, ds motion pak, MK6

*/
#ifndef NDS_MOTION_INCLUDE
#define NDS_MOTION_INCLUDE
//———————————————————————————

typedef struct
{
   short xoff, yoff, zoff, goff;
   short xsens, ysens, zsens, gsens;
}MotionCalibration;

#ifdef __cplusplus
extern "C" {
#endif

/*! \fn int motion_init()
\brief  Initializes the DS Motion Sensor.
Run this before using any of the DS Motion Sensor functions
save the return value and pass it to the other motion_ functions
*/
int motion_init();

/*! \fn int motion_deinit()
\brief  Deinitializes the DS Motion Sensor
*/
void motion_deinit();

/*! \fn void motion_enable()
\brief  read the X acceleration
*/
signed int motion_read_x();

/*! \fn signed int motion_read_y()
\brief  read the Y acceleration
*/
signed int motion_read_y();

/*! \fn signed int motion_read_z()
\brief  read the Z acceleration
*/
signed int motion_read_z();

/*! \fn signed int motion_read_gyro()
\brief  read the Z rotational speed
*/
signed int motion_read_gyro();

/*! \fn int motion_acceleration_x()
\brief gets acceleration value to mili G (where g is 9.8 m/s*s)
*/
int motion_acceleration_x();

/*! \fn int motion_acceleration_y()
\brief gets acceleration value to mili G (where g is 9.8 m/s*s)
*/
int motion_acceleration_y();

/*! \fn int motion_acceleration_z()
\brief gets acceleration value to mili G (where g is 9.8 m/s*s)
*/
int motion_acceleration_z();

/*! \fn void motion_set_sens_x(int sens)
\brief this should be passed the raw reading at 1g for accurate
acceleration calculations.  Default is 819
*/
void motion_set_sens_x(int sens);

/*! \fn void motion_set_sens_y(int sens)
\brief this should be passed the raw reading at 1g for accurate
acceleration calculations.  Default is 819
*/
void motion_set_sens_y(int sens);

/*! \fn void motion_set_sens_z(int sens)
\brief this should be passed the raw reading at 1g for accurate
acceleration calculations.  Default is 819
*/
void motion_set_sens_z(int sens);

/*! \fn void motion_set_sens_x(int sens)
\brief this should be passed the raw reading at 1g for accurate
acceleration calculations.  Default is 825
*/
void motion_set_sens_gyro(int sens);

/*! \fn void motion_set_offs_x()
\brief this should be called when the axis is under no acceleration
default is 2048
*/
void motion_set_offs_x();

/*! \fn void motion_set_offs_y()
\brief this should be called when the axis is under no acceleration
default is 2048
*/
void motion_set_offs_y();

/*! \fn void motion_set_offs_z()
\brief this should be called when the axis is under no acceleration
default is 2048
*/
void motion_set_offs_z();

/*! \fn void motion_set_offs_gyro()
\brief this should be called when the axis is under no rotation
default is 1680
*/
void motion_set_offs_gyro();

/*! \fn int motion_rotation(int rotation)
\brief converts raw rotation value to degrees per second
*/
int motion_rotation();

/*! \fn MotionCalibration* motion_get_calibration()
\brief This returns the current calibration settings for saving
*/
MotionCalibration* motion_get_calibration();

/*! \fn MotionCalibration* motion_get_calibraion()
\brief This sets the calibration settings.  Intended
to restore saved calibration settings
*/
void motion_set_calibration(MotionCalibration* cal);

/*! \fn MotionCalibration* motion_enable_ain_1()
\brief This enables the analog input number 1.
Required before reading analog input number 1.
*/
void motion_enable_ain_1();

/*! \fn MotionCalibration* motion_enable_ain_2()
\brief This enables the analog input number 2.
Required before reading analog input number 2.
*/
void motion_enable_ain_2();

/*! \fn MotionCalibration* motion_read_ain_1()
\brief This reads the analog input number 1.
analog input number 1 needs to be enabled before reading.
*/
int motion_read_ain_1();

/*! \fn MotionCalibration* motion_read_ain_2()
\brief This reads the analog input number 2.
analog input number 2 needs to be enabled before reading.
*/
int motion_read_ain_2();

#ifdef __cplusplus
}
#endif

#endif