Go Down

Topic: Problem with transmitting IMU/GPS/Ultrasonic data over Nrf24L01 (Read 5 times) previous topic - next topic

Cyric

#60
Feb 01, 2013, 04:02 pm Last Edit: Feb 01, 2013, 04:03 pm by Cyric Reason: 1
Part (1)of(2)
Code: [Select]

#include <NewPing.h>
#include <Arduino.h>
#include <Wire.h>
#include <stdio.h>
#include <nRF24L01.h>
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include <nmea.h>
#include <PString.h>
#include <string.h>
//-+-+-+>>>> START Ultrasonic Sensor section<<<<<<-+-+-+
#define TRIGGER_PIN  23  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     22  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
//-+-+-+>>>> END Ultrasonic Sensor section<<<<<<-+-+-+
//-+-+-+>>>> START GPS Vars section<<<<<<-+-+-+
#define WPT_IN_RANGE_M 1
//-+-+-+>>>> END GPS Vars section<<<<<<-+-+-+
//-+-+-+>>>> SRTART POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
// LSM303 accelerometer: 8 g sensitivity
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer
#define ToRad(x) ((x)*0.01745329252)  // *pi/180
#define ToDeg(x) ((x)*57.2957795131)  // *180/pi
// L3G4200D gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
// LSM303 magnetometer calibration constants; use the Calibrate example from
// the Pololu LSM303 library to find the right values for your board
#define M_X_MIN -602
#define M_Y_MIN -668
#define M_Z_MIN -592
#define M_X_MAX 391
#define M_Y_MAX 422
#define M_Z_MAX 524
#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002
//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1
//#define PRINT_DCM 0     //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw
#define STATUS_LED 13
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> SRTART POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
int SENSOR_SIGN[9] = {
 1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible
long timer=0;   //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={
 0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;
float Accel_Vector[3]= {
 0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {
 0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {
 0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {
 0,0,0};//Omega Proportional correction
float Omega_I[3]= {
 0,0,0};//Omega Integrator
float Omega[3]= {
 0,0,0};
// Euler angles
float roll;
float pitch;
float yaw;
float errorRollPitch[3]= {
 0,0,0};
float errorYaw[3]= {
 0,0,0};
unsigned int counter=0;
byte gyro_sat=0;
float DCM_Matrix[3][3]= {
 {
   1,0,0                                  }
 ,{
   0,1,0                                  }
 ,{
   0,0,1                                  }
};
float Update_Matrix[3][3]={
 {
   0,1,2                                }
 ,{
   3,4,5                                }
 ,{
   6,7,8                                }
}; //Gyros here
float Temporary_Matrix[3][3]={
 {
   0,0,0                                  }
 ,{
   0,0,0                                  }
 ,{
   0,0,0                                  }
};
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> START Ultrasonic Sensor section<<<<<<-+-+-+
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance, centerDistance; //distances on either side
//-+-+-+>>>> END Ultrasonic Sensor section<<<<<<-+-+-+
//-+-+-+>>>> Start US PING averaging section<<<<<<-+-+-+
//------------------Avrageing-----------------------------------
const int numReadings = 5;
int readings[numReadings];      // the readings from the analog input
int index = 0;                  // the index of the current reading
int total = 0;                  // the running total
int average = 0;                // the average
//------------------Avrageing-----------------------------------
//-+-+-+>>>> END US PING averaging section<<<<<<-+-+-+
//-+-+-+>>>> START GPS INIT section<<<<<<-+-+-+
NMEA gps(ALL);    // GPS data connection to all sentence types
//-+-+-+>>>> END GPS INIT section<<<<<<-+-+-+
//-+-+-+>>>> Start VT100 INIT section<<<<<<-+-+-+
String OFF = "\033[0m";
String BOLD = "\033[1m";
String CLR = "\e[2J";  
String CLRl = "\e[2K";
String BYB = "\e[1;33;40m";
//-+-+-+>>>> END VT100 INIT section<<<<<<-+-+-+

RF24 radio(48,49);
void setup()
{
 Serial.begin(57600);
 Serial2.begin(57600); //GPS INPUT (LocoSYS LS20031)
 //**************Start Transiever (NRF24L01) config**************
 printf_begin();
 printf("\n\rRadio Setup\n\r");
 radio.begin();
 radio.setDataRate(RF24_2MBPS);
 radio.setCRCLength(RF24_CRC_16);
 radio.setPayloadSize(12 * sizeof(byte));
 radio.setChannel(2);
 radio.setAutoAck(true);
 radio.openReadingPipe(1,0xF0F0F0F0E1LL);
 radio.openWritingPipe(0xF0F0F0F0D2LL);
 radio.startListening();
 radio.printDetails();
 //**************End Transiever (NRF24L01) config**************
 //-+-+-+>>>> Start US PING averaging section<<<<<<-+-+-+
 for (int thisReading = 0; thisReading < numReadings; thisReading++)
   readings[thisReading] = 0;        
 //-+-+-+>>>> END US PING averaging section<<<<<<-+-+-+
 //-+-+-+>>>> Start VT100 INIT section<<<<<<-+-+-+  
 // Serial.print(BYB);
 //  Serial.println(CLR);  
 //-+-+-+>>>> END VT100 INIT section<<<<<<-+-+-+
 //-+-+-+>>>> START POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
 Serial.println("IMU INIT ... "); // Print some stuff to serial
 I2C_Init();
 delay(1500);
 Accel_Init();
 Compass_Init();
 Gyro_Init();
 delay(20);
 for(int i=0;i<32;i++)    // We take some readings...
 {
   Read_Gyro();
   Read_Accel();
   for(int y=0; y<6; y++)   // Cumulate values
     AN_OFFSET[y] += AN[y];
   delay(20);
 }
 for(int y=0; y<6; y++)
   AN_OFFSET[y] = AN_OFFSET[y]/32;
 AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
 for(int y=0; y<6; y++)
   delay(2000);  
 timer=millis();
 delay(20);
 counter=0;
 Serial.println("IMU Stream active ... "); // Print some stuff to serial
 //-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
}

struct StructBuff_t {
 char GPS[100];  
 char imuRoll[6];
 char imuPitch[6];
 char imuYaw[6];
 char AP[3];
 char SenBuffersAll[128];
}
StructBuff;
//**************************************************
//**************************************************
//************* Start Main Loop section ************
//**************************************************
//**************************************************

Cyric

Part(2) of (2)
Code: [Select]

void loop()
{
 MinImu9();
 StuffStructBuff(ToDeg(roll),ToDeg(pitch),ToDeg(yaw),averagePing(),*gps.sentence());  
 BuildTransBuffer();
 //  TransSenBuffers();
}
//**************************************************
//**************************************************
//************* END Main Loop section***************
//**************************************************
//**************************************************
//-+-+-+>>>> Start Buffer Transmit section<<<<<<-+-+-+
/*
void DisplayGPSBuffer(){
for (int i=0; i < sizeof(StructBuff.GPS);i++){
Serial.print(StructBuff.GPS[i]);
}  
Serial.println("GPS buffer");
}
*/
void StuffStructBuff(float inRoll, float inPitch, float inYaw, unsigned int inAP, char inGPS){//**Working**
 //////////////////////////////////////////////////////////////////////////////////////////////////
 //**Working**Collect data and fill StructBuff elements                                                     //
 // Struct your Buffers here                                                         //+1 for \0 //
 PString(StructBuff.GPS, sizeof(StructBuff.GPS), inGPS);                 //+100 Buff //  
 //----------------------Delimitor------------------------------------------                     //
 PString(StructBuff.imuRoll, sizeof(StructBuff.imuRoll), inRoll);            //+6   Buff //
 //----------------------Seperator------------------------------------------                     //
 PString(StructBuff.imuPitch, sizeof(StructBuff.imuPitch), inPitch);
 //----------------------Seperator------------------------------------------                     //
 PString(StructBuff.imuYaw, sizeof(StructBuff.imuYaw), inYaw);               //+6   Buff //
 //----------------------Delimitor------------------------------------------                     //
 PString(StructBuff.AP, sizeof(StructBuff.AP), inAP);
 //----------------------Delimitor------------------------------------------         //+12  PrSu //
 //////////////////////////////////////////////////////////////////////////////////////Buffer = 134  
 ///Spew to local serial
 //for (int i = 0; i < sizeof(StructBuff.imuRoll); i++){
 //        Serial.print(StructBuff.imuRoll[i]);
 //      }  
 //      Serial.println();
 //////////////////////////////////////////////////////////////////////////////////////  
}
void BuildTransBuffer(){   //***NOT WORKING?***
 ////////////////////////////////////////////////////////////////////////////////////
 //***NOT WORKING?*** format the string for transmit array.
 //<<<|$GPRMC,XXXXXX.00,A,4454.1740,N,XXXXX.0143,W,000.0,128.7,300508,001.1,E,A*2E|-3.87^34.01^0.13|33|>>>  
 //Start string append
 strlcat(StructBuff.SenBuffersAll,"<<<|",sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,StructBuff.GPS,sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,"|",sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,StructBuff.imuRoll,sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,",",sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,StructBuff.imuPitch,sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,",",sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,StructBuff.imuYaw,sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,"|",sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,StructBuff.AP,sizeof(StructBuff.SenBuffersAll));
 strlcat(StructBuff.SenBuffersAll,"|>>>",sizeof(StructBuff.SenBuffersAll));
 //End string append
 //Formatting done.
 /////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///Spew StructBuff.SenBuffersAll to local serial
 for (int i = 0; i < sizeof(StructBuff.SenBuffersAll); i++){
   Serial.print(StructBuff.SenBuffersAll[i]);
 }  
 Serial.println();
 ///////////////////////////////////
}
void TransSenBuffers(){
 radio.stopListening();
 bool ok = radio.write(StructBuff.SenBuffersAll,sizeof(StructBuff.SenBuffersAll));
 if (ok)
   printf("ok...Tflow2byte12\n\r");
 else  
   printf("failed.Tflow2byte12\n\r");
 delay(10);
}
//-+-+-+>>>> END Buffer Transmit section<<<<<<-+-+-+


//-+-+-+>>>> Start Type conversion section<<<<<<-+-+-+
void Tflo2byteIMU(float inRoll, float inPitch, float inYaw){

 union inRoll2byte_t {
   byte  inRollout[4];
   float inRollin;
 }
 inRoll2byte;
 inRoll2byte.inRollin = inRoll;

 union inPitch2byte_t {
   byte  inPitchout[4];
   float inPitchin;
 }
 inPitch2byte;
 inPitch2byte.inPitchin = inPitch;

 union inYaw2byte_t {
   byte  inYawout[4];
   float inYawin;
 }
 inYaw2byte;
 inYaw2byte.inYawin = inYaw;

 byte output[12];
 output[0] = inRoll2byte.inRollout[0];
 output[1] = inRoll2byte.inRollout[1];  
 output[2] = inRoll2byte.inRollout[2];
 output[3] = inRoll2byte.inRollout[3];

 output[4] = inPitch2byte.inPitchout[0];
 output[5] = inPitch2byte.inPitchout[1];  
 output[6] = inPitch2byte.inPitchout[2];
 output[7] = inPitch2byte.inPitchout[3];

 output[8] = inYaw2byte.inYawout[0];
 output[9] = inYaw2byte.inYawout[1];  
 output[10] = inYaw2byte.inYawout[2];
 output[11] = inYaw2byte.inYawout[3];
 radio.stopListening();
 bool ok = radio.write(output,sizeof(output));
 if (ok)
   printf("ok...Tflow2byteIMU\n\r");
 else  
   printf("failed.Tflow2byteIMU\n\r");
 delay(10);
}
//-+-+-+>>>> End Type conversion section<<<<<<-+-+-+
//-+-+-+>>>> START Sensor readings section<<<<<<-+-+-+
void PrnGpsNmea() {
 if (Serial2.available() > 0 ) {
   // read incoming character from GPS and feed it to NMEA type object
   if (gps.decode(Serial2.read())) {
     Serial.println (gps.sentence());
   }
 }
}
unsigned int  ping(){
 delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
 unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
 return uS / US_ROUNDTRIP_CM;
}
unsigned int averagePing(){
 total= total - readings[index]; // subtract the last reading:
 readings[index] = ping();   // read from the sensor:  
 total= total + readings[index];   // add the reading to the total:
 index = index + 1;  // advance to the next position in the array:  
 if (index >= numReadings)  // if we're at the end of the array...
   index = 0;   // ...wrap around to the beginning:                          
 average = total / numReadings;   // calculate the average:
 delay(1);        // delay in between reads for stability            
 return average;   // send it as ASCII digits
}
unsigned int PingScan(unsigned int Fwddist){
}  
//-+-+-+>>>>START POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
void MinImu9() //PoloLU MINIMU -9DOF
{
 if((millis()-timer)>=20)  // Main loop runs at 50Hz
 {
   counter++;
   timer_old = timer;
   timer=millis();
   if (timer>timer_old)
     G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
   else
     G_Dt = 0;
   // *** DCM algorithm
   // Data adquisition
   Read_Gyro();   // This read gyro data
   Read_Accel();     // Read I2C accelerometer
   if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
   {
     counter=0;
     Read_Compass();    // Read I2C magnetometer
     Compass_Heading(); // Calculate magnetic heading  
   }
   // Calculations...
   Matrix_update();
   Normalize();
   Drift_correction();
   Euler_angles();
   // ***
   //   VTprintdata();
   // printdata();
 }
}
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> END Sensor readings section<<<<<<-+-+-+


PaulS

Code: [Select]
struct StructBuff_t {
  char GPS[100]; 
  char imuRoll[6];
  char imuPitch[6];
  char imuYaw[6];
  char AP[3];
  char SenBuffersAll[128];
}
StructBuff;

This is STILL wrong. Until you fix it, I can't help you.

You either store the GPS data as character data and the other stuff as floats (or strings) in the struct, OR you store all the data in and array.

What you have now is a box of stuff to send. Then, you make a copy of the stuff to send, and put the copy in an envelope. Then, you throw the envelope in the box, and send the box. That hardly makes sense.

Cyric


Code: [Select]
struct StructBuff_t {
  char GPS[100]; 
  char imuRoll[6];
  char imuPitch[6];
  char imuYaw[6];
  char AP[3];
  char SenBuffersAll[128];
}
StructBuff;

This is STILL wrong. Until you fix it, I can't help you.

You either store the GPS data as character data and the other stuff as floats (or strings) in the struct, OR you store all the data in and array.

What you have now is a box of stuff to send. Then, you make a copy of the stuff to send, and put the copy in an envelope. Then, you throw the envelope in the box, and send the box. That hardly makes sense.

Ok my understanding was collect all the data in each of there native types chars/floats use PStrings to convert and stuff them into there char type arrays so they can now be use and referenced as char strings collect the char strings up into one and offload it. your saying i have extra steps in there ?
Code: [Select]
struct StructBuff_t { // Global struct
  char GPS[100];   // being filled with PString(StructBuff.GPS, sizeof(StructBuff.GPS), inGPS);
  char imuRoll[6];  // being filled with PString(StructBuff.imuRoll, sizeof(StructBuff.imuRoll), inRoll); // Was a float now a char stings in a char array yes no?
  char imuPitch[6]; // being filled with PString(StructBuff.imuPitch, sizeof(StructBuff.imuPitch), inPitch); "                                                                              "
  char imuYaw[6];  // being filled with   PString(StructBuff.imuYaw, sizeof(StructBuff.imuYaw), inYaw); "                                                                                    "
  char AP[3];  // being filled with   PString(StructBuff.AP, sizeof(StructBuff.AP), inAP);  was an int now a char yes no?     
  char SenBuffersAll[128]; // being filled with strlcat after pulling the newly filled char arrays with sensor data in .GPS .imuRoll etc?
}
StructBuff;

use radio.write StructBuff.SenBuffersAll with completed formatted string?
if not show me where i am going wrong. because that looks like 1234 rinse and repeat to me. (1).get data bits (2).store data bits (3).combine each data bit into data string (4).send datastring

PaulS

Quote
Ok my understanding was collect all the data in each of there native types chars/floats

Correct.

Quote
use PStrings to convert and stuff them into there char type arrays so they can now be use

Not necessary, but, if you have the resources, it's not the end of the world.

Quote
and referenced as char strings collect the char strings up into one and offload it. your saying i have extra steps in there ?

You are copying data from each of the small(er) arrays into one larger array, and storing the larger array in the same box with the smaller arrays.

If you are then going to send only what is in the envelope (the larger array), rather than what is in the box (the struct), there is no reason to have a box or to store the envelope in the box.

A more straight-forward setup would have the box (the struct) containing the floats and char array (the GPS data), and send the box, with no PString or String manipulation (copy into envelope) required.

Go Up