static void ssort1(string x[], int n, int depth)
{ int a, b, c, d, r, v;
if (n <= 1)
return;
a = rand() % n;
swap(0, a);
v = i2c(0);
a = b = 1;
c = d = n-1;
for (;;) {
while (b <= c && (r = i2c(b)-v) <= 0) {
if (r == 0) { swap(a, b); a++; }
b++;
}
while (b <= c && (r = i2c(c)-v) >= 0) {
if (r == 0) { swap(c, d); d--; }
c--;
}
if (b > c) break;
swap(b, c);
b++;
c--;
}
r = min(a, b-a); vecswap(0, b-r, r, x);
r = min(d-c, n-d-1); vecswap(b, n-r, r, x);
r = b-a; ssort1(x, r, depth);
if (i2c(r) != 0)
ssort1(x + r, a + n-d-1, depth+1);
r = d-c; ssort1(x + n-r, r, depth);
}
void ssort1(char *x[], int n, int depth,int* index)
{
int a, b, c, d, r, v;
if (n <= 1)
return;
/// Choosing the pivot
//a = rand() % n;
a=0;
/// /////////////////
swap(0, a);
swapIndex(0,a);
v = i2c(0);
a = b = 1;
c = d = n-1;
for (;;)
{
while (b <= c && (r = i2c(b)-v) <= 0) {
if (r == 0) { swap(a, b); swapIndex(a,b);a++; }
b++;
}
while (b <= c && (r = i2c(c)-v) >= 0) {
if (r == 0) { swap(c, d); swapIndex(c,d);d--; }
c--;
}
if (b > c) break;
swap(b, c);
swapIndex(b,c);
b++;
c--;
}
r = min(a, b-a);
vecswap(0, b-r, r, x);
vecswapIndex(0, b-r, r, index);
r = min(d-c, n-d-1);
vecswap(b, n-r, r, x);
vecswapIndex(b, n-r, r, index);
r = b-a;
ssort1(x, r, depth,index);
if (i2c(r) != 0)
ssort1(x + r, a + n-d-1, depth+1,index+r);
r = d-c;
ssort1(x + n-r, r, depth,index+ n-r);
}
bool
i2cb(u8_t addr, u8_t wrlen, u8_t rdlen, u8_t buf[])
{
static i2c_t t = {.buf = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};
t.addr = addr;
t.wrlen = wrlen;
t.rdlen = rdlen;
t.cb = internal_cb;
memcpy(t.buf, buf, wrlen);
i2c(&t);
pending = true;
while (pending) {
if (i2c_process.needspoll) {
process_post_synch(&i2c_process, PROCESS_EVENT_POLL, NULL);
i2c_process.needspoll = false;
}
}
memcpy(buf+wrlen,t.buf+wrlen,rdlen);
return i2c_status;
}
int main_master()
{
I2C i2c(I2C_SDA, I2C_SCL);
int count = 0;
char data_write[] = "hello slave.";
char data_read[24];
while (1) {
int status = i2c.write(LM75_ADDR, data_write, sizeof(data_write), 0);
if (status != 0) { // Error
printf ( "slave not ack.\n" );
while (1) {
myled = !myled;
wait(0.2);
}
}
count = 5;
do {
status = i2c.read(LM75_ADDR, data_read, 2, 0);
if (status == 0) {
printf ( "%02X %02X\n", data_read[0], data_read[1] );
break;
}
else {
count --;
wait (0.5);
}
} while (count > 0);
myled = !myled;
wait(0.6);
}
}
int Pressure::GetPressure(){
double pressureReading=0;
I2C i2c(0, 100000);
while(1) {
uint8_t pressureData[3];
uint8_t readPressureCmd = 0xF1;
int16_t pressure = 0;
if (i2c.transaction(0x40, &readPressureCmd, 1, pressureData, 3)) {
/* Output temperature. */
pressure = (pressureData[0] << 8) | pressureData[1];
pressureReading = (pressure/240.0)*0.95;
DEBUGOUT("Pressure read over I2C is %.1f Pa\r\n", (pressure/240.0)*0.95);
}
else {
DEBUGOUT("Error reading pressure.\r\n");
}
//Sleep(500);
}
return pressureReading;
}
int compress(vector<char>& chars) {
if (chars.size() == 1) {
return 1;
}
int cnt = 1, ptr = 0;
char c = chars[0];
for (int i = 1; i < chars.size(); ++i) {
if (chars[i] == c) {
++cnt;
}
char bk = chars[i];
if (chars[i] != c || i + 1 == chars.size()) {
if (cnt == 1) {
chars[ptr] = c;
++ptr;
} else {
chars[ptr] = c;
++ptr;
vector<char> r = i2c(cnt);
for (int j = 0; j < r.size(); ++j) {
chars[j + ptr] = r[j];
}
ptr += r.size();
}
if (i + 1 == chars.size() && bk != c) {
chars[ptr] = bk;
++ptr;
}
cnt = 1;
c = bk;
}
}
chars.resize(ptr);
return ptr;
}
bool test_i2c()
{
printf("Opening I2C-2\n");
FhvRobot::I2C i2c(I2C_2);
printf("Setting slave to 0x68\n");
i2c.SetSlave(0x68);
printf("Reading value 0x00\n");
char value = i2c.ReadByte(0x00);
printf("Read value is %d\n", value);
return true;
}
//Generate a pushdown automaton that accepts by empty stack out of the CFG
FiniteAutomaton CFG::generatePDA() const {
FiniteAutomaton PDA;
State state = PDA.addState();
//for each variable A, take 'epsilon' as input and replace A by B on top of stack for each rule A -> B
for (auto var : _variables) {
for (auto rule : _rules) {
if (var == rule.first) { //If the selected variable is in the head of the selected rule
std::vector<int> topOfStack; //Strings are represented by vectors of integers
std::vector<int> newTopOfStack;
for (auto letter : rule.first.getValue()) {
topOfStack.push_back(letter);
}
if (rule.first.getSuffix() != 0)
{
topOfStack.push_back(i2c(rule.first.getSuffix()));
}
for (auto bodySymbol : rule.second) {
for (auto letter : bodySymbol.getValue()) {
newTopOfStack.push_back(letter);
}
if (bodySymbol.getSuffix() != 0)
{
newTopOfStack.push_back(i2c(bodySymbol.getSuffix()));
}
}
PDA.addArc(state, state).setSymbol(Epsilon).setRule(/*topOfStack*/ rule.first.getValue().at(0) , newTopOfStack);
}
}
}
//for each terminal a, take 'a' as input and pop the stack
for (auto ter : _terminals) {
PDA.addArc(state, state).setSymbol(ter.getValue().at(0)).setRule(ter.getValue().at(0), {Epsilon});
}
return PDA;
}
void i2cIo( uint8_t index, uint8_t address, uint8_t sendCnt, uint8_t receiveCnt, uint8_t * sendData )
{
uint8_t i;
TI2C * idc = i2c( index );
uint8_t * d = i2cSendQueue( index );
for ( i=0; i<sendCnt; i++ )
d[i] = sendData[i];
idc->address = address;
idc->sendCnt = sendCnt;
idc->receiveCnt = receiveCnt;
idc->status = I2C_IO_INVOKED;
idc->bytesWritten = 0;
idc->bytesRead = 0;
}
bool test_lidar()
{
printf("Opening I2C-2\n");
FhvRobot::I2C i2c(I2C_2);
FhvRobot::Lidar lidar(&i2c);
printf("Init lidar\n");
if (!lidar.InitLidar()) {
printf("Init lidar failed\n");
return false;
}
printf("Instantiating lidar and reading 50 values\n");
for (int i = 0; i < 100; i++) {
printf("%d. Read distance=%d\n", (i+1), lidar.ReadDistance());
usleep(1000 * 100);
}
return true;
}
void I2C_RT::write_RT(uint8_t address,uint16_t reg,bool isTwoBytes,uint8_t*data,uint8_t dataLenght){
mbed::I2C i2c(I2C_SDA,I2C_SCL);
i2c.frequency(100000);
// Some Cast
//TODO find a more proper solution
int sensorAddress = (int) address;
char dataToWrite[3] = {(char)(reg>>8),(char)reg,(char)*data};
if(isTwoBytes){
i2c.write(sensorAddress,&dataToWrite[0],3);
}
else{
i2c.write(sensorAddress,&dataToWrite[1],2);
}
}
void I2CTest::get_i2c_count(){
PeriphObject::port_t i2c_count = 0;
bool open_result = true;
do {
I2C i2c(i2c_count);
if( i2c.open() < 0 ){
open_result = false;
} else {
i2c_count++;
i2c.close();
}
} while( open_result );
m_i2c_count = i2c_count;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "i2cnode");
ros::NodeHandle nh;
ros::NodeHandle n("~");
double accRate, gyroRate, magRate, barRate, avrRate, pubRate;
ROS_INFO("fmAirframe : Reading parameters...");
n.param<double> ("accRate", accRate, 100);
n.param<double> ("gyroRate", gyroRate, 100);
n.param<double> ("magRate", magRate, 50);
n.param<double> ("barRate", barRate, 25);
n.param<double> ("avrRate", avrRate, 50);
n.param<double> ("pubRate", pubRate, 50);
estimator = new kalman(nh, n);
ROS_INFO("fmAirframe : Advertising topics...");
accPublisher = nh.advertise<fmMsgs::accelerometer>("/accData", 1);
gyroPublisher = nh.advertise<fmMsgs::gyroscope>("/gyroData", 1);
magPublisher = nh.advertise<fmMsgs::magnetometer>("/magData", 1);
statePublisher = nh.advertise<fmMsgs::airframeState>("/airframeState", 1);
radioPublisher = nh.advertise<fmMsgs::airframeControl>("/radioData", 1);
batteryPublisher = nh.advertise<fmMsgs::battery>("/batteryData", 1);
pitotPublisher = nh.advertise<fmMsgs::airSpeed>("/pitotData", 1);
rangePublisher = nh.advertise<fmMsgs::altitude>("/altData", 1);
i2cfile i2c(3);
ROS_INFO("fmAirframe : Starting hardware interfaces...");
avr myAVR(&i2c, &nh, ros::Rate(avrRate), &avrDataCallback);
adxl345 myAcc(&i2c, &nh, ros::Rate(accRate), &accDataCallback);
hmc5883l myMag(&i2c, &nh, ros::Rate(magRate), &magDataCallback);
bmp085 myBar(&i2c, &nh, ros::Rate(barRate), &barDataCallback);
itg3200 myGyro(&i2c, &nh, ros::Rate(gyroRate), &gyroDataCallback);
myAvr = &myAVR;
ROS_INFO("fmAirframe : Subscribint to topics...");
servoSubscriber = nh.subscribe("/servoData", 1, avrSetControlsCallback);
ros::Timer pub_timer = nh.createTimer(ros::Duration(1/pubRate), pubCallback);
ROS_INFO("fmAirframe : Spinning...");
ros::spin();
return 0;
}
void I2C_RT::read_RT(uint8_t address,uint16_t reg,bool isTwoBytes,uint8_t*data,uint8_t dataLenght){
mbed::I2C i2c(I2C_SDA,I2C_SCL);
i2c.frequency(100000);
// Some Cast
//TODO find a more proper solution
int sensorAddress = (int)address;
char dataToWrite[2] = {(char)(reg>>8),(char)reg};
char* readData = (char*) data;
if(isTwoBytes){
i2c.write(sensorAddress,dataToWrite,dataLenght);
}
else{
i2c.write(sensorAddress,&dataToWrite[1],dataLenght);
}
i2c.read(address,readData,dataLenght);
}
void i2cTest() {
int speed=0;
double pressureReading=0;
I2C i2c(0, 100000);
while(1) {
if((60>(pressureReading-2.5)) || 60>(pressureReading+2.5)){
if(speed<10000){
speed=speed+100;
}
}
if((60<(pressureReading-2.5)) || 60<(pressureReading+2.5)){
if(speed>500){
speed=speed-100;
}
}
printf("speed= %d\n",speed);
SetFanSpeed(speed);
uint8_t pressureData[3];
uint8_t readPressureCmd = 0xF1;
int16_t pressure = 0;
if (i2c.transaction(0x40, &readPressureCmd, 1, pressureData, 3)) {
/* Output temperature. */
pressure = (pressureData[0] << 8) | pressureData[1];
pressureReading = (pressure/240.0)*0.95;
DEBUGOUT("Pressure read over I2C is %.1f Pa\r\n", (pressure/240.0)*0.95);
}
else {
DEBUGOUT("Error reading pressure.\r\n");
}
Sleep(500);
}
}
void test()
{
I2CDevice i2c(Dev::I2C_0);
ObjMaster i2c_master(i2c,"i2c[0]");
Video::VideoControl vctrl("i2c[0]");
vctrl.stopOnExit();
ObjMaster vctrl_master(vctrl,"video");
Video::VideoConsole vcon("video");
vcon.waitOpen();
SingleMaster<Video::VideoConsole> vcon_master(Sys::GetConHost(),"!VideoConsoleMaster",vcon);
Printf(Con,"test() done\n");
Task::Sleep(10_sec);
}
void crI2c( xCoRoutineHandle xHandle,
unsigned portBASE_TYPE uxIndex )
{
static uint8_t i;
crSTART( xHandle );
for ( ;; )
{
static uint8_t init = 0;
if ( init == 0 )
{
i2cSetEn( 0, 1 );
uint8_t data[1];
data[0] = 77;
i2cIo( 0, 0, 1, 1, data );
i2cConfig( 0, 0, 123, 10000 );
init = 1;
}
TI2C * idc = i2c( uxIndex );
// Commands loop.
if ( idc->master )
{
if ( ( idc->sendCnt ) || ( idc->receiveCnt ) )
{
idc->status = I2C_BUSY;
// wait for BUSY bit to get cleared.
idc->elapsed = 0;
while ( I2C_GetFlagStatus( idc->i2c, I2C_FLAG_BUSY ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_BUSY;
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
if ( idc->sendCnt )
{
// Generate START condition on a bus.
I2C_GenerateSTART( idc->i2c, ENABLE );
idc->status = I2C_MMS;
// Wait for SB to be set
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_MODE_SELECT ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_MMS;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
// Transmit the slave address with write operation enabled.
I2C_Send7bitAddress( idc->i2c, idc->address, I2C_Direction_Transmitter );
idc->status = I2C_TMS;
// Test on ADDR flag.
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_TMS;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
// Read data from send queue.
for ( i=0; i<idc->sendCnt; i++ )
{
idc = i2c( uxIndex );
// Transmit data.
uint8_t data = idc->sendQueue[ i ];
I2C_SendData( idc->i2c, data );
// Test for TXE flag (data sent).
idc->status = I2C_MBT;
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_MBT;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
idc->bytesWritten = i+1;
}
// Wait untill BTF flag is set before generating STOP.
idc->status = I2C_BTF;
idc->elapsed = 0;
while ( !I2C_GetFlagStatus( idc->i2c, I2C_FLAG_BTF ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_BTF;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
}
// Receiving data if necessary.
if ( idc->receiveCnt )
{
I2C_AcknowledgeConfig( idc->i2c, ENABLE );
// Generate START condition if there was at least one byte written.
I2C_GenerateSTART( idc->i2c, ENABLE );
idc->status = I2C_MMS_R;
// Wait for SB to be set
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_MODE_SELECT ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_MMS_R;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
I2C_Send7bitAddress( idc->i2c, idc->address, I2C_Direction_Receiver );
// Test on ADDR Flag
idc->status = I2C_RMS;
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_RMS;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
// Receiving a number of bytes from slave.
for ( i=0; i<idc->receiveCnt; i++ )
{
// Turn acknowledge off when reading the last byte.
if ( i == (idc->receiveCnt-1) )
I2C_AcknowledgeConfig( idc->i2c, DISABLE );
// Wait for data available.
idc->status = I2C_MBR;
idc->elapsed = 0;
while ( !I2C_CheckEvent( idc->i2c, I2C_EVENT_MASTER_BYTE_RECEIVED ) )
{
if ( idc->elapsed++ > idc->timeout )
{
idc->status = I2C_ERROR_MBR;
I2C_GenerateSTOP( idc->i2c, ENABLE );
goto i2c_end;
}
crDELAY( xHandle, 1 );
idc = i2c( uxIndex );
}
// Read the data.
uint8_t data = I2C_ReceiveData( idc->i2c );
idc->receiveQueue[i] = data;
idc->bytesRead = i+1;
}
}
// Generating STOP.
I2C_GenerateSTOP( idc->i2c, ENABLE );
// Idle status when finished in regular way.
idc->status = I2C_IDLE;
}
}
else // master.
{
// slave mode IO.
//...... implementation.....
// Idle status when finished in regular way.
//idc->status = I2C_IDLE;
crDELAY( xHandle, 1 );
}
i2c_end:
// To prevent cyclic writes of zero data.
idc->sendCnt = 0;
idc->receiveCnt = 0;
// Give other coroutines time for running.
crDELAY( xHandle, 1 );
}
crEND();
}
bool I2CTest::execute_class_api_case(){
bool result = true;
get_i2c_count();
print_case_message("Board has %d I2C", m_i2c_count);
I2CAttr i2c_attr;
i2c_attr.set_slave_addr(2);
mcu_pin_t i2c1_scl;
mcu_pin_t i2c1_sda;
mcu_pin_t i2c2_scl;
mcu_pin_t i2c2_sda;
mcu_pin_t temp_pin;
u16 slave_address_16 = 0x4c4c;
i2c1_scl.port = i2c1_scl_port;
i2c1_scl.pin = i2c1_scl_pin;
i2c1_sda.port = i2c1_sda_port;
i2c1_sda.pin = i2c1_sda_pin;
i2c2_scl.port = i2c2_scl_port;
i2c2_scl.pin = i2c2_scl_pin;
i2c2_sda.port = i2c2_sda_port ;
i2c2_sda.pin = i2c2_sda_pin;
i2c_attr.set_scl(i2c2_scl);
temp_pin = i2c_attr.scl();
if(temp_pin.port!=i2c2_scl.port || temp_pin.pin!=i2c2_scl.pin){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
result = false;
}
i2c_attr.set_sda(i2c2_sda);
temp_pin = i2c_attr.sda();
if(temp_pin.port!=i2c2_sda.port || temp_pin.pin!=i2c2_sda.pin){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
result = false;
}
i2c_attr.set_slave_addr(slave_address_8);
if(i2c_attr.slave_addr()!=slave_address_8){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
result = false;
}
i2c_attr.set_slave_addr16(slave_address_16);
if(i2c_attr.slave_addr16() !=slave_address_16){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
result = false;
}
for(I2C::port_t count = 0; count < m_i2c_count; count++){
I2C i2c(count);
if( execute_i2c_api_case(i2c) == false ){
result = false;
}
}
I2C i2c_master(0);
I2C i2c_slave(1);
if( i2c_master.open(I2C::RDWR|I2C::NONBLOCK) < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
} else {
if(i2c_slave.open(I2C::RDWR|I2C::NONBLOCK)){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}else{
i2c_attr.set_scl(i2c1_scl);
i2c_attr.set_sda(i2c1_sda);
i2c_attr.set_flags(I2C_FLAG_SET_MASTER);
i2c_attr.set_freq(100000);
if(i2c_master.set_attributes(i2c_attr)!=0){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}
i2c_attr.set_flags(I2C_FLAG_SET_SLAVE);
i2c_attr.set_freq(100000);
i2c_attr.set_scl(i2c2_scl);
i2c_attr.set_sda(i2c2_sda);
i2c_attr.set_slave_addr(slave_address_8);
if(i2c_slave.set_attributes(i2c_attr)){
print_case_message("Failed %s %d", __FILE__, __LINE__);
result = false;
}
char recv_buff[sizeof(data)];
Aio aio_r(recv_buff, sizeof(data)); //aio uses buf as it's data
Aio aio_t(&data, sizeof(data)); //aio uses buf as it's data
i2c_slave.read(aio_r);
i2c_master.prepare(slave_address_8, I2C::FLAG_PREPARE_DATA);
i2c_master.write(aio_t);
while( !aio_r.is_done()){
Timer::wait_msec(5); //wait for the operation to complete
}
while( !aio_t.is_done()){
Timer::wait_msec(5); //wait for the operation to complete
}
if(memcmp(&data,recv_buff,sizeof(data))){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
print_case_message("recv %s ", recv_buff);
result = false;
}
if( i2c_slave.close() < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}
}
if( i2c_master.close() < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}
}
/* if( i2c_master.open(I2C::RDWR|I2C::NONBLOCK) < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
} else {
i2c_slave.open(I2C::RDWR|I2C::NONBLOCK);
i2c_attr.set_scl(i2c2_scl);
i2c_attr.set_sda(i2c2_sda);
i2c_attr.set_slave_addr(slave_address_8);
i2c_attr.set_flags(I2C_FLAG_SET_MASTER);
i2c_attr.set_freq(100000);
i2c_master.set_attr(i2c_attr);
i2c_attr.set_flags(I2C_FLAG_SET_SLAVE);
i2c_attr.set_freq(100000);
i2c_attr.set_scl(i2c3_scl);
i2c_attr.set_sda(i2c3_sda);
i2c_attr.set_slave_addr(slave_address_8);
i2c_slave.set_attr(i2c_attr);
//char text[] = "hello_two";
char messege_text[] = "i2c_test";
char recv_buff[sizeof(messege_text)];
while(1){
i2c_master.write(messege_text,2);
Timer::wait_milliseconds(50); //wait for the operation to complete
}
Aio aio_r(recv_buff, sizeof(messege_text)); //aio uses buf as it's data
Aio aio_t(messege_text, sizeof(messege_text)); //aio uses buf as it's data
i2c_slave.read(aio_r);
i2c_master.prepare(slave_address_8, I2C::FLAG_PREPARE_DATA);
i2c_master.write(aio_t);
while( !aio_r.is_done()){
Timer::wait_milliseconds(5); //wait for the operation to complete
}
while( !aio_t.is_done()){
Timer::wait_milliseconds(5); //wait for the operation to complete
}
if(memcmp(messege_text,recv_buff,sizeof(messege_text))){
print_case_message("Failed %s %d:", __FILE__, __LINE__);
print_case_message("recv %s ", recv_buff);
result = false;
}
Timer::wait_milliseconds(100);
if( i2c_master.close() < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}
if( i2c_slave.close() < 0 ){
print_case_message("Failed %s %d: port:%d", __FILE__, __LINE__, i2c_master.port());
result = false;
}
}*/
return result;
}
uint8_t i2cBytesWritten( uint8_t index )
{
TI2C * idc = i2c( index );
return idc->bytesWritten;
}
bool test_mpu9150()
{
printf("Opening I2C-2\n");
FhvRobot::I2C i2c(I2C_2);
printf("Init MPU9150\n");
FhvRobot::MPU9150 m(&i2c);
FhvRobot::FusionFilter filter;
printf("Checking connection (whoami)\n");
printf("%d\n", m.WhoAmI());
m.Sleep(false);
float calib[3] = { 0 };
m.GetCompassCalibration(calib);
printf("Reading 1000 values\n");
short comp_x, comp_y, comp_z;
float mx, my, mz;
float magbias[3] = { 0 };
for (int i = 0; i < 1000; i++) {
if( m.DataReady() )
{
int temp = m.GetTemp();
double t = (temp + 11900.0) / 340.0;
(void)(t);
// printf("%d. Temperature is \t\t\t\t %f°C\n", (i+1), t);
float accel_x = m.GetAccelerometerX() / 16384.0; // * G; // * A_GAIN;
float accel_y = m.GetAccelerometerY() / 16384.0; // * G; // * A_GAIN;
float accel_z = m.GetAccelerometerZ() / 16384.0; // * G; // * A_GAIN;
// printf("%d. Accel data:\n x=%f | y=%f | z=%f\n", (i+1), accel_x, accel_y, accel_z);
float gyro_x = m.GetGyroscopeX() / 131.0;
float gyro_y = m.GetGyroscopeY() / 131.0;
float gyro_z = m.GetGyroscopeZ() / 131.0;
// printf("%d. Gyro data:\n x=%f | y=%f | z=%f\n", (i+1), gyro_x, gyro_y, gyro_z);
//magbias[0] = -5.; // User environmental x-axis correction in milliGauss
//magbias[1] = -95.; // User environmental y-axis correction in milliGauss
//magbias[2] = -260.; // User environmental z-axis correction in milliGauss
if (m.GetCompass(&comp_x, &comp_y, &comp_z))
{
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)comp_x*m.getMRes()*calib[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)comp_y*m.getMRes()*calib[1] - magbias[1];
mz = (float)comp_z*m.getMRes()*calib[2] - magbias[2];
// printf("%d. Comp data:\n x=%d | y=%d | z=%d\n", (i+1), comp_x, comp_y, comp_z);
// printf("%d. Comp data:\n x=%f | y=%f | z=%f\n", (i+1), mx, my, mz);
}
filter.UpdateValues(accel_x, accel_y, accel_z, gyro_x*PI/180.0f, gyro_y*PI/180.0f, gyro_z*PI/180.0f, my, mx, mz);
float roll, pitch, yaw;
filter.ReadValues(&roll, &pitch, &yaw);
printf("{ %f , %f , %f },\t\t // %f %f %f \n\r", yaw, pitch, roll, mx, my, mz);
}
usleep( 1000 );
}
return true;
}
uint8_t i2cBytesRead( uint8_t index )
{
TI2C * idc = i2c( index );
return idc->bytesRead;
}
void i2cIrqHandler( uint8_t index )
{
TI2C * idc = i2c( index );
uint32_t reason = I2C_GetLastEvent( idc->i2c );
switch ( reason )
{
case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED:
idc->bytesRead = 0;
idc->status = I2C_SRAM;
I2C_ClearFlag( idc->i2c, I2C_FLAG_ADDR );
break;
case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED:
idc->bytesWritten = 0;
idc->slaveStopped = 0;
idc->status = I2C_STAM;
I2C_SendData( idc->i2c, idc->sendQueue[ idc->bytesWritten++ ] );
break;
case I2C_EVENT_SLAVE_BYTE_RECEIVED:
idc->receiveQueue[ idc->bytesRead++ ] = I2C_ReceiveData( idc->i2c );
idc->bytesRead %= idc->receiveCnt;
idc->status = I2C_SBR;
break;
case I2C_EVENT_SLAVE_BYTE_TRANSMITTED:
// Second condition makes sense only when there is just one byte to send.
// And it is already sent in I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED case.
if ( ( !idc->slaveStopped ) && ( idc->bytesWritten < idc->sendCnt ) )
{
I2C_SendData( idc->i2c, idc->sendQueue[ idc->bytesWritten++ ] );
idc->bytesWritten %= idc->sendCnt;
idc->status = I2C_SBT;
}
break;
case I2C_EVENT_SLAVE_ACK_FAILURE:
idc->slaveStopped = 1;
idc->status = I2C_SAF;
I2C_ClearFlag( idc->i2c, I2C_FLAG_AF );
break;
case I2C_EVENT_SLAVE_STOP_DETECTED:
idc->slaveStopped = 1;
idc->status = I2C_SSD;
//if(I2C_GetFlagStatus( idc->i2c, I2C_FLAG_ADDR ) == SET )
// I2C_ClearFlag( idc->i2c, I2C_FLAG_ADDR );
//if(I2C_GetFlagStatus( idc->i2c, I2C_FLAG_STOPF ) == SET )
// I2C_ClearFlag( idc->i2c, I2C_FLAG_STOPF );
// This causes also infinite invocation.
I2C_ClearFlag( idc->i2c, I2C_FLAG_STOPF );
volatile uint32_t temp;
temp=idc->i2c->SR1;
idc->i2c->CR1 |= 0x1;
//I2C_ClearFlag( idc->i2c, 0x00FFFFFF );
break;
default:
I2C_ClearFlag( idc->i2c, 0x00FFFFFF );
break;
}
//I2C_CleanADDRandSTOPF();
}
void I2C1_ER_IRQHandler(void)
{
TI2C * idc = i2c( 0 );
uint32_t reason = I2C_GetLastEvent( idc->i2c );
reason = reason;
}
i2c_msg[1].buf=&i2c_data[1];
i2c_priv_data.msgs=i2c_msg;
i2c_priv_data.number=2;
if (rt_device_open(device, 0) == RT_EOK)
{
rt_device_control(device, RT_I2C_DEV_CTRL_RW, &i2c_priv_data);
i2c_dbg("read msg addr=0x%x, len=%d, addr=0x%d, data=0x%x\n",i2c_msg[0].addr, i2c_msg[0].len, i2c_data[0], i2c_data[1]);
rt_device_close(device);
}
else
{
i2c_err("open:r err\n");
goto l_err;
}
}
l_err:
return ret;
}
#ifdef RT_USING_FINSH
#include <finsh.h>
#include <rtdevice.h>
FINSH_FUNCTION_EXPORT(i2c, i2c(W[0]/R[1] Addr reg data))
#endif
static void
acc_startsample(bool status)
{ /* this callback is *not* called in irq-context */
if (acc_active) i2c(&acc_transaction);
sensors_changed(&acc_sensor);
}
