mdcompat.cpp

00001 #include "mdcompat.h"
00002 #include "mbed.h" // must be out of the scope of extern "C"
00003 I2C imu9250(I2C_SDA,I2C_SCL);
00004 Ticker stamper;
00005 unsigned long stamper_count=0;
00006 void counter(){
00007     stamper_count++;    
00008 }
00009 void stamper_init(void){
00010 stamper.attach(&counter,1);
00011 }
00012 void imu_init(void){
00013 imu9250.frequency(400000);    
00014 }
00015 unsigned short constrain(
00016     unsigned short x,
00017     unsigned short a, 
00018     unsigned short b) {unsigned short result;
00019     result= (x<a) ? a:x;
00020     result= (x>b) ? b:x;
00021     return result;    
00022 }
00023 
00024 extern "C" {
00025 
00026 int mbed_i2c_write(
00027     unsigned char slave_addr,
00028     unsigned char reg_addr,
00029     unsigned char length,
00030     unsigned char *data) {
00031     
00032     unsigned char buffer[length+1];
00033     buffer[0]=reg_addr;
00034     for(int i=0;i<length;i++){
00035         buffer[i+1]=data[i];
00036     }
00037     const char* x=(const char*)buffer;//
00038     
00039     imu9250.write((int)slave_addr<<1,x,length+1,0);//sending register adress first to indicate which register we are writing and a write register(0)
00040     return 0;
00041 }
00042 
00043 int mbed_i2c_read(
00044     unsigned char slave_addr,
00045     unsigned char reg_addr,
00046     unsigned char length,
00047     unsigned char *data) {const char RA[]={reg_addr};
00048     unsigned char buffer[length];
00049     for(int i=0;i<length;i++){
00050         buffer[i]=data[i];
00051     }
00052     imu9250.write((int)slave_addr<<1,RA, 1, 1); // no stop
00053     imu9250.read((int)slave_addr<<1,(char*)data, length, 0); 
00054     return 0; 
00055 }
00056 
00057 int delay_ms(
00058     unsigned long num_ms) {
00059     wait_ms(num_ms);
00060     return 0;
00061 }
00062 
00063 int get_ms(
00064     unsigned long *count=(unsigned long*)malloc(sizeof(unsigned long))) {
00065         *count=stamper_count;
00066     return (int)stamper_count;
00067 }
00068 
00069 int reg_int_cb(
00070     void (*cb)(void),
00071     unsigned char port,
00072     unsigned char pin) {
00073     return 0;   
00074 }
00075 
00076 long labs(long x) {
00077     return x > 0 ? x : -x;
00078 }
00079 
00080 float fabsf(float x) {
00081     return x > 0 ? x : -x;
00082 }
00083 
00084 int min(int a, int b) {
00085     return a > b ? b : a;
00086 }
00087 
00088 };