void matrix_activate_output(uint8_t output)
{
uint8_t transmission[2];
switch (output) {
case 7:
DDRB |= 1;
break;
case 8:
DDRB |= 1 << 1;
break;
case 9:
DDRB |= 1 << 2;
break;
case 10:
DDRB |= 1 << 3;
break;
case 11:
DDRD |= 1 << 2;
break;
case 12:
DDRD |= 1 << 3;
break;
case 13:
DDRC |= 1 << 6;
break;
default:
transmission[0] = IODIRA_ADDR;
transmission[1] = ~(1 << 7 | 1 << output);
i2cMasterTransmit(MCP23018_ADDR, transmission, 2, 0, 0);
transmission[0] = GPIOB_ADDR;
i2cMasterTransmit(MCP23018_ADDR, transmission, 1, &GPIOB_state, 1);
break;
}
}
//-----------------------------------------------------------------------------
static void lcd_HD44780_write(uint8_t data) {
if (engineConfiguration->displayMode == DM_HD44780) {
palWritePad(HD44780_PORT_DB7, HD44780_PIN_DB7, data & 0x80 ? 1 : 0);
palWritePad(HD44780_PORT_DB6, HD44780_PIN_DB6, data & 0x40 ? 1 : 0);
palWritePad(HD44780_PORT_DB5, HD44780_PIN_DB5, data & 0x20 ? 1 : 0);
palWritePad(HD44780_PORT_DB4, HD44780_PIN_DB4, data & 0x10 ? 1 : 0);
palSetPad(HD44780_PORT_E, HD44780_PIN_E); // En high
lcdSleep(10); // enable pulse must be >450ns
palClearPad(HD44780_PORT_E, HD44780_PIN_E); // En low
lcdSleep(40); // commands need > 37us to settle
} else {
// LCD D4_pin -> P4
// LCD D5_pin -> P5
// LCD D6_pin -> P6
// LCD D7_pin -> P7
// LCD Pin RS -> P0
// LCD Pin RW -> P1
// LCD Pin E -> P2
i2cAcquireBus(&I2CD1);
txbuf[0] = 4;
i2cMasterTransmit(&I2CD1, LCD_PORT_EXP_ADDR, txbuf, 1, NULL, 0);
lcdSleep(10); // enable pulse must be >450ns
txbuf[0] = 0;
i2cMasterTransmit(&I2CD1, LCD_PORT_EXP_ADDR, txbuf, 1, NULL, 0);
i2cReleaseBus(&I2CD1);
}
}
void matrix_init(void)
{
uint8_t transmission[3];
PORTF |= ~(1 << 3 | 1 << 2);
transmission[0] = IODIRA_ADDR;
transmission[1] = (uint8_t)~(1 << 7);
transmission[2] = (uint8_t)~(1 << 6 | 1 << 7);
i2cMasterTransmit(MCP23018_ADDR, transmission, 3, 0, 0);
transmission[0] = GPPUB_ADDR;
transmission[1] = (uint8_t)~(1 << 6 | 1 << 7);
i2cMasterTransmit(MCP23018_ADDR, transmission, 2, 0, 0);
}
void i2cScanner(I2CDriver *FindI2C, const char *driverName) {
uint8_t x = 0, txbuf[2], rxbuf[2] ;
msg_t messages = 0 ;
for(x = 0 ; x < 128 ; x++){
txbuf[0] = 0x00 ;
txbuf[1] = 0x00 ;
i2cAcquireBus(FindI2C) ;
messages = i2cMasterTransmit(FindI2C, x, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(FindI2C) ;
if(messages == 0) {
if(x == 0x28)
chprintf((BaseSequentialStream *)&OUTPUT, "Differential Pressure Sensor\tDetected on %s at Address 0x28\r\n", driverName) ;
if(x == 0x1E)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Magnetometer\t\tDetected on %s at Address 0x1E\r\n", driverName) ;
if(x == 0x68)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Accel, 3-axis Gyro\tDetected on %s at Address 0x68\r\n", driverName) ;
if(x == 0x69)
chprintf((BaseSequentialStream *)&OUTPUT, "3-axis Accel, 3-axis Gyro\tDetected on %s at Address 0x69\r\n", driverName) ;
if(x == 0x77)
chprintf((BaseSequentialStream *)&OUTPUT, "Static Pressure Sensor (Baro)\tDetected on %s at Address 0x77\r\n", driverName) ;
}
chThdSleepMilliseconds(1) ;
}
chThdSleepMilliseconds(100) ;
}
int hmc5843Read (int16_t *xouth, int16_t *youth, int16_t *zouth,
int16_t *xoutl, int16_t *youtl, int16_t *zoutl)
{
int s = 0;
i2cAcquireBus(d.i2cp);
/* Read X/Y/Z high/low bits and status from 8-bit registers */
d.rx[0] = d.rx[1] = d.rx[2] = d.rx[3] = d.rx[4] = d.rx[5] = d.rx[6] = 0;
i2cMasterReceive(d.i2cp, &d.cfg, d.addr, d.rx, 2);
#if 1
d.tx[0] = HMC5843_MODE;
d.tx[1] = HMC5843_MODE_SINGLE;
i2cMasterTransmit(d.i2cp, &dummycfg, d.addr, d.tx, 2, NULL, 0);
#endif
i2cReleaseBus(d.i2cp);
chThdSleepMilliseconds(60);
*xouth = d.rx[0];
*xoutl = d.rx[1];
*youth = d.rx[2];
*youtl = d.rx[3];
*zouth = d.rx[4];
*zoutl = d.rx[5];
return s * 5;
}
/*
* Application entry point.
*/
int main(void) {
uint8_t tx[3];
uint8_t rx[2];
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
palClearPad(IOPORT2, PORTB_LED1);
i2cStart(&I2CD1, NULL);
/*
* Starts the LED blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
while (TRUE) {
/* Write value 0xAA to address 0 on the 24C64 */
tx[0] = 0x00;
tx[1] = 0x00;
tx[2] = 0xAA;
i2cMasterTransmit(&I2CD1, 0x50, tx, 3, NULL, 0);
/* Send address 0 and read value at that location */
i2cMasterTransmit(&I2CD1, 0x50, tx, 2, rx, 1);
chThdSleepMilliseconds(1000);
}
}
void hmc5843Init(I2CDriver *i2cp)
{
/* Make sure the power-up has finished (spec says 8.3ms) */
chThdSleepMilliseconds(9);
d.i2cp = i2cp;
i2cAcquireBus(d.i2cp);
/* Set up continuous measurement mode */
d.tx[0] = HMC5843_MODE;
d.tx[1] = HMC5843_MODE_SINGLE;
i2cMasterTransmit(d.i2cp, &dummycfg, d.addr, d.tx, 2, NULL, 0);
i2cReleaseBus(d.i2cp);
}
void matrix_deactivate_output(uint8_t output)
{
uint8_t transmission[2];
switch (output) {
case 0:
transmission[0] = IODIRA_ADDR;
transmission[1] = (uint8_t)~(1 << 7);
i2cMasterTransmit(MCP23018_ADDR, transmission, 2, 0, 0);
break;
case 7 ... 10:
DDRB &= ~(1 << 3 | 1 << 2 | 1 << 1 | 1);
break;
case 11 ... 12:
DDRD &= ~(1 << 3 | 1 << 2);
break;
case 13:
DDRC &= ~(1 << 6);
break;
}
}
/*
* This function sets up the MPU and the I2C driver.
* MPU registers can be read thereafter.
*/
void set_mpu60X0(IMUData *imudat, IMUConfig *imucfg) {
uint8_t txbuf[2], rxbuf[2] ;
msg_t status = RDY_OK ;
txbuf[0] = PWR_MGMT_1 ;
txbuf[1] = 0x01 ;
i2cAcquireBus(&I2C_MPU) ;
status = i2cMasterTransmit(&I2C_MPU, MPU_ADDR, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(&I2C_MPU) ;
if(status != RDY_OK)
return ;
chThdSleepMilliseconds(35) ;
txbuf[0] = GYRO_CONFIG ;
txbuf[1] = 0x00 ;
i2cAcquireBus(&I2C_MPU) ;
status = i2cMasterTransmit(&I2C_MPU, MPU_ADDR, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(&I2C_MPU) ;
switch(imucfg->GYRO_FS_SEL) {
case FS_SEL_250 : imudat->GYRO_SENS = 131.0f ;
break ;
case FS_SEL_500 : imudat->GYRO_SENS = 65.5f ;
break ;
case FS_SEL_1000 : imudat->GYRO_SENS = 32.8f ;
break ;
case FS_SEL_2000 : imudat->GYRO_SENS = 16.4f ;
break ;
}
if(status != RDY_OK)
return ;
chThdSleepMilliseconds(35) ;
txbuf[0] = ACCEL_CONFIG ;
txbuf[1] = 0x00 ;
i2cAcquireBus(&I2C_MPU) ;
status = i2cMasterTransmit(&I2C_MPU, MPU_ADDR, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(&I2C_MPU) ;
switch(imucfg->ACCEL_FS_SEL) {
case AFS_SEL_2g : imudat->ACCEL_SENS = 8192.0f ;
break ;
case AFS_SEL_4g : imudat->ACCEL_SENS = 4096.0f ;
break ;
case AFS_SEL_8g : imudat->ACCEL_SENS = 2048.0f ;
break ;
case AFS_SEL_16g : imudat->ACCEL_SENS = 1024.0f ;
break ;
}
if(status != RDY_OK)
return ;
chThdSleepMilliseconds(35) ;
txbuf[0] = SMPRT_DIV ;
txbuf[1] = 0x09 ;
i2cAcquireBus(&I2C_MPU) ;
status = i2cMasterTransmit(&I2C_MPU, MPU_ADDR, txbuf, 2, rxbuf, 0) ;
i2cReleaseBus(&I2C_MPU) ;
if(status != RDY_OK)
return ;
imudat->ACCEL_X = 0.0f ;
imudat->ACCEL_Y = 0.0f ;
imudat->ACCEL_Z = 0.0f ;
imudat->RAW_ACCEL_X = 0 ;
imudat->RAW_ACCEL_Y = 0 ;
imudat->RAW_ACCEL_Z = 0 ;
imudat->GYRO_X = 0.0f ;
imudat->GYRO_Y = 0.0f ;
imudat->GYRO_Z = 0.0f ;
imudat->RAW_GYRO_X = 0 ;
imudat->RAW_GYRO_Y = 0 ;
imudat->RAW_GYRO_Z = 0 ;
imudat->RAW_TEMP = 0 ;
chThdSleepMilliseconds(35) ;
#if PLUTO_CALIBRATE_IMU
calibrateIMU(reg_coeffs) ;
#else
uint8_t i ;
for(i = 0 ; i < 11 ; i++) {
if(i % 2 == 0)
reg_coeffs[i] = 0.0f ;
else
reg_coeffs[i] = 1.0f ;
}
#endif /*PLUTO_CALIBRATE_IMU */
}
static void sendI2Cbyte(int addr, int data) {
i2cAcquireBus(&I2CD1);
txbuf[0] = data;
i2cMasterTransmit(&I2CD1, addr, txbuf, 1, NULL, 0);
i2cReleaseBus(&I2CD1);
}
