This is my maxbotix sonar to I2c recoded source code with i2c communication.
This code work in all MultiWii versions
Code: Select all
/* ================================================================================================
// SONAR I2C SENSOR FOR MULTIWII based on ATMEGA 168
// developed by Michal Maslik(MichalM_sk) in 2011
// recoded for Arduino in 2012
//
// Copyright (C) 2012, Michal Maslik, mobicek@gmail.com
// ONLY FOR NON-COMMERCIAL USAGE
//
// THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE COMMONS PUBLIC LICENSE
// ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY COPYRIGHT AND/OR OTHER APPLICABLE LAW.
// ANY USE OF THE WORK OTHER THAN AS AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
//
// BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO BE BOUND BY THE
// TERMS OF THIS LICENSE. TO THE EXTENT THIS LICENSE MAY BE CONSIDERED TO BE A CONTRACT,
// THE LICENSOR GRANTS YOU THE RIGHTS CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF
// SUCH TERMS AND CONDITIONS.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
// ================================================================================================*/
#include <stdio.h>
#include <math.h>
#include <util/delay.h>
#define LED_pin 13
#define TWIS_ReadBytes 0x60
#define TWIS_WriteBytes 0xA8
#define TWI_NO_STATE 0xF8
#define TWI_BUS_ERROR 0x00
#define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; //SDA & SCL
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5);
uint8_t TWIS_ResponseType;
struct I2cPart { // 2 bytes
uint8_t NewData;
uint8_t Alt;
};
union DataBlock {
struct I2cPart I2cData;
uint8_t bytes[2];
};
DataBlock dataBlock, dataBlock2;
char DataIsLocked;
static uint8_t newDataFlag = 0;
static uint8_t sonarReaded = 0;
//maxbotix range finder
uint8_t SonarValue = 0;
uint8_t buff;
char first_char;
//I2c routines ///////////////////////////////////////////////////////////////
void TWIS_Init (uint8_t Address, uint32_t Bitrate){
I2C_PULLUPS_DISABLE;
TWBR = ((16000000L/Bitrate)-16)/2;
TWAR = (Address << 1);
TWCR = (1<<TWEN) | (1<<TWEA) | (1<<TWIE) | (1<<TWSTO);
}
void TWIS_Stop (void){
TWCR = (1<<TWSTO) | (1<<TWINT) | (0<<TWWC) | (1<<TWEN) | (1<<TWEA) | (1<<TWIE);
}
void TWIS_Write (uint8_t b){
TWDR = b;
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA)|(1<<TWSTO)|(1<<TWSTA);
while (!(TWCR & (1<<TWINT)));
}
uint8_t TWIS_ResponseRequired (uint8_t *TWI_ResponseType){
*TWI_ResponseType = TWSR;
return TWCR & (1<<TWINT);
}
////////////////////////////////////////////////////////////////////
void lock_out_data()
{
for (int i=0; i<2; i++)
dataBlock2.bytes[i] = dataBlock.bytes[i];
DataIsLocked = true;
}
void setup() {
sei(); // enable all interupts
delay(10);
TWIS_Init(8,400000L); // I2c slave address - 8 , 400Khz I2c frequency
delay(10);
pinMode(LED_pin, OUTPUT);
delay(20);
DataIsLocked = false;
dataBlock.I2cData.NewData = 0;
dataBlock.I2cData.Alt = 0;
Serial.begin(9600);
digitalWrite(LED_pin, HIGH);
delay(100);
}
void loop() {
//read MaxBotix LV sonar sensor
if(Serial.available() > 0){
first_char = Serial.read(); // reading char
//if statement is used to parse the incomming maxbotix serial data
if(first_char == 'R'){
lock_out_data();
SonarValue = 0;
buff = Serial.read() - '0';
SonarValue += buff*100;
buff = Serial.read() - '0';
SonarValue += buff*10;
buff = Serial.read() - '0';
SonarValue += buff;
sonarReaded = 1;
}
Serial.flush();
}
if(sonarReaded == 1){
newDataFlag++;
if(newDataFlag >=60)
newDataFlag = 1;
dataBlock.I2cData.Alt = SonarValue;
dataBlock.I2cData.NewData = newDataFlag;
sonarReaded = 0;
DataIsLocked = false;
}
}
// I2c interrupt ---------------------------------------------------------------------------------------------------
SIGNAL(SIG_2WIRE_SERIAL)
{
if (TWIS_ResponseRequired (&TWIS_ResponseType)){
switch (TWIS_ResponseType){
case TWIS_ReadBytes:
TWIS_Stop ();
break;
case TWIS_WriteBytes:
for (int i=0;i<2;i++){
if(DataIsLocked)
TWIS_Write (dataBlock2.bytes[i]);
else
TWIS_Write (dataBlock.bytes[i]);
}
TWIS_Stop ();
break;
case TWI_BUS_ERROR:
TWIS_Stop ();
break;
}
}
}
MultiWii Master part
Code: Select all
struct RfI2cPart {
uint8_t NewData;
uint8_t Alt; //in inches
};
union RfDataUnion {
struct RfI2cPart RfI2cData;
uint8_t bytes[2];
} rfBlock;
//I2C comunication speed changed to 400Khz
TWBR = ((16000000L / 400000L) - 16) / 2;
i2c_rep_start((8<<1) + 1);
rfBlock.bytes[0]= i2c_readAck();
rfBlock.bytes[1]= i2c_readNak();
