// Get rows from other half over i2c
bool transport_master(matrix_row_t matrix[]) {
i2c_readReg(SLAVE_I2C_ADDRESS, I2C_KEYMAP_START, (void *)matrix, sizeof(i2c_buffer->smatrix), TIMEOUT);
// write backlight info
# ifdef BACKLIGHT_ENABLE
uint8_t level = get_backlight_level();
if (level != i2c_buffer->backlight_level) {
if (i2c_writeReg(SLAVE_I2C_ADDRESS, I2C_BACKLIGHT_START, (void *)&level, sizeof(level), TIMEOUT) >= 0) {
i2c_buffer->backlight_level = level;
}
}
# endif
# if defined(RGBLIGHT_ENABLE) && defined(RGBLIGHT_SPLIT)
if (rgblight_get_change_flags()) {
rgblight_syncinfo_t rgblight_sync;
rgblight_get_syncinfo(&rgblight_sync);
if (i2c_writeReg(SLAVE_I2C_ADDRESS, I2C_RGB_START,
(void *)&rgblight_sync, sizeof(rgblight_sync), TIMEOUT) >= 0) {
rgblight_clear_change_flags();
}
}
# endif
# ifdef ENCODER_ENABLE
i2c_readReg(SLAVE_I2C_ADDRESS, I2C_ENCODER_START, (void *)i2c_buffer->encoder_state, sizeof(I2C_slave_buffer_t.encoder_state), TIMEOUT);
encoder_update_raw(i2c_buffer->encoder_state);
# endif
return true;
}
bool MPU_init() {
uint8_t ret=0;
#if defined(TRACE)
Serial.println(">MPU_init");
#endif
// FIX ME change the I2C clock rate to 400kHz like multiwii ?
ret=i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
if (ret> 0) return false;
delay(5);
ret=i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
if (ret> 0) return false;
ret=i2c_writeReg(MPU6050_ADDRESS, 0x1A, 0x00); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
if (ret> 0) return false;
return true;
}
bool ACC_init () {
#if defined(TRACE)
Serial.println(">ACC_init");
#endif
i2c_writeReg(MPU6050_ADDRESS, 0x1C, 0x10); //ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec.
//note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
//confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480
}
bool Gyro_init() {
uint8_t ret=0;
ret=i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
if (ret> 0) return false;
return true;
}
// To set the TVP6146 mode
Bool setTVP5146Mode(Uint32 modeSTM ) {
Bool resultSTM = FALSE;
Uint8 videoStandardSTM = 0;
Uint8 bufferSTM[2] = {0};
PAL_osWaitMsecs(100u);
if ( TRUE == i2c_readReg( I2C_TVP_SLAVE_ADDR, bufferSTM, 2u) ) {
bufferSTM[0] = OUTPUT_FORMATTER_1_REG;
bufferSTM[1] |= ((Uint8)((modeSTM & 0x8) << 4)); // 4th-bit for squre pixel sampling
if(TRUE == i2c_writeReg( I2C_TVP_SLAVE_ADDR, bufferSTM, 2u)) {
resultSTM = TRUE;
}
}
if ( resultSTM == TRUE ) {
resultSTM = FALSE;
bufferSTM[0] = VIDEO_STANDARD_REG;
bufferSTM[1] = (Uint8)( modeSTM & 0x7);
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR, bufferSTM, 2u)) {
resultSTM = TRUE;
}
}
if ( resultSTM == TRUE ) {
resultSTM = FALSE;
if ( PSP_VPFE_TVP5146_MODE_AUTO == videoStandardSTM ) {
bufferSTM[0] = AUTOSWITCH_MASK_REG;
bufferSTM[1] = 0x3F; // enable autoswitch for all standards
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTM,2u)) {
resultSTM = TRUE;
}
} else {
bufferSTM[0] = AUTOSWITCH_MASK_REG;
bufferSTM[1] = 0x23; // For NTSC and PAL
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTM,2u)) {
resultSTM = TRUE;
}
}
}
return(resultSTM);
}
bool init_accel(void) {
// Set Internal Clock to PLL with X gyroscope, recommended
// Disables Temperature Sensor
uint8_t c = (1 << 3) | (1 << 0);
if (i2c_writeReg(ACCEL_ADDR, PWR_MGMT_1, &c, 1) != 0) {
return false;
}
set_sensitivity(1);
// Set Motion Trigger Duration to 2ms
c = 2;
if (i2c_writeReg(ACCEL_ADDR, MOT_DUR, &c, 1) != 0) {
return false;
}
// Clear Interrupts
accel_clear_int();
// Enable Motion Trigger Interrupts
c = 0x40;
if (i2c_writeReg(ACCEL_ADDR, INT_ENABLE, &c, 1) != 0) {
return false;
}
return true;
}
// To set the TVP6146 Format
Bool setTVP5146Format(Uint32 formatSTF) {
Bool resultSTF = FALSE;
Uint8 bufferSTF[2] = {0};
PAL_osWaitMsecs(100u);
if (PSP_VPFE_TVP5146_FORMAT_COMPOSITE == formatSTF) {
bufferSTF[0] = INPUT_SELECT_REG;
bufferSTF[1] = 0x05;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTF,2u)) {
resultSTF = TRUE;
}
} else if (PSP_VPFE_TVP5146_FORMAT_SVIDEO == formatSTF ) {
bufferSTF[0] = INPUT_SELECT_REG;
bufferSTF[1] = 0x46;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTF,2u)) {
resultSTF = TRUE;
}
} else {
resultSTF = FALSE;
}
return(resultSTF);
}
bool ACC_init () {
uint8_t ret=0;
#if defined(TRACE)
Serial.println(">>>Start ACC_init");
#endif
i2c_writeReg(MPU6050_ADDRESS, 0x1C, 0x10); //ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0
if (ret> 0) return false;
#if defined(TRACE)
Serial.println("<<<End OK ACC_init");
#endif
return true;
}
bool Gyro_init() {
uint8_t ret=0;
#if defined(TRACE)
Serial.println(">>>Start Gyro_init");
#endif
ret=i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: (Full scale set to +/- 2000 deg/sec)
if (ret> 0) return false;
#if defined(TRACE)
Serial.println("<<<End OK Gyro_init");
#endif
return true;
}
/***************************************************************************
PRIVATE FUNCTIONS
***************************************************************************/
uint8_t lsm303_write8(byte address, byte reg, byte value)
{
int ret;
byte buf[1];
buf[0] = value;
ret = 0;
ret = i2c_writeReg(address, reg, buf, 1);
if(ret!=0)
{
#ifdef DEBUG
shift(0x7777);
_delay_ms(1000);
#endif
return 1;
}
return 0;
}
/*
* This is not necessary for the operation of the sniffer!
* It was for self test (MAKE_TWI_TRAFFIC_FOR_SELF_TEST).
* An MPU6050 was connected to the TWI interface
* and the sniffer listened to the traffic to that.
*/
void MPU6050_init(uint8_t stage) {
switch (stage) {
case 0:
i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
break;
case 1:
i2c_writeReg(MPU6050_ADDRESS, 0x68, 0x7); //PWR_MGMT_1 -- RESETS
break;
case 2:
i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
break;
case 3:
i2c_writeReg(MPU6050_ADDRESS, 0x1A, 6); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; 5Hz BW on all.
break;
case 4:
i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x10); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 1000 deg/sec
break;
}
}
// brief To set the TVP6146 Sync 656
Bool setTVP5146Sync656(Bool enable656Sync) {
Bool resultSTS = FALSE;
Uint8 output1BufferSTS[2] = {0};
Uint8 output2BufferSTS[2] = {0};
Uint8 lumProcCntrlBufferSTS[2] = {0};
Uint8 output4BufferSTS[2] = {0};
Uint8 chrProcCntrlBufferSTS[2] = {0};
Uint8 syncCntBufferSTS[2] = {0};
PAL_osWaitMsecs(100u);
output2BufferSTS[0] = OUTPUT_FORMATTER_2_REG;
output2BufferSTS[1] = 0x11;
lumProcCntrlBufferSTS[0] = LUMINANCE_PROCESSING_CONTROL_3_REG;
lumProcCntrlBufferSTS[1] = 0x00;
chrProcCntrlBufferSTS[0] = CHROMINANCE_PROCESSING_CONTROL_2_REG;
chrProcCntrlBufferSTS[1] = 0x04;
switch(enable656Sync) {
case ENABLE_SYNC_656:
output1BufferSTS[0] = OUTPUT_FORMATTER_1_REG;
output1BufferSTS[1] = 0x40;
output4BufferSTS[0] = OUTPUT_FORMATTER_4_REG;
output4BufferSTS[1] = 0xFF;
syncCntBufferSTS[0] = SYNC_CONTROL_REG;
syncCntBufferSTS[1] = 0x00;
break;
case DISABLE_SYNC_656:
output1BufferSTS[0] = OUTPUT_FORMATTER_1_REG;
output1BufferSTS[1] = 0x43;
output4BufferSTS[0] = OUTPUT_FORMATTER_4_REG;
output4BufferSTS[1] = 0xAF;
syncCntBufferSTS[0] = SYNC_CONTROL_REG;
syncCntBufferSTS[1] = 0x1E;
break;
default:
break;
}
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,output1BufferSTS,2u)) {
resultSTS = TRUE;
}
if(resultSTS == TRUE) {
resultSTS = FALSE;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,output4BufferSTS,2u)) {
resultSTS = TRUE;
}
}
if ( resultSTS == TRUE) {
resultSTS = FALSE;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,lumProcCntrlBufferSTS,2u)) {
resultSTS = TRUE;
}
}
if ( resultSTS == TRUE ) {
resultSTS = FALSE;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,chrProcCntrlBufferSTS,2u)) {
resultSTS = TRUE;
}
}
if ( resultSTS == TRUE) {
resultSTS = FALSE;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,syncCntBufferSTS,2u)) {
resultSTS = TRUE;
}
}
if ( resultSTS == TRUE) {
resultSTS = FALSE;
if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,output2BufferSTS,2u)) {
resultSTS = TRUE;
}
}
return(resultSTS);
}
bool set_sensitivity(uint8_t value) {
// Set Motion Trigger Threshold to value * 1mg
return i2c_writeReg(ACCEL_ADDR, MOT_THR, &value, 1) == 0;
}