diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c index d0de65700e..3ae02d1012 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c @@ -1,6 +1,6 @@ /* * - * Copyright (C) 2009-2013 The Paparazzi Team + * Copyright (C) 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -22,58 +22,104 @@ */ /** @file arch/omap/mcu_periph/i2c_arch.h - * I2C functionality (unimplemented) + * I2C functionality */ #include "mcu_periph/i2c.h" +#include +#include +#include +#include +#include +#include +void i2c_event(void) +{ +} -bool_t i2c_idle(struct i2c_periph *p __attribute__ ((unused))) { return TRUE; } -bool_t i2c_submit(struct i2c_periph* p __attribute__ ((unused)), struct i2c_transaction* t __attribute__ ((unused))) { return TRUE;} -void i2c_event(void) { } -void i2c_setbitrate(struct i2c_periph* p, int bitrate __attribute__ ((unused))) { } +void i2c_setbitrate(struct i2c_periph *p __attribute__((unused)), int bitrate __attribute__((unused))) +{ +} + +bool_t i2c_idle(struct i2c_periph *p __attribute__((unused))) +{ + return TRUE; +} + +bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) +{ + int file = (int)p->reg_addr; + + // Set the slave address + ioctl(file, I2C_SLAVE, t->slave_addr); + + // Switch the different transaction types + switch (t->type) { + // Just transmitting + case I2CTransTx: + if (write(file, (uint8_t *)t->buf, t->len_w) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + // Just reading + case I2CTransRx: + if (read(file, (uint8_t *)t->buf, t->len_r) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + // First Transmit and then read + case I2CTransTxRx: + if (write(file, (uint8_t *)t->buf, t->len_w) < 0 || read(file, (uint8_t *)t->buf, t->len_r) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + default: + break; + } + + // Successfull transfer + t->status = I2CTransSuccess; + return TRUE; +} #if USE_I2C0 struct i2c_errors i2c0_errors; -void i2c0_hw_init(void) { +void i2c0_hw_init(void) +{ + i2c1.reg_addr = (void *)open("/dev/i2c-0", O_RDWR); i2c0.errors = &i2c0_errors; + + /* zeros error counter */ ZEROS_ERR_COUNTER(i2c0_errors); } - -void i2c0_ev_isr(void) { -} - -void i2c0_er_isr(void) { -} #endif #if USE_I2C1 struct i2c_errors i2c1_errors; -void i2c1_hw_init(void) { +void i2c1_hw_init(void) +{ + i2c1.reg_addr = (void *)open("/dev/i2c-1", O_RDWR); i2c1.errors = &i2c1_errors; + + /* zeros error counter */ ZEROS_ERR_COUNTER(i2c1_errors); } - -void i2c1_ev_isr(void) { -} - -void i2c1_er_isr(void) { -} #endif #if USE_I2C2 struct i2c_errors i2c2_errors; -void i2c2_hw_init(void) { +void i2c2_hw_init(void) +{ + i2c2.reg_addr = (void *)open("/dev/i2c-2", O_RDWR); i2c2.errors = &i2c2_errors; + + /* zeros error counter */ ZEROS_ERR_COUNTER(i2c2_errors); } - -void i2c2_ev_isr(void) { -} - -void i2c2_er_isr(void) { -} #endif diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_arch.h b/sw/airborne/arch/omap/mcu_periph/i2c_arch.h index 2d1a835e79..0be85364cd 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_arch.h +++ b/sw/airborne/arch/omap/mcu_periph/i2c_arch.h @@ -1,6 +1,6 @@ /* * - * Copyright (C) 2009-2013 The Paparazzi Team + * Copyright (C) 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -22,7 +22,7 @@ */ /** @file arch/omap/mcu_periph/i2c_arch.h - * I2C functionality (unimplemented) + * I2C functionality */ #ifndef OMAP_MCU_PERIPH_I2C_ARCH_H @@ -30,27 +30,18 @@ #include "mcu_periph/i2c.h" -#define I2cSendStart() {} - - #if USE_I2C0 - extern void i2c0_hw_init(void); - #endif /* USE_I2C0 */ #if USE_I2C1 - extern void i2c1_hw_init(void); - #endif /* USE_I2C1 */ #if USE_I2C2 - extern void i2c2_hw_init(void); - #endif /* USE_I2C2 */ diff --git a/sw/airborne/boards/ardrone/i2c-dev.h b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h similarity index 71% rename from sw/airborne/boards/ardrone/i2c-dev.h rename to sw/airborne/arch/omap/mcu_periph/i2c_smbus.h index 79985d2a6b..77cd9575ea 100644 --- a/sw/airborne/boards/ardrone/i2c-dev.h +++ b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h @@ -31,6 +31,7 @@ #define LIB_I2CDEV_H #include +#include #include /* @@ -39,12 +40,12 @@ struct i2c_msg { __u16 addr; /* slave address */ unsigned short flags; -#define I2C_M_TEN 0x10 /* we have a ten bit chip address */ -#define I2C_M_RD 0x01 -#define I2C_M_NOSTART 0x4000 +#define I2C_M_TEN 0x10 /* we have a ten bit chip address */ +#define I2C_M_RD 0x01 +#define I2C_M_NOSTART 0x4000 #define I2C_M_REV_DIR_ADDR 0x2000 -#define I2C_M_IGNORE_NAK 0x1000 -#define I2C_M_NO_RD_ACK 0x0800 +#define I2C_M_IGNORE_NAK 0x1000 +#define I2C_M_NO_RD_ACK 0x0800 short len; /* msg length */ char *buf; /* pointer to msg data */ }; @@ -92,7 +93,7 @@ union i2c_smbus_data { __u8 byte; __u16 word; __u8 block[I2C_SMBUS_BLOCK_MAX + 2]; /* block[0] is used for length */ - /* and one more for PEC */ + /* and one more for PEC */ }; /* smbus_access read or write markers */ @@ -117,20 +118,20 @@ union i2c_smbus_data { * dependent layers - these can be listed here, or see the * corresponding header files. */ - /* -> bit-adapter specific ioctls */ +/* -> bit-adapter specific ioctls */ #define I2C_RETRIES 0x0701 /* number of times a device address */ - /* should be polled when not */ - /* acknowledging */ +/* should be polled when not */ +/* acknowledging */ #define I2C_TIMEOUT 0x0702 /* set timeout - call with int */ /* this is for i2c-dev.c */ #define I2C_SLAVE 0x0703 /* Change slave address */ - /* Attn.: Slave address is 7 or 10 bits */ +/* Attn.: Slave address is 7 or 10 bits */ #define I2C_SLAVE_FORCE 0x0706 /* Change slave address */ - /* Attn.: Slave address is 7 or 10 bits */ - /* This changes the address, even if it */ - /* is already taken! */ +/* Attn.: Slave address is 7 or 10 bits */ +/* This changes the address, even if it */ +/* is already taken! */ #define I2C_TENBIT 0x0704 /* 0 for 7 bit addrs, != 0 for 10 bit */ #define I2C_FUNCS 0x0705 /* Get the adapter functionality */ @@ -144,20 +145,6 @@ union i2c_smbus_data { /* Note: 10-bit addresses are NOT supported! */ -/* This is the structure as used in the I2C_SMBUS ioctl call */ -struct i2c_smbus_ioctl_data { - char read_write; - __u8 command; - int size; - union i2c_smbus_data *data; -}; - -/* This is the structure as used in the I2C_RDWR ioctl call */ -struct i2c_rdwr_ioctl_data { - struct i2c_msg *msgs; /* pointers to i2c_msgs */ - int nmsgs; /* number of i2c_msgs */ -}; - static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, union i2c_smbus_data *data) @@ -168,65 +155,68 @@ static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, args.command = command; args.size = size; args.data = data; - return ioctl(file,I2C_SMBUS,&args); + return ioctl(file, I2C_SMBUS, &args); } static inline __s32 i2c_smbus_write_quick(int file, __u8 value) { - return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,NULL); + return i2c_smbus_access(file, value, 0, I2C_SMBUS_QUICK, NULL); } static inline __s32 i2c_smbus_read_byte(int file) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,0,I2C_SMBUS_BYTE,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, 0, I2C_SMBUS_BYTE, &data)) { return -1; - else + } else { return 0x0FF & data.byte; + } } static inline __s32 i2c_smbus_write_byte(int file, __u8 value) { - return i2c_smbus_access(file,I2C_SMBUS_WRITE,value, - I2C_SMBUS_BYTE,NULL); + return i2c_smbus_access(file, I2C_SMBUS_WRITE, value, + I2C_SMBUS_BYTE, NULL); } static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_BYTE_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_BYTE_DATA, &data)) { return -1; - else + } else { return 0x0FF & data.byte; + } } static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command, - __u8 value) + __u8 value) { union i2c_smbus_data data; data.byte = value; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); } static inline __s32 i2c_smbus_read_word_data(int file, __u8 command) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_WORD_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_WORD_DATA, &data)) { return -1; - else + } else { return 0x0FFFF & data.word; + } } static inline __s32 i2c_smbus_write_word_data(int file, __u8 command, - __u16 value) + __u16 value) { union i2c_smbus_data data; data.word = value; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_WORD_DATA, &data); } @@ -234,41 +224,45 @@ static inline __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value) { union i2c_smbus_data data; data.word = value; - if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, - I2C_SMBUS_PROC_CALL,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_WRITE, command, + I2C_SMBUS_PROC_CALL, &data)) { return -1; - else + } else { return 0x0FFFF & data.word; + } } /* Returns the number of read bytes */ static inline __s32 i2c_smbus_read_block_data(int file, __u8 command, - __u8 *values) + __u8 *values) { union i2c_smbus_data data; int i; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_BLOCK_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_BLOCK_DATA, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } static inline __s32 i2c_smbus_write_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BLOCK_DATA, &data); } @@ -277,56 +271,63 @@ static inline __s32 i2c_smbus_write_block_data(int file, __u8 command, ask for less than 32 bytes, your code will only work with kernels 2.6.23 and later. */ static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; + } data.block[0] = length; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN : - I2C_SMBUS_I2C_BLOCK_DATA,&data)) + I2C_SMBUS_I2C_BLOCK_DATA, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } static inline __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_I2C_BLOCK_BROKEN, &data); } /* Returns the number of read bytes */ static inline __s32 i2c_smbus_block_process_call(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, - I2C_SMBUS_BLOCK_PROC_CALL,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_WRITE, command, + I2C_SMBUS_BLOCK_PROC_CALL, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c index 4f768b049c..6a9d2488a3 100644 --- a/sw/airborne/boards/ardrone/electrical_raw.c +++ b/sw/airborne/boards/ardrone/electrical_raw.c @@ -35,7 +35,7 @@ #include #include #include -#include "i2c-dev.h" +#include "mcu_periph/i2c_smbus.h" #include "subsystems/commands.h" #include "generated/airframe.h" @@ -67,45 +67,48 @@ static struct { int fd; -void electrical_init(void) { +void electrical_init(void) +{ // First we try to kill the program.elf and its respawner if it is running (done here because initializes first) int ret = system("killall -9 program.elf.respawner.sh; killall -9 program.elf"); (void) ret; // Initialize 12c device for power - fd = open( "/dev/i2c-1", O_RDWR ); - if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) { - fprintf( stderr, "Failed to set slave address: %m\n" ); + fd = open("/dev/i2c-1", O_RDWR); + if (ioctl(fd, I2C_SLAVE_FORCE, 0x4a) < 0) { + fprintf(stderr, "Failed to set slave address: %m\n"); } electrical_setup(); electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY; } -void electrical_setup(void) { +void electrical_setup(void) +{ // Turn on MADC in CTRL1 - if( i2c_smbus_write_byte_data( fd, 0x00, 0x01)) { - fprintf( stderr, "Failed to write to I2C device. 1\n" ); + if (i2c_smbus_write_byte_data(fd, 0x00, 0x01)) { + fprintf(stderr, "Failed to write to I2C device. 1\n"); } // Select ADCIN0 for conversion in SW1SELECT_LSB - if( i2c_smbus_write_byte_data( fd, 0x06, 0xff)){ - fprintf( stderr, "Failed to write to I2C device. 2\n" ); + if (i2c_smbus_write_byte_data(fd, 0x06, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 2\n"); } // Select ADCIN12 for conversion in SW1SELECT_MSB - if( i2c_smbus_write_byte_data( fd, 0x07, 0xff)) { - fprintf( stderr, "Failed to write to I2C device. 3\n" ); + if (i2c_smbus_write_byte_data(fd, 0x07, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 3\n"); } // Setup register for averaging - if( i2c_smbus_write_byte_data( fd, 0x08, 0xff)) { - fprintf( stderr, "Failed to write to I2C device. 4\n" ); + if (i2c_smbus_write_byte_data(fd, 0x08, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 4\n"); } // Start all channel conversion by setting bit 5 to one in CTRL_SW1 - if( i2c_smbus_write_byte_data( fd, 0x12, 0x20)) { - fprintf( stderr, "Failed to write to I2C device. 5\n" ); + if (i2c_smbus_write_byte_data(fd, 0x12, 0x20)) { + fprintf(stderr, "Failed to write to I2C device. 5\n"); } } -void electrical_periodic(void) { +void electrical_periodic(void) +{ electrical_setup(); @@ -122,7 +125,7 @@ void electrical_periodic(void) { //from raw measurement we got quite a lineair response //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923 //leading to our 0.13595166 magic number for decivolts conversion - electrical.vsupply = raw_voltage*0.13595166; + electrical.vsupply = raw_voltage * 0.13595166; /* * Superellipse: abs(x/a)^n + abs(y/b)^n = 1 @@ -139,5 +142,5 @@ void electrical_periodic(void) { /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor */ - electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); + electrical.current = b - pow((pow(b, electrical_priv.nonlin_factor) - pow((b * x), electrical_priv.nonlin_factor)), (1. / electrical_priv.nonlin_factor)); }