I wrote a simple code to read data from WM+ with my STM32VL-discovery and it work perfectly...
Code: Select all
#include "ch.h"
#include "hal.h"
#include "chprintf.h"
/*
byte 0 is the yaw down speed (bit 7-0),
byte 1 is the roll left speed (bit 7-0),
byte 2 is the pitch left speed (bit 7-0),
byte 3
bit 7-2 is bit 13-8 of the yaw down speed,
bit 1 is the yaw slow mode,
bit 0 is the pitch slow mode,
byte 4
bit 7-2 is bit 13-8 of the roll left speed,
bit 1 is the row slow mode,
bit 0 is the extension connected bit,
byte 5
bit 7-2 is bit 13-8 of the pitch left speed,
bit 1 is always 1,
bit 0 is always 0.
*/
// IDG600 rate gyro
#define GYRO_RX_DEPTH 6
#define GYRO_TX_DEPTH 2
#define GYRO_ON 0x53
#define GYRO_ZERO 0x52
#define GYRO_REC 0x52
msg_t status = RDY_OK;
systime_t tmo = MS2ST(20);
int16_t yaw, pitch, roll; //three axes
int16_t yaw0=0, pitch0=0, roll0=0; //calibration zeroes
int8_t yawR, rollR, pitchR; //what's the rate? (boolean)
void wmpOn(void){
//send 0x04 to address 0xFE to activate WM+
uint8_t txbuf[GYRO_TX_DEPTH];
uint8_t rxbuf[GYRO_RX_DEPTH];
txbuf[0] = 0xFE; /* register address */
txbuf[1] = 0x04;
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, GYRO_ON, txbuf, 2, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD1);
}
void wmpSendZero(void){
//send zero to signal we want info
uint8_t rxbuf[GYRO_RX_DEPTH];
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, GYRO_ZERO, 0x00, 1, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD1);
}
void receiveData(void){
uint8_t rxbuf[GYRO_RX_DEPTH];
//send zero before each request (same as nunchuck)
//request the six bytes from the WM+
// wmpSendZero();
i2cAcquireBus(&I2CD1);
status=i2cMasterTransmitTimeout(&I2CD1, GYRO_REC, 0x00, 1, rxbuf, 6, tmo);
// status=i2cMasterReceiveTimeout(&I2CD1, GYRO_REC, rxbuf, 6, tmo);
i2cReleaseBus(&I2CD1);
yaw=((rxbuf[3]>>2)<<8)+rxbuf[0]-yaw0; //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
pitch=((rxbuf[4]>>2)<<8)+rxbuf[1]-pitch0; //for info on what each byte represents
roll=((rxbuf[5]>>2)<<8)+rxbuf[2]-roll0;
yawR= (rxbuf[3] & (1<<1))>>1;
rollR= (rxbuf[4] & (1<<1))>>1;
pitchR=(rxbuf[3] & (1<<0));
}
void calibrateZeroes(void){
//send zero to signal we want info
int16_t y[3];
int16_t p[3];
int16_t r[3];
uint8_t i;
for (i=0; i<3 ;i++){
receiveData();
y[i]=yaw;
p[i]=pitch;
r[i]=roll;
chThdSleepMilliseconds(20);
}
yaw0=(y[1]+y[2]+y[0])/3;
pitch0=(p[1]+p[2]+p[0])/3;
roll0=(r[1]+r[2]+r[0])/3;
}
static const I2CConfig i2cfg1 = { OPMODE_I2C, 400000, FAST_DUTY_CYCLE_2 };
int main(void) {
BaseSequentialStream *chp;
halInit();
chSysInit();
/* Activates the serial driver 2 using the driver default configuration.
* PA2(TX) and PA3(RX) are routed to USART2.*/
sdStart(&SD2, NULL);
// palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
// palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
chp=&SD2;
/* Starts I2C1 PB7=SDA PB6=SCL */
i2cStart(&I2CD1, &i2cfg1);
palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
palSetPadMode(GPIOB, 7, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
chThdSleepMilliseconds(100);
wmpOn();
chThdSleepMilliseconds(2000);
// calibration
calibrateZeroes();
chprintf(chp,"\n\n\n\r %d %d %d \r\n",yaw0,pitch0,roll0);
while (TRUE) {
receiveData();
chprintf(chp,"%d %d %d %d %d %d\r\n",yaw,pitch,roll,yawR,pitchR,rollR);
chThdSleepMilliseconds(50);
}
return 0;
}
Now I want to port this code to my new STM32F4-discovery but I don't know why the following code doesn't work... I can't understand...
Code: Select all
#include "ch.h"
#include "hal.h"
#include "chprintf.h"
/*
byte 0 is the yaw down speed (bit 7-0),
byte 1 is the roll left speed (bit 7-0),
byte 2 is the pitch left speed (bit 7-0),
byte 3
bit 7-2 is bit 13-8 of the yaw down speed,
bit 1 is the yaw slow mode,
bit 0 is the pitch slow mode,
byte 4
bit 7-2 is bit 13-8 of the roll left speed,
bit 1 is the row slow mode,
bit 0 is the extension connected bit,
byte 5
bit 7-2 is bit 13-8 of the pitch left speed,
bit 1 is always 1,
bit 0 is always 0.
*/
// IDG600 rate gyro
#define GYRO_RX_DEPTH 6
#define GYRO_TX_DEPTH 2
#define GYRO_ON 0x53
#define GYRO_ZERO 0x52
#define GYRO_REC 0x52
msg_t status = RDY_OK;
systime_t tmo = MS2ST(20);
uint8_t rxbuf[GYRO_RX_DEPTH];
int16_t yaw, pitch, roll; //three axes
int8_t yawR, rollR, pitchR; //what's the rate? (boolean)
void wmpOn(void){
//send 0x04 to address 0xFE to activate WM+
uint8_t txbuf[GYRO_TX_DEPTH];
txbuf[0] = 0xFE; /* register address */
txbuf[1] = 0x04;
i2cAcquireBus(&I2CD3);
status = i2cMasterTransmitTimeout(&I2CD3, GYRO_ON, txbuf, 2, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD3);
}
void wmpSendZero(void){
//send zero to signal we want info
i2cAcquireBus(&I2CD3);
status = i2cMasterTransmitTimeout(&I2CD3, GYRO_ZERO, 0x00, 1, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD3);
}
void receiveData(void){
//send zero before each request (same as nunchuck)
//request the six bytes from the WM+
//wmpSendZero();
i2cAcquireBus(&I2CD3);
status=i2cMasterTransmitTimeout(&I2CD3, GYRO_REC, 0x00, 1, rxbuf, 6, tmo);
// status=i2cMasterReceiveTimeout(&I2CD3, GYRO_REC, rxbuf, 6, tmo);
i2cReleaseBus(&I2CD3);
//
// status = i2cMasterTransmitTimeout(&I2CD2, mma8451_addr, txbuf, 1, rxbuf, 6, tmo);
//
yaw=((rxbuf[3]>>2)<<8)+rxbuf[0]; //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
pitch=((rxbuf[4]>>2)<<8)+rxbuf[1]; //for info on what each byte represents
roll=((rxbuf[5]>>2)<<8)+rxbuf[2];
yawR= (rxbuf[3] & (1<<1))>>1;
rollR= (rxbuf[4] & (1<<1))>>1;
pitchR=(rxbuf[3] & (1<<0));
}
static const I2CConfig i2cfg3 = { OPMODE_I2C, 400000, FAST_DUTY_CYCLE_2 };
int main(void) {
BaseSequentialStream *chp;
halInit();
chSysInit();
/* Activates the serial driver 2 using the driver default configuration.
* PA2(TX) and PA3(RX) are routed to USART2.*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
chp=&SD2;
/* Starts I2C3 PC9=SDA PA8=SCL */
i2cStart(&I2CD3, &i2cfg3);
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(4)); // is that correct?
palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(4)); // is that correct?
chThdSleepMilliseconds(100);
wmpOn();
chThdSleepMilliseconds(100);
while (TRUE) {
receiveData();
chprintf(chp,"%d %d %d %d %d %d\r\n",yaw,pitch,roll,yawR,pitchR,rollR);
chThdSleepMilliseconds(100);
}
return 0;
}
The code run without any problem but no data is received... allways 0 0 0 0 0 0
Some idea? What is the mistake? thank you very much in advance.
fede
Ok, I will try to send him a private message! 
