fork(13) download
copy 
  1.  
  2.  
  3. //This code is based on protions of code from:
  4. //Pololu's miniahrs system
  5. //adafruit's GPS RMC test sketch
  6. //below are the liscenses 
  7. /*
  8.  MinIMU-9-Arduino-AHRS
  9.  Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
  10.  
  11.  Copyright (c) 2011 Pololu Corporation.
  12.  http://w...content-available-to-author-only...u.com/
  13.  
  14.  MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
  15.  http://code.google.com/p/sf9domahrs/
  16.  
  17.  sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
  18.  Julio and Doug Weibel:
  19.  http://code.google.com/p/ardu-imu/
  20.  
  21.  MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
  22.  under the terms of the GNU Lesser General Public License as published by the
  23.  Free Software Foundation, either version 3 of the License, or (at your option)
  24.  any later version.
  25.  
  26.  MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
  27.  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  28.  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
  29.  more details.
  30.  
  31.  You should have received a copy of the GNU Lesser General Public License along
  32.  with MinIMU-9-Arduino-AHRS. If not, see <http://w...content-available-to-author-only...u.org/licenses/>.
  33.  
  34.  */
  35. // A simple sketch to read GPS data and parse the $GPRMC string 
  36. // see http://w...content-available-to-author-only...a.net/make/gpsshield for more info
  37.  
  38.  
  39. //includes
  40. #include <SoftwareSerial.h>
  41. #include <PString.h>//for pstrings
  42. #include <Wire.h>//I2C
  43. #include <L3G4200D.h>//mag gyro acc
  44. #include <LSM303.h>
  45. #include <SD.h>
  46.  
  47. //------------------------------------
  48. //pins---------------------
  49. //------------------------------------
  50. #define chipSelect  10// pin to enable SD card
  51. //---------------NOTE-------------------------
  52. //in Sd2Card.h in /arduino-1.0/libraries/SD/utility 
  53. //you must change the code to #define MEGA_SOFT_SPI 1 
  54. //this is because the SPI connections on the mega are on 51-53
  55. //----------------------------------------------------
  56. #define GPS_LED 5
  57. #define GPS_POWER 4
  58. #define RS485RWPIN 7 //this is the read write pin 
  59. #define CAL_FIN 8
  60. #define STATUS_LED 6 
  61.  
  62. //------------------------------------
  63. //AHRS variables 
  64. //------------------------------------
  65. #define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 
  66. #define XMISSION_BUFFER_SIZE 1000 //this can be adjusted based on the size of the data set to be transmitted
  67. #define ToRad(x) ((x)*0.01745329252)  // *pi/180
  68. #define ToDeg(x) ((x)*57.2957795131)  // *180/pi
  69.  
  70. // L3G4200D gyro: 2000 dps full scale
  71. // 70 mdps/digit; 1 dps = 0.07
  72. #define Gyro_Gain_X 0.07 //X axis Gyro gain
  73. #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
  74. #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
  75. #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
  76. #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
  77. #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
  78.  
  79. // LSM303 magnetometer calibration constants; use the Calibrate example from
  80. // the Pololu LSM303 library to find the right values for your board
  81. /*#define M_X_MIN -481//removed for active config
  82.  #define M_Y_MIN -248
  83.  #define M_Z_MIN -250
  84.  #define M_X_MAX 65
  85.  #define M_Y_MAX 312
  86.  #define M_Z_MAX 249*///removed to become variables
  87.  
  88. #define Kp_ROLLPITCH 0.02
  89. #define Ki_ROLLPITCH 0.00002
  90. #define Kp_YAW 1.2
  91. #define Ki_YAW 0.00002
  92. #define OUTPUTMODE 1
  93.  
  94. //#define PRINT_DCM 0     //Will print the whole direction cosine matrix
  95. #define PRINT_ANALOGS 0 //Will print the analog raw data
  96. #define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw
  97.  
  98. #define MAG_GAIN 96 //must match value from calibration
  99. /* Mag gain values - (plus / minus gauss value)
  100.  32 - 1.3
  101.  64 - 1.9
  102.  96 - 2.5
  103.  128 - 4
  104.  160 - 4.7
  105.  192 - 5.6
  106.  224 - 8.1
  107.  */
  108. //note to self:
  109. //move these to AHRS tab and see if they work
  110. L3G4200D gyro;
  111. LSM303 compass;
  112. LSM303::vector running_min = {
  113.   2047, 2047, 2047}
  114. , running_max = {
  115.   -2048, -2048, -2048};
  116.  
  117. //-------------------------------------------
  118. //buffer vars and parse flags
  119. //-------------------------------------------
  120. char GPSInBuf[90];
  121. char weatherInBuf[90];
  122. char radInBuf[40];
  123. char GPSParseBuf[90];
  124. char weatherParseBuf[90];
  125. char radParseBuf[40];
  126. char *parseptr;
  127.  
  128. PString GPSInBuffer(GPSInBuf,90);
  129. PString weatherInBuffer(weatherInBuf,90);
  130. PString radInBuffer(radInBuf,40);
  131.  
  132. boolean GPSParseReady = false;
  133. boolean weatherParseReady = false;
  134. boolean radParseReady = false;
  135. //---------------------------------------
  136. //parse vars
  137. //-----------------------------------------
  138. //gps vars:
  139. uint32_t timeHHMMSS;
  140. int8_t fixQuality;
  141. float longitude,lattitude,alt,trueHeading,magneticHeading,speedKnots,speedKMH;
  142. boolean gpsFix=false;
  143. //weather vars:
  144. float barometricPressureInches,barometricPressureBars,airTemp,relativeHumidity,dewPoint;
  145. float windDirectionTrue,windDirectionMagnetic,windSpeedKnots,windSpeedMetersPerSecond;
  146. float weatherPitch,weatherRoll,weatherYaw,relativeWindChill,theoreticalWindChill;
  147. //radation vars
  148. float microSievertPerHour;
  149. int countPerMinute;
  150. //AHRS vars:
  151.  
  152. int M_X_MIN;//calibration vars
  153. int M_Y_MIN;
  154. int M_Z_MIN;
  155. int M_X_MAX;
  156. int M_Y_MAX;
  157. int M_Z_MAX;
  158.  
  159. float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible
  160.  
  161. long timer=0;   //general purpuse timer
  162. long timer_old;
  163. long timer24=0; //Second timer used to print values 
  164. int AN[6]; //array that stores the gyro and accelerometer data
  165. int AN_OFFSET[6]={
  166.   0,0,0,0,0,0}; //Array that stores the Offset of the sensors
  167.  
  168. int gyro_x;
  169. int gyro_y;
  170. int gyro_z;
  171. int accel_x;
  172. int accel_y;
  173. int accel_z;
  174. int magnetom_x;
  175. int magnetom_y;
  176. int magnetom_z;
  177. float c_magnetom_x;
  178. float c_magnetom_y;
  179. float c_magnetom_z;
  180. float MAG_Heading;
  181.  
  182. float Accel_Vector[3]= {
  183.   0,0,0}; //Store the acceleration in a vector
  184. float Gyro_Vector[3]= {
  185.   0,0,0};//Store the gyros turn rate in a vector
  186. float Omega_Vector[3]= {
  187.   0,0,0}; //Corrected Gyro_Vector data
  188. float Omega_P[3]= {
  189.   0,0,0};//Omega Proportional correction
  190. float Omega_I[3]= {
  191.   0,0,0};//Omega Integrator
  192. float Omega[3]= {
  193.   0,0,0};
  194.  
  195. // Euler angles
  196. float roll;
  197. float pitch;
  198. float yaw;
  199.  
  200. float errorRollPitch[3]= {
  201.   0,0,0}; 
  202. float errorYaw[3]= {
  203.   0,0,0};
  204.  
  205. unsigned int counter=0;
  206. byte gyro_sat=0;
  207.  
  208. float DCM_Matrix[3][3]= {
  209.   {
  210.     1,0,0          }
  211.   ,{
  212.     0,1,0          }
  213.   ,{
  214.     0,0,1          }
  215. }; 
  216. float Update_Matrix[3][3]={
  217.   {
  218.     0,1,2        }
  219.   ,{
  220.     3,4,5        }
  221.   ,{
  222.     6,7,8        }
  223. }; //Gyros here
  224.  
  225.  
  226. float Temporary_Matrix[3][3]={
  227.   {
  228.     0,0,0          }
  229.   ,{
  230.     0,0,0          }
  231.   ,{
  232.     0,0,0          }
  233. };
  234. // Uncomment the below line to use this axis definition: 
  235. // X axis pointing forward
  236. // Y axis pointing to the right 
  237. // and Z axis pointing down.
  238. // Positive pitch : nose up
  239. // Positive roll : right wing down
  240. // Positive yaw : clockwise
  241. int SENSOR_SIGN[9] = {
  242.   1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
  243. // Uncomment the below line to use this axis definition: 
  244. // X axis pointing forward
  245. // Y axis pointing to the left 
  246. // and Z axis pointing up.
  247. // Positive pitch : nose down
  248. // Positive roll : right wing down
  249. // Positive yaw : counterclockwise
  250. //int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
  251. //-------------------------------------------------
  252. //transmission vars//
  253. //-------------------------------------------------
  254. char transmissionBuffer[XMISSION_BUFFER_SIZE];//large because all the data is a significant size
  255. PString transmissionString(transmissionBuffer,XMISSION_BUFFER_SIZE);
  256. long transmissionTimer=0;
  257.  
  258.  
  259. boolean SDCardPresent = false;
  260.  
  261. //SoftwareSerial Serial3(10,11);
  262.  
  263. void setup(){
  264.  
  265.   //-------------------------------------------------------------
  266.   //GPS Setup-----------------------------------------------------
  267.   //-------------------------------------------------------------
  268.   pinMode(GPS_POWER,OUTPUT);
  269.   pinMode(GPS_LED,OUTPUT);
  270.   digitalWrite(GPS_POWER,LOW);
  271.   Serial.begin(115200);
  272.   pinMode(11,OUTPUT);
  273.   pinMode(10,INPUT);
  274.   Serial3.begin(38400);
  275.   Serial.println("GPS config start");
  276.   delay(2000);
  277.   Serial3.println("$PMTK314,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0*29");//config string
  278.   Serial.println("GPS config complete");
  279.  
  280.   //-----------------------------------------------------------------------
  281.   //Weather station setup-----------------------------------------------
  282.   //-----------------------------------------------------------------------
  283.   pinMode(RS485RWPIN,OUTPUT);
  284.   digitalWrite(RS485RWPIN,HIGH);
  285.   Serial1.begin(4800);
  286.   Serial.println("waiting for weather station boot");
  287.  
  288.   for (int j=15;j>0;j--){
  289.     Serial.println(j);
  290.     delay(1000);
  291.   }
  292.   Serial.println("transmitting strings");
  293.  
  294.   Serial1.println("$PAMTC,HEATER,A*05");//enable heater
  295.   delay(100);
  296.   Serial1.println("$PAMTC,BAUD,38400*66");//set baud rate to 38400
  297.   delay(100);
  298.   Serial1.end();
  299.   Serial1.begin(38400);
  300.   delay(100);
  301.   digitalWrite(RS485RWPIN,LOW);//Set mode to read now that config is done 
  302.   Serial.println("Weather station config complete"); 
  303.  
  304.   //----------------------------------------------------------------------
  305.   //radation stup----------------------------------------------------------
  306.   Serial2.begin(115200);
  307.   //----------------------------------------------------------------------
  308.   //AHRS setup----------------------------------------------------------
  309.   Serial.println("AHRS configuration start");
  310.  
  311.   pinMode (STATUS_LED,OUTPUT);  // Status LED
  312.   pinMode(CAL_FIN,INPUT);
  313.  
  314.   digitalWrite(STATUS_LED,LOW);
  315.   I2C_Init();
  316.   delay(1500);
  317.   Accel_Init();
  318.   Compass_Init();
  319.   Gyro_Init();
  320.   delay(20);
  321.   Calibrate();
  322.   for(int i=0;i<32;i++)    // We take some readings...
  323.   {
  324.     Read_Gyro();
  325.     Read_Accel();
  326.     for(int y=0; y<6; y++)   // Cumulate values
  327.       AN_OFFSET[y] += AN[y];
  328.     delay(20);
  329.   }
  330.   for(int y=0; y<6; y++)
  331.     AN_OFFSET[y] = AN_OFFSET[y]/32;
  332.  
  333.   AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
  334.   //Serial.println("Offset:");
  335.   for(int y=0; y<6; y++)
  336.     Serial.println(AN_OFFSET[y]);
  337.   delay(2000);
  338.   digitalWrite(STATUS_LED,HIGH);
  339.   timer=millis();
  340.   delay(20);
  341.   counter=0;
  342.   Serial.println("AHRS configuration complete");
  343.  
  344.   //--------------------------------------------------------------------
  345.   //initialize 5 hz transmission timer
  346.   transmissionTimer = millis();
  347.   //---------------------------------------------------------
  348.   //SD card setup
  349.   //-------------------------------------------------------------
  350.   if (!SD.begin(chipSelect)) {
  351.     Serial.println("Card failed, or not present");
  352.     // don't do anything more:
  353.  
  354.   }
  355.   else{
  356.     Serial.println("card initialized.");
  357.     SDCardPresent = true;
  358.   }
  359. }
  360. void loop(){
  361.   if(millis()- transmissionTimer >=200){
  362.     transmission();
  363.     transmissionTimer = millis();
  364.   }
  365.  
  366.   getString();
  367.   if(GPSParseReady == true){
  368.     parseptr = GPSParseBuf;
  369.     parseString();
  370.     GPSParseReady = false;
  371.   }
  372.   if(weatherParseReady==true){
  373.     parseptr = weatherParseBuf;
  374.     parseString();
  375.     weatherParseReady = false;
  376.   }
  377.   if(radParseReady==true){
  378.     parseptr = radParseBuf;
  379.     parseString();
  380.     radParseReady = false;
  381.   } 
  382.   if((millis()-timer)>=20)  // Main loop runs at 50Hz
  383.   {
  384.     counter++;
  385.     timer_old = timer;
  386.     timer=millis();
  387.     if (timer>timer_old)
  388.       G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
  389.     else
  390.       G_Dt = 0;
  391.     // *** DCM algorithm
  392.     // Data adquisition
  393.     Read_Gyro();   // This read gyro data
  394.     Read_Accel();     // Read I2C accelerometer
  395.     if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
  396.     {
  397.       counter=0;
  398.       Read_Compass();    // Read I2C magnetometer
  399.       Compass_Heading(); // Calculate magnetic heading  
  400.     }
  401.     // Calculations...
  402.     Matrix_update(); 
  403.     Normalize();
  404.     Drift_correction();
  405.     Euler_angles();
  406.   } 
  407. }
  408.  
  409.  
  410.  
  411.  
  412.  
  413.  
  414.  
Compilation error #stdin compilation error #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
compilation info 
prog.cpp:40:28: error: SoftwareSerial.h: No such file or directory
prog.cpp:41:35: error: PString.h: No such file or directory
prog.cpp:42:23: error: Wire.h: No such file or directory
prog.cpp:43:36: error: L3G4200D.h: No such file or directory
prog.cpp:44:20: error: LSM303.h: No such file or directory
prog.cpp:45:16: error: SD.h: No such file or directory
prog.cpp:110: error: ‘L3G4200D’ does not name a type
prog.cpp:111: error: ‘LSM303’ does not name a type
prog.cpp:112: error: ‘LSM303’ has not been declared
prog.cpp:112: error: expected constructor, destructor, or type conversion before ‘running_min’
prog.cpp:114: error: expected unqualified-id before ‘,’ token
prog.cpp:114: error: expected constructor, destructor, or type conversion before ‘=’ token
prog.cpp:128: error: ‘PString’ does not name a type
prog.cpp:129: error: ‘PString’ does not name a type
prog.cpp:130: error: ‘PString’ does not name a type
prog.cpp:132: error: ‘boolean’ does not name a type
prog.cpp:133: error: ‘boolean’ does not name a type
prog.cpp:134: error: ‘boolean’ does not name a type
prog.cpp:139: error: ‘uint32_t’ does not name a type
prog.cpp:140: error: ‘int8_t’ does not name a type
prog.cpp:142: error: ‘boolean’ does not name a type
prog.cpp:206: error: ‘byte’ does not name a type
prog.cpp:255: error: ‘PString’ does not name a type
prog.cpp:259: error: ‘boolean’ does not name a type
prog.cpp: In function ‘void setup()’:
prog.cpp:268: error: ‘OUTPUT’ was not declared in this scope
prog.cpp:268: error: ‘pinMode’ was not declared in this scope
prog.cpp:270: error: ‘LOW’ was not declared in this scope
prog.cpp:270: error: ‘digitalWrite’ was not declared in this scope
prog.cpp:271: error: ‘Serial’ was not declared in this scope
prog.cpp:273: error: ‘INPUT’ was not declared in this scope
prog.cpp:274: error: ‘Serial3’ was not declared in this scope
prog.cpp:276: error: ‘delay’ was not declared in this scope
prog.cpp:284: error: ‘HIGH’ was not declared in this scope
prog.cpp:285: error: ‘Serial1’ was not declared in this scope
prog.cpp:306: error: ‘Serial2’ was not declared in this scope
prog.cpp:315: error: ‘I2C_Init’ was not declared in this scope
prog.cpp:317: error: ‘Accel_Init’ was not declared in this scope
prog.cpp:318: error: ‘Compass_Init’ was not declared in this scope
prog.cpp:319: error: ‘Gyro_Init’ was not declared in this scope
prog.cpp:321: error: ‘Calibrate’ was not declared in this scope
prog.cpp:324: error: ‘Read_Gyro’ was not declared in this scope
prog.cpp:325: error: ‘Read_Accel’ was not declared in this scope
prog.cpp:339: error: ‘millis’ was not declared in this scope
prog.cpp:350: error: ‘SD’ was not declared in this scope
prog.cpp:357: error: ‘SDCardPresent’ was not declared in this scope
prog.cpp: In function ‘void loop()’:
prog.cpp:361: error: ‘millis’ was not declared in this scope
prog.cpp:362: error: ‘transmission’ was not declared in this scope
prog.cpp:366: error: ‘getString’ was not declared in this scope
prog.cpp:367: error: ‘GPSParseReady’ was not declared in this scope
prog.cpp:369: error: ‘parseString’ was not declared in this scope
prog.cpp:372: error: ‘weatherParseReady’ was not declared in this scope
prog.cpp:374: error: ‘parseString’ was not declared in this scope
prog.cpp:377: error: ‘radParseReady’ was not declared in this scope
prog.cpp:379: error: ‘parseString’ was not declared in this scope
prog.cpp:382: error: ‘millis’ was not declared in this scope
prog.cpp:393: error: ‘Read_Gyro’ was not declared in this scope
prog.cpp:394: error: ‘Read_Accel’ was not declared in this scope
prog.cpp:398: error: ‘Read_Compass’ was not declared in this scope
prog.cpp:399: error: ‘Compass_Heading’ was not declared in this scope
prog.cpp:402: error: ‘Matrix_update’ was not declared in this scope
prog.cpp:403: error: ‘Normalize’ was not declared in this scope
prog.cpp:404: error: ‘Drift_correction’ was not declared in this scope
prog.cpp:405: error: ‘Euler_angles’ was not declared in this scope
stdout
copy 
Standard output is empty
https://ideone.com/wCJUp
language:
C++ (gcc 8.3)
created:
11 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!