mirror of https://github.com/ArduPilot/ardupilot
First version. This is the "core" of the project and libraries.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
8af9e5e867
commit
3976200f50
|
|
@ -0,0 +1,679 @@
|
|||
/* ********************************************************************** */
|
||||
/* ArduCopter Quadcopter code */
|
||||
/* */
|
||||
/* Code based on ArduIMU DCM code from Diydrones.com */
|
||||
/* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */
|
||||
/* Ardupilot core code : from DIYDrones.com development team */
|
||||
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
|
||||
/* Authors : Jose Julio, Ted Carancho, Jordi Muñoz */
|
||||
/* Date : 27-05-2010 */
|
||||
/* Version : 1.0 beta */
|
||||
/* Hardware : ArduPilot Mega + Sensor Shield (codename:FOXTRAP2) */
|
||||
/* This code use this libraries : */
|
||||
/* APM_RC_QUAD : Radio library (adapted for quads) */
|
||||
/* APM_ADC : External ADC library */
|
||||
/* DataFlash : DataFlash log library */
|
||||
/* APM_BMP085 : BMP085 barometer library */
|
||||
/* APM_Compass : HMC5843 compass library [optional] */
|
||||
/* GPS_UBLOX or GPS_NMEA: GPS library [optional] */
|
||||
/* ********************************************************************** */
|
||||
|
||||
#include <Wire.h>
|
||||
#include <APM_ADC.h>
|
||||
#include <APM_RC_QUAD.h>
|
||||
#include <DataFlash.h>
|
||||
#include <APM_Compass.h>
|
||||
// Put your GPS library here:
|
||||
#include <GPS_NMEA.h> // MTK GPS
|
||||
|
||||
/* APM Hardware definitions */
|
||||
#define LED_Yellow 36
|
||||
#define LED_Red 35
|
||||
#define LED_Green 37
|
||||
#define RELE_pin 47
|
||||
#define SW1_pin 41
|
||||
#define SW2_pin 42
|
||||
/* *** */
|
||||
|
||||
/* ***************************************************************************** */
|
||||
/* CONFIGURATION PART */
|
||||
/* ***************************************************************************** */
|
||||
//Adjust this parameter for your lattitude
|
||||
#define GEOG_CORRECTION_FACTOR 0.87 // cos(lattitude)
|
||||
|
||||
#define RADIO_TEST_MODE 0 // 0:Normal 1:Radio Test mode (to test radio channels)
|
||||
#define MAGNETOMETER 1 // 0 : No magnetometer, 1: Magnetometer
|
||||
|
||||
// QuadCopter Attitude control PID GAINS
|
||||
#define KP_QUAD_ROLL 1.7 // 1.5 //1.75
|
||||
#define KD_QUAD_ROLL 0.45 //0.35 // 0.4 //Absolute max:0.85
|
||||
#define KI_QUAD_ROLL 0.4 // 0.4 //0.45
|
||||
#define KP_QUAD_PITCH 1.7
|
||||
#define KD_QUAD_PITCH 0.45
|
||||
#define KI_QUAD_PITCH 0.4
|
||||
#define KP_QUAD_YAW 3.5 // 3.8
|
||||
#define KD_QUAD_YAW 1.2 // 1.3
|
||||
#define KI_QUAD_YAW 0.15 // 0.15
|
||||
|
||||
#define KD_QUAD_COMMAND_PART 12.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs
|
||||
|
||||
// Position control PID GAINS
|
||||
#define KP_GPS_ROLL 0.03
|
||||
#define KD_GPS_ROLL 0.0
|
||||
#define KI_GPS_ROLL 0.01
|
||||
#define KP_GPS_PITCH 0.03
|
||||
#define KD_GPS_PITCH 0.0
|
||||
#define KI_GPS_PITCH 0.01
|
||||
|
||||
#define GPS_MAX_ANGLE 16 // Maximun command roll and pitch angle from position control
|
||||
|
||||
// Altitude control PID GAINS
|
||||
#define KP_ALTITUDE 0.8
|
||||
#define KD_ALTITUDE 0.3
|
||||
#define KI_ALTITUDE 0.2
|
||||
|
||||
// The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets:
|
||||
// We have to take this values with the IMU flat (0º roll, 0º pitch)
|
||||
#define acc_offset_x 2039
|
||||
#define acc_offset_y 2035
|
||||
#define acc_offset_z 2014 // We need to rotate the IMU exactly 90º to take this value
|
||||
#define gyro_offset_roll 1650
|
||||
#define gyro_offset_pitch 1690
|
||||
#define gyro_offset_yaw 1690
|
||||
|
||||
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
|
||||
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
|
||||
// Tested value : 414
|
||||
#define GRAVITY 414 //this equivalent to 1G in the raw data coming from the accelerometer
|
||||
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
|
||||
// Tested values :
|
||||
#define Gyro_Gain_X 0.4 //X axis Gyro gain
|
||||
#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
|
||||
#define Gyro_Gain_Z 0.41 //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
|
||||
|
||||
//#define Kp_ROLLPITCH 0.0125 //0.010 // Pitch&Roll Proportional Gain
|
||||
#define Kp_ROLLPITCH 0.002 //0.003125
|
||||
//#define Ki_ROLLPITCH 0.000010 // Pitch&Roll Integrator Gain
|
||||
#define Ki_ROLLPITCH 0.0000025
|
||||
#define Kp_YAW 1.5 // Yaw Porportional Gain
|
||||
#define Ki_YAW 0.00005 // Yaw Integrator Gain
|
||||
|
||||
/*For debugging purposes*/
|
||||
#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
|
||||
|
||||
//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware
|
||||
|
||||
//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
int SENSOR_SIGN[]={-1,1,-1,1,-1,1,-1,-1,-1}; //{1,-1,-1,1,-1,1,-1,-1,-1}
|
||||
|
||||
int AN[6]; //array that store the 6 ADC channels
|
||||
int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
|
||||
|
||||
float G_Dt=0.02; // Integration time for the gyros (DCM algorithm)
|
||||
|
||||
float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
|
||||
float Accel_Vector_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector
|
||||
//float Accel_magnitude;
|
||||
//float Accel_weight;
|
||||
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn 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};
|
||||
|
||||
float errorRollPitch[3]= {0,0,0};
|
||||
float errorYaw[3]= {0,0,0};
|
||||
float errorCourse=0;
|
||||
float COGX=0; //Course overground X axis
|
||||
float COGY=1; //Course overground Y axis
|
||||
|
||||
float roll=0;
|
||||
float pitch=0;
|
||||
float yaw=0;
|
||||
|
||||
unsigned int counter=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}
|
||||
};
|
||||
|
||||
// GPS variables
|
||||
float speed_3d=0;
|
||||
int GPS_ground_speed=0;
|
||||
|
||||
//Log variables
|
||||
int log_roll;
|
||||
int log_pitch;
|
||||
int log_yaw;
|
||||
|
||||
long timer=0; //general porpuse timer
|
||||
long timer_old;
|
||||
|
||||
// Sonar variables
|
||||
static volatile unsigned long sonar_start_ms;
|
||||
static volatile unsigned char sonar_start_t0;
|
||||
static volatile unsigned long sonar_pulse_start_ms;
|
||||
static volatile unsigned char sonar_pulse_start_t0;
|
||||
static volatile unsigned long sonar_pulse_end_ms;
|
||||
static volatile unsigned char sonar_pulse_end_t0;
|
||||
static volatile byte sonar_status=0;
|
||||
static volatile byte sonar_new_data=0;
|
||||
int sonar_value=0;
|
||||
|
||||
// Attitude control variables
|
||||
float command_rx_roll=0; // comandos recibidos rx
|
||||
float command_rx_roll_old;
|
||||
float command_rx_roll_diff;
|
||||
float command_rx_pitch=0;
|
||||
float command_rx_pitch_old;
|
||||
float command_rx_pitch_diff;
|
||||
float command_rx_yaw=0;
|
||||
float command_rx_yaw_diff;
|
||||
int control_roll; // resultados del control
|
||||
int control_pitch;
|
||||
int control_yaw;
|
||||
float K_aux;
|
||||
|
||||
// Attitude control
|
||||
float roll_I=0;
|
||||
float roll_D;
|
||||
float err_roll;
|
||||
float pitch_I=0;
|
||||
float pitch_D;
|
||||
float err_pitch;
|
||||
float yaw_I=0;
|
||||
float yaw_D;
|
||||
float err_yaw;
|
||||
|
||||
//Position control
|
||||
long target_longitude;
|
||||
long target_lattitude;
|
||||
byte target_position;
|
||||
float gps_err_roll;
|
||||
float gps_err_roll_old;
|
||||
float gps_roll_D;
|
||||
float gps_roll_I=0;
|
||||
float gps_err_pitch;
|
||||
float gps_err_pitch_old;
|
||||
float gps_pitch_D;
|
||||
float gps_pitch_I=0;
|
||||
float command_gps_roll;
|
||||
float command_gps_pitch;
|
||||
|
||||
//Altitude control
|
||||
int Initial_Throttle;
|
||||
int target_sonar_altitude;
|
||||
int err_altitude;
|
||||
int err_altitude_old;
|
||||
float command_altitude;
|
||||
float altitude_I;
|
||||
float altitude_D;
|
||||
|
||||
|
||||
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
|
||||
byte AP_mode = 2;
|
||||
|
||||
long t0;
|
||||
int num_iter;
|
||||
float aux_debug;
|
||||
|
||||
// Radio definitions
|
||||
int Neutro_yaw;
|
||||
int ch_roll;
|
||||
int ch_pitch;
|
||||
int ch_throttle;
|
||||
int ch_yaw;
|
||||
#define CHANN_CENTER 1500
|
||||
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
|
||||
|
||||
/* ************************************************************ */
|
||||
/* Altitude control... (based on sonar) */
|
||||
void Altitude_control(int target_sonar_altitude)
|
||||
{
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_sonar_altitude - sonar_value;
|
||||
altitude_D = (float)(err_altitude-err_altitude_old)/G_Dt;
|
||||
altitude_I += (float)err_altitude*G_Dt;
|
||||
altitude_I = constrain(altitude_I,-100,100);
|
||||
command_altitude = Initial_Throttle + KP_ALTITUDE*err_altitude + KD_ALTITUDE*altitude_D + KI_ALTITUDE*altitude_I;
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
/* Position control... */
|
||||
void Position_control(long lat_dest, long lon_dest)
|
||||
{
|
||||
long Lon_diff;
|
||||
long Lat_diff;
|
||||
float gps_err_roll;
|
||||
float gps_err_pitch;
|
||||
|
||||
Lon_diff = lon_dest - GPS.Longitude;
|
||||
Lat_diff = lat_dest - GPS.Lattitude;
|
||||
|
||||
// ROLL
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
//Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0]
|
||||
gps_err_roll = (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[0][0] - (float)Lat_diff*DCM_Matrix[1][0];
|
||||
|
||||
gps_roll_D = (gps_err_roll-gps_err_roll_old)/G_Dt;
|
||||
|
||||
gps_roll_I += gps_err_roll*G_Dt;
|
||||
gps_roll_I = constrain(gps_roll_I,-500,500);
|
||||
|
||||
command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I;
|
||||
command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
// PITCH
|
||||
gps_err_pitch_old = gps_err_pitch;
|
||||
gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0];
|
||||
|
||||
gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/G_Dt;
|
||||
|
||||
gps_pitch_I += gps_err_pitch*G_Dt;
|
||||
gps_pitch_I = constrain(gps_pitch_I,-500,500);
|
||||
|
||||
command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I;
|
||||
command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
// ROLL, PITCH and YAW PID controls...
|
||||
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
|
||||
void Attitude_control()
|
||||
{
|
||||
// ROLL CONTROL
|
||||
if (AP_mode==2) // Normal Mode => Stabilization mode
|
||||
err_roll = command_rx_roll - ToDeg(roll);
|
||||
else
|
||||
err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control
|
||||
|
||||
err_roll = constrain(err_roll,-25,25); // to limit max roll command...
|
||||
|
||||
roll_I += err_roll*G_Dt;
|
||||
roll_I = constrain(roll_I,-20,20);
|
||||
// D term implementation => two parts: gyro part and command part
|
||||
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
|
||||
// Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
|
||||
roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command)
|
||||
|
||||
// PID control
|
||||
control_roll = KP_QUAD_ROLL*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I;
|
||||
|
||||
// PITCH CONTROL
|
||||
if (AP_mode==2) // Normal mode => Stabilization mode
|
||||
err_pitch = command_rx_pitch - ToDeg(pitch);
|
||||
else
|
||||
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control
|
||||
|
||||
err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command...
|
||||
|
||||
pitch_I += err_pitch*G_Dt;
|
||||
pitch_I = constrain(pitch_I,-20,20);
|
||||
// D term
|
||||
pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]);
|
||||
|
||||
// PID control
|
||||
control_pitch = KP_QUAD_PITCH*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I;
|
||||
|
||||
// YAW CONTROL
|
||||
|
||||
err_yaw = command_rx_yaw - ToDeg(yaw);
|
||||
if (err_yaw > 180) // Normalize to -180,180
|
||||
err_yaw -= 360;
|
||||
else if(err_yaw < -180)
|
||||
err_yaw += 360;
|
||||
|
||||
err_yaw = constrain(err_yaw,-25,25); // to limit max yaw command...
|
||||
|
||||
yaw_I += err_yaw*G_Dt;
|
||||
yaw_I = constrain(yaw_I,-20,20);
|
||||
yaw_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]);
|
||||
|
||||
// PID control
|
||||
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
|
||||
}
|
||||
|
||||
// Maximun slope filter for radio inputs... (limit max differences between readings)
|
||||
int channel_filter(int ch, int ch_old)
|
||||
{
|
||||
int diff_ch_old;
|
||||
|
||||
if (ch_old==0) // ch_old not initialized
|
||||
return(ch);
|
||||
diff_ch_old = ch - ch_old; // Difference with old reading
|
||||
if (diff_ch_old<0)
|
||||
{
|
||||
if (diff_ch_old<-40)
|
||||
return(ch_old-40); // We limit the max difference between readings
|
||||
}
|
||||
else
|
||||
{
|
||||
if (diff_ch_old>40)
|
||||
return(ch_old+40);
|
||||
}
|
||||
//return((ch+ch_old)>>1); // Small filtering
|
||||
return(ch);
|
||||
}
|
||||
|
||||
|
||||
/* ****** SETUP ********************************************************************* */
|
||||
void setup()
|
||||
{
|
||||
int i;
|
||||
float aux_float[3];
|
||||
|
||||
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
|
||||
pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
|
||||
pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
|
||||
|
||||
pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0)
|
||||
|
||||
pinMode(RELE_pin,OUTPUT); // Rele output
|
||||
digitalWrite(RELE_pin,LOW);
|
||||
|
||||
APM_RC_QUAD.Init(); // APM Radio initialization
|
||||
APM_ADC.Init(); // APM ADC library initialization
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
GPS.Init(); // GPS Initialization
|
||||
|
||||
// RC channels Initialization (Quad motors)
|
||||
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE+15); // Motors stoped
|
||||
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE+15);
|
||||
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15);
|
||||
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15);
|
||||
|
||||
#if (MAGNETOMTER==1)
|
||||
APM_Compass.Init(); // I2C initialization
|
||||
#endif
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
||||
Serial.begin(57600);
|
||||
Serial.println();
|
||||
Serial.println("ArduCopter Quadcopter v1.0");
|
||||
|
||||
// Check if we enable the DataFlash log Read Mode (switch)
|
||||
// If we press switch 1 at startup we read the Dataflash eeprom
|
||||
while (digitalRead(SW1_pin)==0)
|
||||
{
|
||||
Serial.println("Entering Log Read Mode...");
|
||||
Log_Read(1,1000);
|
||||
delay(30000);
|
||||
}
|
||||
|
||||
delay(3000);
|
||||
|
||||
Read_adc_raw();
|
||||
delay(20);
|
||||
|
||||
// Offset values for accels and gyros...
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
aux_float[0] = gyro_offset_roll;
|
||||
aux_float[1] = gyro_offset_pitch;
|
||||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
// Take the gyro offset values
|
||||
for(i=0;i<250;i++)
|
||||
{
|
||||
Read_adc_raw();
|
||||
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
|
||||
{
|
||||
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
|
||||
//Serial.print(AN[y]);
|
||||
//Serial.print(",");
|
||||
}
|
||||
//Serial.println();
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
delay(14);
|
||||
}
|
||||
for(int y=0; y<=2; y++)
|
||||
AN_OFFSET[y]=aux_float[y];
|
||||
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
}
|
||||
|
||||
Neutro_yaw = APM_RC_QUAD.InputCh(3); // Take yaw neutral radio value
|
||||
Serial.print("Yaw neutral value:");
|
||||
Serial.println(Neutro_yaw);
|
||||
|
||||
#if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS
|
||||
while(1)
|
||||
{
|
||||
if (APM_RC_QUAD.GetState()==1)
|
||||
{
|
||||
Serial.print("AIL:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(0));
|
||||
Serial.print("ELE:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(1));
|
||||
Serial.print("THR:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(2));
|
||||
Serial.print("YAW:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(3));
|
||||
Serial.print("AUX(mode):");
|
||||
Serial.print(APM_RC_QUAD.InputCh(4));
|
||||
Serial.print("AUX2:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(5));
|
||||
Serial.println();
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(1000);
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
timer = millis();
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
delay(20);
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
||||
/* ***** MAIN LOOP ***** */
|
||||
void loop(){
|
||||
|
||||
int aux;
|
||||
int i;
|
||||
float aux_float;
|
||||
|
||||
if((millis()-timer)>=14) // Main loop 70Hz
|
||||
{
|
||||
counter++;
|
||||
timer_old = timer;
|
||||
timer=millis();
|
||||
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run
|
||||
|
||||
// IMU DCM Algorithm
|
||||
Read_adc_raw();
|
||||
#if (MAGNETOMETER==1)
|
||||
if (counter > 8) // Read compass data at 10Hz... (7 loop runs)
|
||||
{
|
||||
counter=0;
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
}
|
||||
#endif
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
// *****************
|
||||
|
||||
// Output data
|
||||
log_roll = ToDeg(roll)*10;
|
||||
log_pitch = ToDeg(pitch)*10;
|
||||
log_yaw = ToDeg(yaw)*10;
|
||||
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
|
||||
// Write Sensor raw data to DataFlash log
|
||||
//Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
// Write attitude to DataFlash log
|
||||
Log_Write_Attitude(log_roll,log_pitch,log_yaw);
|
||||
|
||||
if (APM_RC_QUAD.GetState()==1) // New radio frame?
|
||||
{
|
||||
// Commands from radio Rx...
|
||||
// Stick position defines the desired angle in roll, pitch and yaw
|
||||
ch_roll = channel_filter(APM_RC_QUAD.InputCh(0),ch_roll);
|
||||
ch_pitch = channel_filter(APM_RC_QUAD.InputCh(1),ch_pitch);
|
||||
ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle);
|
||||
ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw);
|
||||
command_rx_roll_old = command_rx_roll;
|
||||
command_rx_roll = (ch_roll-CHANN_CENTER)/15.0;
|
||||
command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
|
||||
command_rx_pitch_old = command_rx_pitch;
|
||||
command_rx_pitch = (ch_pitch-CHANN_CENTER)/15.0;
|
||||
command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old;
|
||||
aux_float = (ch_yaw-Neutro_yaw)/180.0;
|
||||
command_rx_yaw += aux_float;
|
||||
command_rx_yaw_diff = aux_float;
|
||||
if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees
|
||||
command_rx_yaw -= 360.0;
|
||||
else if (command_rx_yaw < -180)
|
||||
command_rx_yaw += 360.0;
|
||||
|
||||
// I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now]
|
||||
//K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
|
||||
K_aux = K_aux*0.8 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 0.4)*0.2;
|
||||
if (K_aux < 0)
|
||||
K_aux = 0;
|
||||
|
||||
/*
|
||||
Serial.print(",");
|
||||
Serial.print(K_aux);
|
||||
*/
|
||||
|
||||
// We read the Quad Mode from Channel 5
|
||||
if (APM_RC_QUAD.InputCh(4) < 1200)
|
||||
{
|
||||
AP_mode = 1; // Position hold mode (GPS position control)
|
||||
digitalWrite(LED_Yellow,HIGH); // Yellow LED On
|
||||
}
|
||||
else
|
||||
{
|
||||
AP_mode = 2; // Normal mode (Stabilization assist mode)
|
||||
digitalWrite(LED_Yellow,LOW); // Yellow LED off
|
||||
}
|
||||
// Write Radio data to DataFlash log
|
||||
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
|
||||
} // END new radio data
|
||||
|
||||
if (AP_mode==1) // Position Control
|
||||
{
|
||||
if (target_position==0) // If this is the first time we switch to Position control, actual position is our target position
|
||||
{
|
||||
target_lattitude = GPS.Lattitude;
|
||||
target_longitude = GPS.Longitude;
|
||||
Serial.println();
|
||||
Serial.print("* Target:");
|
||||
Serial.print(target_longitude);
|
||||
Serial.print(",");
|
||||
Serial.println(target_lattitude);
|
||||
target_position=1;
|
||||
//target_sonar_altitude = sonar_value;
|
||||
//Initial_Throttle = ch3;
|
||||
// Reset I terms
|
||||
altitude_I = 0;
|
||||
gps_roll_I = 0;
|
||||
gps_pitch_I = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
target_position=0;
|
||||
|
||||
//Read GPS
|
||||
GPS.Read();
|
||||
if (GPS.NewData) // New GPS data?
|
||||
{
|
||||
GPS.NewData=0; // We Reset the flag...
|
||||
|
||||
//Output GPS data
|
||||
//Serial.print(",");
|
||||
//Serial.print(GPS.Lattitude);
|
||||
//Serial.print(",");
|
||||
//Serial.print(GPS.Longitude);
|
||||
|
||||
// Write GPS data to DataFlash log
|
||||
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
|
||||
|
||||
if (GPS.Fix)
|
||||
digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED
|
||||
else
|
||||
digitalWrite(LED_Red,LOW);
|
||||
|
||||
if (AP_mode==1)
|
||||
{
|
||||
if ((target_position==1)&&(GPS.Fix))
|
||||
{
|
||||
Position_control(target_lattitude,target_longitude); // Call position hold routine
|
||||
/*
|
||||
Serial.print("PC:");
|
||||
Serial.print(command_gps_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(command_gps_pitch);
|
||||
Serial.println();
|
||||
*/
|
||||
}
|
||||
else
|
||||
{
|
||||
//Serial.print("NOFIX");
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Attitude control (Roll, Pitch, yaw)
|
||||
Attitude_control();
|
||||
|
||||
// Quadcopter mix
|
||||
if (ch_throttle > (MIN_THROTTLE+20)) // Minimun throttle to start control
|
||||
{
|
||||
APM_RC_QUAD.OutputCh(0,ch_throttle - control_roll - control_yaw); // Right motor
|
||||
APM_RC_QUAD.OutputCh(1,ch_throttle + control_roll - control_yaw); // Left motor
|
||||
APM_RC_QUAD.OutputCh(2,ch_throttle + control_pitch + control_yaw); // Front motor
|
||||
APM_RC_QUAD.OutputCh(3,ch_throttle - control_pitch + control_yaw); // Back motor
|
||||
}
|
||||
else
|
||||
{
|
||||
roll_I = 0; // reset I terms of PID controls
|
||||
pitch_I = 0;
|
||||
yaw_I = 0;
|
||||
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE);
|
||||
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE);
|
||||
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE);
|
||||
// Initialize yaw command to actual yaw
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
command_rx_yaw_diff = 0;
|
||||
}
|
||||
Serial.println(); // Line END
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
/* ******* ADC functions ********************* */
|
||||
// Read all the ADC channles
|
||||
void Read_adc_raw(void)
|
||||
{
|
||||
int temp;
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
AN[i] = APM_ADC.Ch(sensors[i]);
|
||||
|
||||
// Correction for non ratiometric sensor (test)
|
||||
//temp = APM_ADC.Ch(3);
|
||||
//AN[0] += 1500-temp;
|
||||
//AN[1] += 1500-temp;
|
||||
//AN[2] += 1500-temp;
|
||||
}
|
||||
|
||||
// Returns an analog value with the offset
|
||||
float read_adc(int select)
|
||||
{
|
||||
if (SENSOR_SIGN[select]<0)
|
||||
return (AN_OFFSET[select]-AN[select]);
|
||||
else
|
||||
return (AN[select]-AN_OFFSET[select]);
|
||||
}
|
||||
/* ******************************************* */
|
||||
|
||||
/* ******* DCM IMU functions ********************* */
|
||||
/**************************************************/
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
|
||||
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
float errorCourse;
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
float Accel_magnitude;
|
||||
float Accel_weight;
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
// Calculate the magnitude of the accelerometer vector
|
||||
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
#if (MAGNETOMETER==1)
|
||||
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_X) - (DCM_Matrix[1][0]*APM_Compass.Heading_Y); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
#endif
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
//Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
//Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
|
||||
Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
|
||||
Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
|
||||
|
||||
//Accel_Vector[0]=read_adc(3); // acc x
|
||||
//Accel_Vector[1]=read_adc(4); // acc y
|
||||
//Accel_Vector[2]=read_adc(5); // acc z
|
||||
|
||||
// Low pass filter on accelerometer data (to filter vibrations)
|
||||
Accel_Vector[0]=Accel_Vector[0]*0.4 + (float)read_adc(3)*0.6; // acc x
|
||||
Accel_Vector[1]=Accel_Vector[1]*0.4 + (float)read_adc(4)*0.6; // acc y
|
||||
Accel_Vector[2]=Accel_Vector[2]*0.4 + (float)read_adc(5)*0.6; // acc z
|
||||
|
||||
Accel_Vector[1]=0;
|
||||
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
|
||||
|
||||
//Accel_adjust();//adjusting centrifugal acceleration. // Not used for quadcopter
|
||||
|
||||
#if OUTPUTMODE==1
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
#if OUTPUTMODE==0
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
pitch = -asin((Accel_Vector[0])/(float)GRAVITY); // asin(acc_x)
|
||||
yaw = 0;
|
||||
#else // Euler angles from DCM matrix
|
||||
pitch = asin(-DCM_Matrix[2][0]);
|
||||
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
|
||||
Install notes
|
||||
-------------
|
||||
|
||||
To install the libraries:
|
||||
- copy Library Directories to your \arduino\hardware\lirbaries\ directory
|
||||
- Restart arduino IDE
|
||||
|
||||
Each library comes with a simple example. You can find the examples in menu File->Examples
|
||||
|
||||
This code works with Ardupilot Mega Hardware and Beta sensor shield (FOXTRAP2).
|
||||
|
|
@ -0,0 +1,232 @@
|
|||
// Test code to Write and Read packets from DataFlash log memory
|
||||
// Packets : Attitude, Raw sensor data, Radio and GPS
|
||||
|
||||
#define HEAD_BYTE1 0xA3
|
||||
#define HEAD_BYTE2 0x95
|
||||
#define END_BYTE 0xBA
|
||||
|
||||
#define LOG_ATTITUDE_MSG 0x01
|
||||
#define LOG_GPS_MSG 0x02
|
||||
#define LOG_RADIO_MSG 0x03
|
||||
#define LOG_SENSOR_MSG 0x04
|
||||
|
||||
// Write a Sensor Raw data packet
|
||||
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_SENSOR_MSG);
|
||||
DataFlash.WriteInt(s1);
|
||||
DataFlash.WriteInt(s2);
|
||||
DataFlash.WriteInt(s3);
|
||||
DataFlash.WriteInt(s4);
|
||||
DataFlash.WriteInt(s5);
|
||||
DataFlash.WriteInt(s6);
|
||||
DataFlash.WriteInt(s7);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Write an attitude packet. Total length : 10 bytes
|
||||
void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
|
||||
DataFlash.WriteInt(log_roll);
|
||||
DataFlash.WriteInt(log_pitch);
|
||||
DataFlash.WriteInt(log_yaw);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Write an GPS packet. Total length : 30 bytes
|
||||
void Log_Write_GPS(long log_Time, long log_Lattitude, long log_Longitude, long log_Altitude,
|
||||
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_GPS_MSG);
|
||||
DataFlash.WriteLong(log_Time);
|
||||
DataFlash.WriteByte(log_Fix);
|
||||
DataFlash.WriteByte(log_NumSats);
|
||||
DataFlash.WriteLong(log_Lattitude);
|
||||
DataFlash.WriteLong(log_Longitude);
|
||||
DataFlash.WriteLong(log_Altitude);
|
||||
DataFlash.WriteLong(log_Ground_Speed);
|
||||
DataFlash.WriteLong(log_Ground_Course);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Write a Radio packet
|
||||
void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_RADIO_MSG);
|
||||
DataFlash.WriteInt(ch1);
|
||||
DataFlash.WriteInt(ch2);
|
||||
DataFlash.WriteInt(ch3);
|
||||
DataFlash.WriteInt(ch4);
|
||||
DataFlash.WriteInt(ch5);
|
||||
DataFlash.WriteInt(ch6);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// **** READ ROUTINES ****
|
||||
|
||||
// Read a Sensor raw data packet
|
||||
void Log_Read_Sensor()
|
||||
{
|
||||
Serial.print("SENSOR:");
|
||||
Serial.print(DataFlash.ReadInt()); // GX
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // GY
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // GZ
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCX
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCY
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // ACCZ
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // AUX
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// Read an attitude packet
|
||||
void Log_Read_Attitude()
|
||||
{
|
||||
int log_roll;
|
||||
int log_pitch;
|
||||
int log_yaw;
|
||||
|
||||
log_roll = DataFlash.ReadInt();
|
||||
log_pitch = DataFlash.ReadInt();
|
||||
log_yaw = DataFlash.ReadInt();
|
||||
Serial.print("ATT:");
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// Read a GPS packet
|
||||
void Log_Read_GPS()
|
||||
{
|
||||
long log_Time;
|
||||
byte log_Fix;
|
||||
byte log_NumSats;
|
||||
long log_Lattitude;
|
||||
long log_Longitude;
|
||||
long log_Altitude;
|
||||
long log_Ground_Speed;
|
||||
long log_Ground_Course;
|
||||
|
||||
log_Time = DataFlash.ReadLong();
|
||||
log_Fix = DataFlash.ReadByte();
|
||||
log_NumSats = DataFlash.ReadByte();
|
||||
log_Lattitude = DataFlash.ReadLong();
|
||||
log_Longitude = DataFlash.ReadLong();
|
||||
log_Altitude = DataFlash.ReadLong();
|
||||
log_Ground_Speed = DataFlash.ReadLong();
|
||||
log_Ground_Course = DataFlash.ReadLong();
|
||||
|
||||
Serial.print("GPS:");
|
||||
Serial.print(log_Time);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_Fix);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_NumSats);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Lattitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Longitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Altitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Speed);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Course);
|
||||
Serial.println();
|
||||
|
||||
}
|
||||
|
||||
// Read an Radio packet
|
||||
void Log_Read_Radio()
|
||||
{
|
||||
Serial.print("RADIO:");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt());
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// Read the DataFlash log memory : Packet Parser
|
||||
void Log_Read(int start_page, int end_page)
|
||||
{
|
||||
byte data;
|
||||
byte log_step=0;
|
||||
int packet_count=0;
|
||||
|
||||
DataFlash.StartRead(start_page);
|
||||
while (DataFlash.GetPage() < end_page)
|
||||
{
|
||||
data = DataFlash.ReadByte();
|
||||
switch(log_step) //This is a state machine to read the packets
|
||||
{
|
||||
case 0:
|
||||
if(data==HEAD_BYTE1) // Head byte 1
|
||||
log_step++;
|
||||
break;
|
||||
case 1:
|
||||
if(data==HEAD_BYTE2) // Head byte 2
|
||||
log_step++;
|
||||
break;
|
||||
case 2:
|
||||
switch (data)
|
||||
{
|
||||
case LOG_ATTITUDE_MSG:
|
||||
Log_Read_Attitude();
|
||||
log_step++;
|
||||
break;
|
||||
case LOG_GPS_MSG:
|
||||
Log_Read_GPS();
|
||||
log_step++;
|
||||
break;
|
||||
case LOG_RADIO_MSG:
|
||||
Log_Read_Radio();
|
||||
log_step++;
|
||||
break;
|
||||
case LOG_SENSOR_MSG:
|
||||
Log_Read_Sensor();
|
||||
log_step++;
|
||||
break;
|
||||
default:
|
||||
Serial.print("Error Reading Packet: ");
|
||||
Serial.print(packet_count);
|
||||
log_step=0; // Restart, we have a problem...
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if(data==END_BYTE)
|
||||
packet_count++;
|
||||
else
|
||||
Serial.println("Error Reading END_BYTE");
|
||||
log_step=0; // Restart sequence: new packet...
|
||||
}
|
||||
}
|
||||
Serial.print("Number of packets read: ");
|
||||
Serial.println(packet_count);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
//Computes the dot product of two vectors
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
float op=0;
|
||||
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
op+=vector1[c]*vector2[c];
|
||||
}
|
||||
|
||||
return op;
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn[c]*scale2;
|
||||
}
|
||||
}
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
|
||||
}
|
||||
}
|
||||
|
||||
/********* MATRIX FUNCTIONS *****************************************/
|
||||
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
||||
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=0;
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
|
||||
float test=mat[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
/*
|
||||
APM_ADC.cpp - ADC ADS7844 Library for Ardupilot Mega
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
|
||||
TXD2 = MOSI = pin PH1
|
||||
RXD2 = MISO = pin PH0
|
||||
XCK2 = SCK = pin PH2
|
||||
Chip Select pin is PC4 (33) [PH6 (9)]
|
||||
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
||||
The sampling frequency is 400Hz (Timer2 overflow interrupt)
|
||||
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
|
||||
we have an 4x oversampling and averaging.
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
||||
Ch(ch_num) : Return the ADC channel value
|
||||
|
||||
On Ardupilot Mega Hardware:
|
||||
Channel 1 : Gyro Z
|
||||
Channel 2 : Gyro X
|
||||
Channel 3 : Gyro Y
|
||||
Channel 4 : Acc X
|
||||
Channel 5 : Acc Y
|
||||
Channel 6 : Acc Z
|
||||
Channel 7 : Differential pressure sensor
|
||||
|
||||
*/
|
||||
extern "C" {
|
||||
// AVR LibC Includes
|
||||
#include <inttypes.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
|
||||
#include "APM_ADC.h"
|
||||
|
||||
// Commands for reading ADC channels on ADS7844
|
||||
const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
|
||||
volatile long adc_value[8];
|
||||
volatile unsigned char adc_counter[8];
|
||||
|
||||
unsigned char ADC_SPI_transfer(unsigned char data)
|
||||
{
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSR2A & (1<<UDRE2)) );
|
||||
/* Put data into buffer, sends the data */
|
||||
UDR2 = data;
|
||||
/* Wait for data to be received */
|
||||
while ( !(UCSR2A & (1<<RXC2)) );
|
||||
/* Get and return received data from buffer */
|
||||
return UDR2;
|
||||
}
|
||||
|
||||
|
||||
ISR (TIMER2_OVF_vect)
|
||||
{
|
||||
uint8_t ch;
|
||||
unsigned int adc_tmp;
|
||||
|
||||
//bit_set(PORTL,6); // To test performance
|
||||
bit_clear(PORTC,4); // Enable Chip Select (PIN PC4)
|
||||
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
||||
for (ch=0;ch<7;ch++)
|
||||
{
|
||||
adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte
|
||||
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch+1]); // Read second byte and send next command
|
||||
adc_value[ch] += adc_tmp>>3; // Shift to 12 bits
|
||||
adc_counter[ch]++; // Number of samples
|
||||
}
|
||||
bit_set(PORTC,4); // Disable Chip Select (PIN PC4)
|
||||
//bit_clear(PORTL,6); // To test performance
|
||||
TCNT2 = 104; // 400 Hz
|
||||
}
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
APM_ADC_Class::APM_ADC_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void APM_ADC_Class::Init(void)
|
||||
{
|
||||
unsigned char tmp;
|
||||
|
||||
pinMode(ADC_CHIP_SELECT,OUTPUT);
|
||||
|
||||
digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low)
|
||||
|
||||
// Setup Serial Port2 in SPI mode
|
||||
UBRR2 = 0;
|
||||
DDRH |= (1<<PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
|
||||
// Set MSPI mode of operation and SPI data mode 0.
|
||||
UCSR2C = (1<<UMSEL21)|(1<<UMSEL20); //|(0<<UCPHA2)|(0<<UCPOL2);
|
||||
// Enable receiver and transmitter.
|
||||
UCSR2B = (1<<RXEN2)|(1<<TXEN2);
|
||||
// Set Baud rate
|
||||
UBRR2 = 2; // SPI clock running at 2.6MHz
|
||||
|
||||
|
||||
// Enable Timer2 Overflow interrupt to capture ADC data
|
||||
TIMSK2 = 0; // Disable interrupts
|
||||
TCCR2A = 0; // normal counting mode
|
||||
TCCR2B = _BV(CS21)|_BV(CS22); // Set prescaler of 256
|
||||
TCNT2 = 0;
|
||||
TIFR2 = _BV(TOV2); // clear pending interrupts;
|
||||
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
|
||||
}
|
||||
|
||||
// Read one channel value
|
||||
int APM_ADC_Class::Ch(unsigned char ch_num)
|
||||
{
|
||||
int result;
|
||||
|
||||
cli(); // We stop interrupts to read the variables
|
||||
if (adc_counter[ch_num]>0)
|
||||
result = adc_value[ch_num]/adc_counter[ch_num];
|
||||
else
|
||||
result = 0;
|
||||
adc_value[ch_num] = 0; // Initialize for next reading
|
||||
adc_counter[ch_num] = 0;
|
||||
sei();
|
||||
return(result);
|
||||
}
|
||||
|
||||
// make one instance for the user to use
|
||||
APM_ADC_Class APM_ADC;
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#ifndef APM_ADC_h
|
||||
#define APM_ADC_h
|
||||
|
||||
#define bit_set(p,m) ((p) |= (1<<m))
|
||||
#define bit_clear(p,m) ((p) &= ~(1<<m))
|
||||
|
||||
// We use Serial Port 2 in SPI Mode
|
||||
#define ADC_DATAOUT 51 // MOSI
|
||||
#define ADC_DATAIN 50 // MISO
|
||||
#define ADC_SPICLOCK 52 // SCK
|
||||
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
|
||||
|
||||
class APM_ADC_Class
|
||||
{
|
||||
private:
|
||||
public:
|
||||
APM_ADC_Class(); // Constructor
|
||||
void Init();
|
||||
int Ch(unsigned char ch_num);
|
||||
};
|
||||
|
||||
extern APM_ADC_Class APM_ADC;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
/*
|
||||
Example of APM_ADC library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <APM_ADC.h> // ArduPilot Mega ADC Library
|
||||
|
||||
unsigned long timer;
|
||||
|
||||
void setup()
|
||||
{
|
||||
APM_ADC.Init(); // APM ADC initialization
|
||||
Serial.begin(57600);
|
||||
Serial.println("ArduPilot Mega ADC library test");
|
||||
delay(1000);
|
||||
timer = millis();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int ch;
|
||||
|
||||
if((millis()- timer) > 20)
|
||||
{
|
||||
timer = millis();
|
||||
for (ch=0;ch<7;ch++)
|
||||
{
|
||||
Serial.print(APM_ADC.Ch(ch));
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
APM_ADC KEYWORD1
|
||||
Init KEYWORD2
|
||||
Ch KEYWORD2
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
/*
|
||||
APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor is conected to I2C port
|
||||
Sensor End of Conversion (EOC) pin is PC7 (30)
|
||||
|
||||
Variables:
|
||||
RawTemp : Raw temperature data
|
||||
RawPress : Raw pressure data
|
||||
|
||||
Temp : Calculated temperature (in 0.1ºC units)
|
||||
Press : Calculated pressure (in Pa units)
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of I2C and read sensor calibration data
|
||||
Read() : Read sensor data and calculate Temperature and Pressure
|
||||
This function is optimized so the main host don´t need to wait
|
||||
You can call this function in your main loop
|
||||
It returns a 1 if there are new data.
|
||||
|
||||
Internal functions:
|
||||
Command_ReadTemp(): Send commando to read temperature
|
||||
Command_ReadPress(): Send commando to read Pressure
|
||||
ReadTemp() : Read temp register
|
||||
ReadPress() : Read press register
|
||||
Calculate() : Calculate Temperature and Pressure in real units
|
||||
|
||||
|
||||
*/
|
||||
extern "C" {
|
||||
// AVR LibC Includes
|
||||
#include <inttypes.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
|
||||
#include <Wire.h>
|
||||
#include "APM_BMP085.h"
|
||||
|
||||
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
|
||||
#define BMP085_EOC 30 // End of conversion pin PC7
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
APM_BMP085_Class::APM_BMP085_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void APM_BMP085_Class::Init(void)
|
||||
{
|
||||
unsigned char tmp;
|
||||
byte buff[22];
|
||||
int i=0;
|
||||
|
||||
pinMode(BMP085_EOC,INPUT); // End Of Conversion (PC7) input
|
||||
Wire.begin();
|
||||
oss = 3; // Over Sampling setting 3 = High resolution
|
||||
BMP085_State = 0; // Initial state
|
||||
|
||||
// We read the calibration data registers
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.send(0xAA);
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.requestFrom(BMP085_ADDRESS, 22);
|
||||
//Wire.endTransmission();
|
||||
while(Wire.available())
|
||||
{
|
||||
buff[i] = Wire.receive(); // receive one byte
|
||||
i++;
|
||||
}
|
||||
ac1 = ((int)buff[0] << 8) | buff[1];
|
||||
ac2 = ((int)buff[2] << 8) | buff[3];
|
||||
ac3 = ((int)buff[4] << 8) | buff[5];
|
||||
ac4 = ((int)buff[6] << 8) | buff[7];
|
||||
ac5 = ((int)buff[8] << 8) | buff[9];
|
||||
ac6 = ((int)buff[10] << 8) | buff[11];
|
||||
b1 = ((int)buff[12] << 8) | buff[13];
|
||||
b2 = ((int)buff[14] << 8) | buff[15];
|
||||
mb = ((int)buff[16] << 8) | buff[17];
|
||||
mc = ((int)buff[18] << 8) | buff[19];
|
||||
md = ((int)buff[20] << 8) | buff[21];
|
||||
|
||||
//Send a command to read Temp
|
||||
Command_ReadTemp();
|
||||
BMP085_State=1;
|
||||
}
|
||||
|
||||
|
||||
// Read the sensor. This is a state machine
|
||||
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
|
||||
uint8_t APM_BMP085_Class::Read()
|
||||
{
|
||||
uint8_t result=0;
|
||||
|
||||
if (BMP085_State==1)
|
||||
{
|
||||
if (digitalRead(BMP085_EOC))
|
||||
{
|
||||
ReadTemp(); // On state 1 we read temp
|
||||
BMP085_State++;
|
||||
Command_ReadPress();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (BMP085_State==5)
|
||||
{
|
||||
if (digitalRead(BMP085_EOC))
|
||||
{
|
||||
ReadPress();
|
||||
Calculate();
|
||||
BMP085_State = 1; // Start again from state=1
|
||||
Command_ReadTemp(); // Read Temp
|
||||
result=1; // New pressure reading
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (digitalRead(BMP085_EOC))
|
||||
{
|
||||
ReadPress();
|
||||
Calculate();
|
||||
BMP085_State++;
|
||||
Command_ReadPress();
|
||||
result=1; // New pressure reading
|
||||
}
|
||||
}
|
||||
}
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
// Send command to Read Pressure
|
||||
void APM_BMP085_Class::Command_ReadPress()
|
||||
{
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.send(0xF4);
|
||||
Wire.send(0x34+(oss<<6)); //write_register(0xF4,0x34+(oversampling_setting<<6));
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
// Read Raw Pressure values
|
||||
void APM_BMP085_Class::ReadPress()
|
||||
{
|
||||
byte msb;
|
||||
byte lsb;
|
||||
byte xlsb;
|
||||
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.send(0xF6);
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
|
||||
while(!Wire.available()) {
|
||||
// waiting
|
||||
}
|
||||
msb = Wire.receive();
|
||||
while(!Wire.available()) {
|
||||
// waiting
|
||||
}
|
||||
lsb = Wire.receive();
|
||||
while(!Wire.available()) {
|
||||
// waiting
|
||||
}
|
||||
xlsb = Wire.receive();
|
||||
RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss);
|
||||
}
|
||||
|
||||
// Send Command to Read Temperature
|
||||
void APM_BMP085_Class::Command_ReadTemp()
|
||||
{
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.send(0xF4);
|
||||
Wire.send(0x2E);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
// Read Raw Temperature values
|
||||
void APM_BMP085_Class::ReadTemp()
|
||||
{
|
||||
byte tmp;
|
||||
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.send(0xF6);
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.beginTransmission(BMP085_ADDRESS);
|
||||
Wire.requestFrom(BMP085_ADDRESS,2);
|
||||
while(!Wire.available()); // wait
|
||||
RawTemp = Wire.receive();
|
||||
while(!Wire.available()); // wait
|
||||
tmp = Wire.receive();
|
||||
RawTemp = RawTemp<<8 | tmp;
|
||||
}
|
||||
|
||||
// Calculate Temperature and Pressure in real units.
|
||||
void APM_BMP085_Class::Calculate()
|
||||
{
|
||||
long x1, x2, x3, b3, b5, b6, p;
|
||||
unsigned long b4, b7;
|
||||
int32_t tmp;
|
||||
|
||||
// See Datasheet page 13 for this formulas
|
||||
// Based also on Jee Labs BMP085 example code. Thanks for share.
|
||||
// Temperature calculations
|
||||
x1 = ((long)RawTemp - ac6) * ac5 >> 15;
|
||||
x2 = ((long) mc << 11) / (x1 + md);
|
||||
b5 = x1 + x2;
|
||||
Temp = (b5 + 8) >> 4;
|
||||
|
||||
// Pressure calculations
|
||||
b6 = b5 - 4000;
|
||||
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
|
||||
x2 = ac2 * b6 >> 11;
|
||||
x3 = x1 + x2;
|
||||
//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
|
||||
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0
|
||||
tmp = ac1;
|
||||
tmp = (tmp*4 + x3)<<oss;
|
||||
b3 = (tmp+2)/4;
|
||||
x1 = ac3 * b6 >> 13;
|
||||
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
|
||||
x3 = ((x1 + x2) + 2) >> 2;
|
||||
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
|
||||
b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
|
||||
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
|
||||
|
||||
x1 = (p >> 8) * (p >> 8);
|
||||
x1 = (x1 * 3038) >> 16;
|
||||
x2 = (-7357 * p) >> 16;
|
||||
Press = p + ((x1 + x2 + 3791) >> 4);
|
||||
}
|
||||
|
||||
|
||||
// make one instance for the user to use
|
||||
APM_BMP085_Class APM_BMP085;
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
#ifndef APM_BMP085_h
|
||||
#define APM_BMP085_h
|
||||
|
||||
|
||||
class APM_BMP085_Class
|
||||
{
|
||||
private:
|
||||
// State machine
|
||||
uint8_t BMP085_State;
|
||||
// Internal calibration registers
|
||||
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
|
||||
uint16_t ac4, ac5, ac6;
|
||||
void Command_ReadPress();
|
||||
void Command_ReadTemp();
|
||||
void ReadPress();
|
||||
void ReadTemp();
|
||||
void Calculate();
|
||||
public:
|
||||
int32_t RawPress;
|
||||
int32_t RawTemp;
|
||||
int16_t Temp;
|
||||
int32_t Press;
|
||||
//int Altitude;
|
||||
uint8_t oss;
|
||||
//int32_t Press0; // Pressure at sea level
|
||||
|
||||
APM_BMP085_Class(); // Constructor
|
||||
void Init();
|
||||
uint8_t Read();
|
||||
};
|
||||
|
||||
extern APM_BMP085_Class APM_BMP085;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
Example of APM_BMP085 (absolute pressure sensor) library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <Wire.h>
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
|
||||
unsigned long timer;
|
||||
|
||||
void setup()
|
||||
{
|
||||
APM_BMP085.Init(); // APM ADC initialization
|
||||
Serial.begin(57600);
|
||||
Serial.println("ArduPilot Mega BMP085 library test");
|
||||
delay(1000);
|
||||
timer = millis();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int ch;
|
||||
float tmp_float;
|
||||
float Altitude;
|
||||
|
||||
if((millis()- timer) > 50)
|
||||
{
|
||||
timer=millis();
|
||||
APM_BMP085.Read();
|
||||
Serial.print("Pressure:");
|
||||
Serial.print(APM_BMP085.Press);
|
||||
Serial.print(" Temperature:");
|
||||
Serial.print(APM_BMP085.Temp/10.0);
|
||||
Serial.print(" Altitude:");
|
||||
tmp_float = (APM_BMP085.Press/101325.0);
|
||||
tmp_float = pow(tmp_float,0.190295);
|
||||
Altitude = 44330*(1.0-tmp_float);
|
||||
Serial.print(Altitude);
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
APM_BMP085 KEYWORD1
|
||||
Init KEYWORD2
|
||||
Read KEYWORD2
|
||||
Press KEYWORD2
|
||||
Temp KEYWORD2
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
/*
|
||||
APM_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor is conected to I2C port
|
||||
Sensor is initialized in Continuos mode (10Hz)
|
||||
|
||||
Variables:
|
||||
Heading : Magnetic heading
|
||||
Heading_X : Magnetic heading X component
|
||||
Heading_Y : Magnetic heading Y component
|
||||
Mag_X : Raw X axis magnetometer data
|
||||
Mag_Y : Raw Y axis magnetometer data
|
||||
Mag_Z : Raw Z axis magnetometer data
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of I2C and sensor
|
||||
Read() : Read Sensor data
|
||||
|
||||
To do : Calibration of the sensor, code optimization
|
||||
Mount position : Big capacitor pointing forward, connector backward
|
||||
|
||||
*/
|
||||
extern "C" {
|
||||
// AVR LibC Includes
|
||||
#include <math.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
|
||||
#include <Wire.h>
|
||||
#include "APM_Compass.h"
|
||||
|
||||
#define CompassAddress 0x1E
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
APM_Compass_Class::APM_Compass_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void APM_Compass_Class::Init(void)
|
||||
{
|
||||
Wire.begin();
|
||||
Wire.beginTransmission(CompassAddress);
|
||||
Wire.send(0x02);
|
||||
Wire.send(0x00); // Set continouos mode (default to 10Hz)
|
||||
Wire.endTransmission(); //end transmission
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
void APM_Compass_Class::Read()
|
||||
{
|
||||
int i = 0;
|
||||
byte buff[6];
|
||||
|
||||
Wire.beginTransmission(CompassAddress);
|
||||
Wire.send(0x03); //sends address to read from
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
//Wire.beginTransmission(CompassAddress);
|
||||
Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device
|
||||
while(Wire.available())
|
||||
{
|
||||
buff[i] = Wire.receive(); // receive one byte
|
||||
i++;
|
||||
}
|
||||
Wire.endTransmission(); //end transmission
|
||||
|
||||
if (i==6) // All bytes received?
|
||||
{
|
||||
// MSB byte first, then LSB, X,Y,Z
|
||||
Mag_X = ((((int)buff[0]) << 8) | buff[1]); // X axis
|
||||
Mag_Y = -((((int)buff[2]) << 8) | buff[3]); // Y axis
|
||||
Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis
|
||||
}
|
||||
}
|
||||
|
||||
void APM_Compass_Class::Calculate(float roll, float pitch)
|
||||
{
|
||||
float Head_X;
|
||||
float Head_Y;
|
||||
float cos_roll;
|
||||
float sin_roll;
|
||||
float cos_pitch;
|
||||
float sin_pitch;
|
||||
|
||||
cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM?
|
||||
sin_roll = sin(roll);
|
||||
cos_pitch = cos(pitch);
|
||||
sin_pitch = sin(pitch);
|
||||
// Tilt compensated Magnetic field X component:
|
||||
Head_X = Mag_X*cos_pitch+Mag_Y*sin_roll*sin_pitch+Mag_Z*cos_roll*sin_pitch;
|
||||
// Tilt compensated Magnetic field Y component:
|
||||
Head_Y = Mag_Y*cos_roll-Mag_Z*sin_roll;
|
||||
// Magnetic Heading
|
||||
Heading = atan2(-Head_Y,Head_X);
|
||||
// Optimization for external DCM use. Calculate normalized components
|
||||
Heading_X = cos(Heading);
|
||||
Heading_Y = sin(Heading);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// make one instance for the user to use
|
||||
APM_Compass_Class APM_Compass;
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef APM_Compass_h
|
||||
#define APM_Compass_h
|
||||
|
||||
class APM_Compass_Class
|
||||
{
|
||||
private:
|
||||
public:
|
||||
int Mag_X;
|
||||
int Mag_Y;
|
||||
int Mag_Z;
|
||||
float Heading;
|
||||
float Heading_X;
|
||||
float Heading_Y;
|
||||
|
||||
APM_Compass_Class(); // Constructor
|
||||
void Init();
|
||||
void Read();
|
||||
void Calculate(float roll, float pitch);
|
||||
};
|
||||
|
||||
extern APM_Compass_Class APM_Compass;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
Example of APM_Compass library (HMC5843 sensor).
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <Wire.h>
|
||||
#include <APM_Compass.h> // Compass Library
|
||||
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
unsigned long timer;
|
||||
|
||||
void setup()
|
||||
{
|
||||
APM_Compass.Init(); // Initialization
|
||||
Serial.begin(57600);
|
||||
Serial.println("Compass library test (HMC5843)");
|
||||
delay(1000);
|
||||
timer = millis();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
float tmp;
|
||||
|
||||
if((millis()- timer) > 100)
|
||||
{
|
||||
timer = millis();
|
||||
APM_Compass.Read();
|
||||
APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example
|
||||
Serial.print("Heading:");
|
||||
Serial.print(ToDeg(APM_Compass.Heading));
|
||||
Serial.print(" (");
|
||||
Serial.print(APM_Compass.Mag_X);
|
||||
Serial.print(",");
|
||||
Serial.print(APM_Compass.Mag_Y);
|
||||
Serial.print(",");
|
||||
Serial.print(APM_Compass.Mag_Z);
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
APM_Compass KEYWORD1
|
||||
Init KEYWORD2
|
||||
Read KEYWORD2
|
||||
Calculate KEYWORD2
|
||||
Heading KEYWORD2
|
||||
Heading_X KEYWORD2
|
||||
Heading_Y KEYWORD2
|
||||
Mag_X KEYWORD2
|
||||
Mag_Y KEYWORD2
|
||||
Mag_Z KEYWORD2
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
/*
|
||||
APM_RC_QUAD.cpp - Radio Control Library for Ardupilot Mega. Arduino
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This is a special version of the library for use in Quadcopters
|
||||
Because we have modified servo outputs 0..3 to have a higher rate (300Hz)
|
||||
|
||||
RC Input : PPM signal on IC4 pin
|
||||
RC Output : 8 servo Outputs,
|
||||
OUT0..OUT3 : 300Hz Servo output (for Quad ESC´s)
|
||||
OUT4..OUT7 : standard 50Hz servo outputs
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of interrupts an Timers
|
||||
OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
|
||||
InputCh(ch) : Read a channel input value. ch=0..7
|
||||
GetState() : Returns the state of the input. 1 => New radio frame to process
|
||||
Automatically resets when we call InputCh to read channels
|
||||
|
||||
*/
|
||||
#include "APM_RC_QUAD.h"
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include "WProgram.h"
|
||||
|
||||
// Variable definition for Input Capture interrupt
|
||||
volatile unsigned int ICR4_old;
|
||||
volatile unsigned char PPM_Counter=0;
|
||||
volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400};
|
||||
volatile unsigned char radio_status=0;
|
||||
|
||||
/****************************************************
|
||||
Input Capture Interrupt ICP4 => PPM signal read
|
||||
****************************************************/
|
||||
ISR(TIMER4_CAPT_vect)
|
||||
{
|
||||
unsigned int Pulse;
|
||||
unsigned int Pulse_Width;
|
||||
|
||||
Pulse=ICR4;
|
||||
if (Pulse<ICR4_old) // Take care of the overflow of Timer4 (TOP=40000)
|
||||
Pulse_Width=(Pulse + 40000)-ICR4_old; //Calculating pulse
|
||||
else
|
||||
Pulse_Width=Pulse-ICR4_old; //Calculating pulse
|
||||
if (Pulse_Width>8000) // SYNC pulse?
|
||||
PPM_Counter=0;
|
||||
else
|
||||
{
|
||||
PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7)
|
||||
PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse.
|
||||
if (PPM_Counter >= NUM_CHANNELS)
|
||||
radio_status = 1;
|
||||
}
|
||||
ICR4_old = Pulse;
|
||||
}
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
|
||||
APM_RC_QUAD_Class::APM_RC_QUAD_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void APM_RC_QUAD_Class::Init(void)
|
||||
{
|
||||
// Init PWM Timer 1
|
||||
//pinMode(11,OUTPUT); // (PB5/OC1A)
|
||||
pinMode(12,OUTPUT); //OUT2 (PB6/OC1B)
|
||||
pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
|
||||
|
||||
//Remember the registers not declared here remains zero by default...
|
||||
TCCR1A =((1<<WGM11)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
|
||||
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
|
||||
//OCR1A = 3000; //PB5, none
|
||||
OCR1B = 3000; //PB6, OUT2
|
||||
OCR1C = 3000; //PB7 OUT3
|
||||
ICR1 = 6600; //300hz freq...
|
||||
|
||||
// Init PWM Timer 3
|
||||
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
|
||||
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
|
||||
//pinMode(4,OUTPUT); // (PE3/OC3A)
|
||||
TCCR3A =((1<<WGM31)|(1<<COM3B1)|(1<<COM3C1));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
//OCR3A = 3000; //PE3, NONE
|
||||
OCR3B = 3000; //PE4, OUT7
|
||||
OCR3C = 3000; //PE5, OUT6
|
||||
ICR3 = 40000; //50hz freq (standard servos)
|
||||
|
||||
// Init PWM Timer 5
|
||||
pinMode(44,OUTPUT); //OUT1 (PL5/OC5C)
|
||||
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
|
||||
//pinMode(46,OUTPUT); // (PL3/OC5A)
|
||||
|
||||
TCCR5A =((1<<WGM51)|(1<<COM5B1)|(1<<COM5C1));
|
||||
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
|
||||
//OCR5A = 3000; //PL3,
|
||||
OCR5B = 3000; //PL4, OUT0
|
||||
OCR5C = 3000; //PL5, OUT1
|
||||
ICR5 = 6600; //300hz freq
|
||||
|
||||
// Init PPM input and PWM Timer 4
|
||||
pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input)
|
||||
pinMode(7,OUTPUT); //OUT5 (PH4/OC4B)
|
||||
pinMode(8,OUTPUT); //OUT4 (PH5/OC4C)
|
||||
|
||||
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
|
||||
//Prescaler set to 8, that give us a resolution of 0.5us
|
||||
// Input Capture rising edge
|
||||
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
|
||||
|
||||
OCR4A = 40000; ///50hz freq. (standard servos)
|
||||
OCR4B = 3000; //PH4, OUT5
|
||||
OCR4C = 3000; //PH5, OUT4
|
||||
|
||||
//TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge).
|
||||
//TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
|
||||
TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
|
||||
}
|
||||
|
||||
void APM_RC_QUAD_Class::OutputCh(unsigned char ch, int pwm)
|
||||
{
|
||||
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
pwm<<=1; // pwm*2;
|
||||
|
||||
switch(ch)
|
||||
{
|
||||
case 0: OCR5B=pwm; break; //ch0
|
||||
case 1: OCR5C=pwm; break; //ch1
|
||||
case 2: OCR1B=pwm; break; //ch2
|
||||
case 3: OCR1C=pwm; break; //ch3
|
||||
case 4: OCR4C=pwm; break; //ch4
|
||||
case 5: OCR4B=pwm; break; //ch5
|
||||
case 6: OCR3C=pwm; break; //ch6
|
||||
case 7: OCR3B=pwm; break; //ch7
|
||||
}
|
||||
}
|
||||
|
||||
int APM_RC_QUAD_Class::InputCh(unsigned char ch)
|
||||
{
|
||||
int result;
|
||||
int result2;
|
||||
|
||||
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
|
||||
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
|
||||
result = PWM_RAW[ch]>>1; // Because timer runs at 0.5us we need to do value/2
|
||||
result2 = PWM_RAW[ch]>>1;
|
||||
if (result != result2)
|
||||
result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine)
|
||||
|
||||
// Limit values to a valid range
|
||||
result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
radio_status=0; // Radio channel read
|
||||
return(result);
|
||||
}
|
||||
|
||||
unsigned char APM_RC_QUAD_Class::GetState(void)
|
||||
{
|
||||
return(radio_status);
|
||||
}
|
||||
|
||||
// make one instance for the user to use
|
||||
APM_RC_QUAD_Class APM_RC_QUAD;
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#ifndef APM_RC_QUAD_h
|
||||
#define APM_RC_QUAD_h
|
||||
|
||||
#define NUM_CHANNELS 8
|
||||
#define MIN_PULSEWIDTH 900
|
||||
#define MAX_PULSEWIDTH 2100
|
||||
|
||||
class APM_RC_QUAD_Class
|
||||
{
|
||||
private:
|
||||
public:
|
||||
APM_RC_QUAD_Class();
|
||||
void Init();
|
||||
void OutputCh(unsigned char ch, int pwm);
|
||||
int InputCh(unsigned char ch);
|
||||
unsigned char GetState();
|
||||
};
|
||||
|
||||
extern APM_RC_QUAD_Class APM_RC_QUAD;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
Example of APM_RC library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
Print Input values and send Output to the servos
|
||||
(Works with last PPM_encoder firmware)
|
||||
*/
|
||||
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
|
||||
void setup()
|
||||
{
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
Serial.begin(57600);
|
||||
Serial.println("ArduPilot Mega RC library test");
|
||||
delay(1000);
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20)
|
||||
{
|
||||
Serial.print("CH:");
|
||||
for(int i=0;i<8;i++)
|
||||
{
|
||||
Serial.print(APM_RC.InputCh(i)); // Print channel values
|
||||
Serial.print(",");
|
||||
APM_RC.OutputCh(i,APM_RC.InputCh(i)); // Copy input to Servos
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
APM_RC_QUAD KEYWORD1
|
||||
begin KEYWORD2
|
||||
InputCh KEYWORD2
|
||||
OutputCh KEYWORD2
|
||||
GetState KEYWORD2
|
||||
|
|
@ -0,0 +1,335 @@
|
|||
/*
|
||||
DataFlash.cpp - DataFlash log library for AT45DB161
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
This code works with boards based on ATMega168/328 and ATMega1280 using SPI port
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Dataflash library for AT45DB161D flash memory
|
||||
Memory organization : 4096 pages of 512 bytes
|
||||
|
||||
Maximun write bandwidth : 512 bytes in 14ms
|
||||
This code is written so the master never has to wait to write the data on the eeprom
|
||||
|
||||
Methods:
|
||||
Init() : Library initialization (SPI initialization)
|
||||
StartWrite(page) : Start a write session. page=start page.
|
||||
WriteByte(data) : Write a byte
|
||||
WriteInt(data) : Write an integer (2 bytes)
|
||||
WriteLong(data) : Write a long (4 bytes)
|
||||
StartRead(page) : Start a read on (page)
|
||||
GetPage() : Returns the last page read
|
||||
ReadByte()
|
||||
ReadInt()
|
||||
ReadLong()
|
||||
|
||||
Properties:
|
||||
|
||||
*/
|
||||
|
||||
extern "C" {
|
||||
// AVR LibC Includes
|
||||
#include <inttypes.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
|
||||
#include "DataFlash.h"
|
||||
|
||||
#define OVERWRITE_DATA 0 // 0: When reach the end page stop, 1: Start overwritten from page 1
|
||||
|
||||
// *** INTERNAL FUNCTIONS ***
|
||||
unsigned char dataflash_SPI_transfer(unsigned char output)
|
||||
{
|
||||
SPDR = output; // Start the transmission
|
||||
while (!(SPSR & (1<<SPIF))); // Wait the end of the transmission
|
||||
return SPDR;
|
||||
}
|
||||
|
||||
void dataflash_CS_inactive()
|
||||
{
|
||||
digitalWrite(DF_SLAVESELECT,HIGH); //disable device
|
||||
}
|
||||
|
||||
void dataflash_CS_active()
|
||||
{
|
||||
digitalWrite(DF_SLAVESELECT,LOW); //enable device
|
||||
}
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
DataFlash_Class::DataFlash_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void DataFlash_Class::Init(void)
|
||||
{
|
||||
byte tmp;
|
||||
|
||||
pinMode(DF_DATAOUT, OUTPUT);
|
||||
pinMode(DF_DATAIN, INPUT);
|
||||
pinMode(DF_SPICLOCK,OUTPUT);
|
||||
pinMode(DF_SLAVESELECT,OUTPUT);
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
pinMode(DF_RESET,OUTPUT);
|
||||
// Reset the chip
|
||||
digitalWrite(DF_RESET,LOW);
|
||||
delay(1);
|
||||
digitalWrite(DF_RESET,HIGH);
|
||||
#endif
|
||||
|
||||
df_Read_END=false;
|
||||
|
||||
dataflash_CS_inactive(); //disable device
|
||||
|
||||
// Setup SPI Master, Mode 3, fosc/4 = 4MHz
|
||||
SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPHA)|(1<<CPOL);
|
||||
// Setup SPI Master, Mode 3, 2MHz
|
||||
//SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPHA)|(1<<CPOL)|(1<<SPR0);
|
||||
//SPSR = (1<<SPI2X);
|
||||
// Cleanup registers...
|
||||
tmp=SPSR;
|
||||
tmp=SPDR;
|
||||
}
|
||||
|
||||
// This function is mainly to test the device
|
||||
void DataFlash_Class::ReadManufacturerID()
|
||||
{
|
||||
byte tmp;
|
||||
|
||||
dataflash_CS_inactive(); // Reset dataflash command decoder
|
||||
dataflash_CS_active();
|
||||
|
||||
// Read manufacturer and ID command...
|
||||
dataflash_SPI_transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
|
||||
|
||||
df_manufacturer = dataflash_SPI_transfer(0xff);
|
||||
df_device_0 = dataflash_SPI_transfer(0xff);
|
||||
df_device_1 = dataflash_SPI_transfer(0xff);
|
||||
tmp = dataflash_SPI_transfer(0xff);
|
||||
}
|
||||
|
||||
// Read the status of the DataFlash
|
||||
byte DataFlash_Class::ReadStatus()
|
||||
{
|
||||
byte tmp;
|
||||
|
||||
dataflash_CS_inactive(); // Reset dataflash command decoder
|
||||
dataflash_CS_active();
|
||||
|
||||
// Read status command
|
||||
dataflash_SPI_transfer(DF_STATUS_REGISTER_READ);
|
||||
tmp = dataflash_SPI_transfer(0x00);
|
||||
return(tmp&0x80); // We only want to extract the READY/BUSY bit
|
||||
}
|
||||
|
||||
// Wait until DataFlash is in ready state...
|
||||
void DataFlash_Class::WaitReady()
|
||||
{
|
||||
while(!ReadStatus());
|
||||
}
|
||||
|
||||
void DataFlash_Class::PageToBuffer(unsigned char BufferNum, unsigned int PageAdr)
|
||||
{
|
||||
dataflash_CS_inactive();
|
||||
dataflash_CS_active();
|
||||
if (BufferNum==1)
|
||||
dataflash_SPI_transfer(DF_TRANSFER_PAGE_TO_BUFFER_1);
|
||||
else
|
||||
dataflash_SPI_transfer(DF_TRANSFER_PAGE_TO_BUFFER_2);
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr >> 6));
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr << 2));
|
||||
dataflash_SPI_transfer(0x00); // don´t care bytes
|
||||
|
||||
dataflash_CS_inactive(); //initiate the transfer
|
||||
dataflash_CS_active();
|
||||
|
||||
while(!ReadStatus()); //monitor the status register, wait until busy-flag is high
|
||||
}
|
||||
|
||||
void DataFlash_Class::BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait)
|
||||
{
|
||||
dataflash_CS_inactive(); // Reset dataflash command decoder
|
||||
dataflash_CS_active();
|
||||
|
||||
if (BufferNum==1)
|
||||
dataflash_SPI_transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE);
|
||||
else
|
||||
dataflash_SPI_transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE);
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr >> 6));
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr << 2));
|
||||
dataflash_SPI_transfer(0x00); // don´t care bytes
|
||||
|
||||
dataflash_CS_inactive(); //initiate the transfer
|
||||
dataflash_CS_active();
|
||||
|
||||
// Check if we need to wait to write the buffer to memory or we can continue...
|
||||
if (wait)
|
||||
while(!ReadStatus()); //monitor the status register, wait until busy-flag is high
|
||||
}
|
||||
|
||||
void DataFlash_Class::BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data)
|
||||
{
|
||||
dataflash_CS_inactive(); // Reset dataflash command decoder
|
||||
dataflash_CS_active();
|
||||
|
||||
if (BufferNum==1)
|
||||
dataflash_SPI_transfer(DF_BUFFER_1_WRITE);
|
||||
else
|
||||
dataflash_SPI_transfer(DF_BUFFER_2_WRITE);
|
||||
dataflash_SPI_transfer(0x00); //don't cares
|
||||
dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address
|
||||
dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address
|
||||
dataflash_SPI_transfer(Data); //write data byte
|
||||
}
|
||||
|
||||
unsigned char DataFlash_Class::BufferRead (unsigned char BufferNum, unsigned int IntPageAdr)
|
||||
{
|
||||
byte tmp;
|
||||
|
||||
dataflash_CS_inactive(); // Reset dataflash command decoder
|
||||
dataflash_CS_active();
|
||||
|
||||
if (BufferNum==1)
|
||||
dataflash_SPI_transfer(DF_BUFFER_1_READ);
|
||||
else
|
||||
dataflash_SPI_transfer(DF_BUFFER_2_READ);
|
||||
dataflash_SPI_transfer(0x00); //don't cares
|
||||
dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address
|
||||
dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address
|
||||
dataflash_SPI_transfer(0x00); //don't cares
|
||||
tmp = dataflash_SPI_transfer(0x00); //read data byte
|
||||
|
||||
return (tmp);
|
||||
}
|
||||
// *** END OF INTERNAL FUNCTIONS ***
|
||||
|
||||
void DataFlash_Class::PageErase (unsigned int PageAdr)
|
||||
{
|
||||
dataflash_CS_inactive(); //make sure to toggle CS signal in order
|
||||
dataflash_CS_active(); //to reset Dataflash command decoder
|
||||
dataflash_SPI_transfer(DF_PAGE_ERASE); // Command
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr >> 6)); //upper part of page address
|
||||
dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); //lower part of page address and MSB of int.page adr.
|
||||
dataflash_SPI_transfer(0x00); // "dont cares"
|
||||
dataflash_CS_inactive(); //initiate flash page erase
|
||||
dataflash_CS_active();
|
||||
while(!ReadStatus());
|
||||
}
|
||||
|
||||
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
||||
void DataFlash_Class::StartWrite(int PageAdr)
|
||||
{
|
||||
df_BufferNum=1;
|
||||
df_BufferIdx=0;
|
||||
df_PageAdr=PageAdr;
|
||||
df_Stop_Write=0;
|
||||
WaitReady();
|
||||
}
|
||||
|
||||
void DataFlash_Class::WriteByte(byte data)
|
||||
{
|
||||
if (!df_Stop_Write)
|
||||
{
|
||||
BufferWrite(df_BufferNum,df_BufferIdx,data);
|
||||
df_BufferIdx++;
|
||||
if (df_BufferIdx >= 512) // End of buffer?
|
||||
{
|
||||
df_BufferIdx=0;
|
||||
BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT
|
||||
df_PageAdr++;
|
||||
if (OVERWRITE_DATA==1)
|
||||
{
|
||||
if (df_PageAdr>=4096) // If we reach the end of the memory, start from the begining
|
||||
df_PageAdr = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (df_PageAdr>=4096) // If we reach the end of the memory, stop here
|
||||
df_Stop_Write=1;
|
||||
}
|
||||
|
||||
if (df_BufferNum==1) // Change buffer to continue writing...
|
||||
df_BufferNum=2;
|
||||
else
|
||||
df_BufferNum=1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_Class::WriteInt(int data)
|
||||
{
|
||||
WriteByte(data>>8); // High byte
|
||||
WriteByte(data&0xFF); // Low byte
|
||||
}
|
||||
|
||||
void DataFlash_Class::WriteLong(long data)
|
||||
{
|
||||
WriteByte(data>>24); // First byte
|
||||
WriteByte(data>>16);
|
||||
WriteByte(data>>8);
|
||||
WriteByte(data&0xFF); // Last byte
|
||||
}
|
||||
|
||||
// Get the last page read
|
||||
int DataFlash_Class::GetPage()
|
||||
{
|
||||
return(df_Read_PageAdr-1);
|
||||
}
|
||||
|
||||
void DataFlash_Class::StartRead(int PageAdr)
|
||||
{
|
||||
df_Read_BufferNum=1;
|
||||
df_Read_BufferIdx=0;
|
||||
df_Read_PageAdr=PageAdr;
|
||||
WaitReady();
|
||||
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer
|
||||
df_Read_PageAdr++;
|
||||
}
|
||||
|
||||
byte DataFlash_Class::ReadByte()
|
||||
{
|
||||
byte result;
|
||||
|
||||
WaitReady();
|
||||
result = BufferRead(df_Read_BufferNum,df_Read_BufferIdx);
|
||||
df_Read_BufferIdx++;
|
||||
if (df_Read_BufferIdx >= 512) // End of buffer?
|
||||
{
|
||||
df_Read_BufferIdx=0;
|
||||
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer
|
||||
df_Read_PageAdr++;
|
||||
if (df_Read_PageAdr>=4096) // If we reach the end of the memory, start from the begining
|
||||
{
|
||||
df_Read_PageAdr = 0;
|
||||
df_Read_END = true;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int DataFlash_Class::ReadInt()
|
||||
{
|
||||
int result;
|
||||
|
||||
result = ReadByte(); // High byte
|
||||
result = (result<<8) | ReadByte(); // Low byte
|
||||
return result;
|
||||
}
|
||||
|
||||
long DataFlash_Class::ReadLong()
|
||||
{
|
||||
long result;
|
||||
|
||||
result = ReadByte(); // First byte
|
||||
result = (result<<8) | ReadByte();
|
||||
result = (result<<8) | ReadByte();
|
||||
result = (result<<8) | ReadByte(); // Last byte
|
||||
return result;
|
||||
}
|
||||
|
||||
// make one instance for the user to use
|
||||
DataFlash_Class DataFlash;
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/* ************************************************************ */
|
||||
/* Test for DataFlash Log library */
|
||||
/* ************************************************************ */
|
||||
#ifndef DataFlash_h
|
||||
#define DataFlash_h
|
||||
|
||||
// arduino mega SPI pins
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
#define DF_DATAOUT 51 // MOSI
|
||||
#define DF_DATAIN 50 // MISO
|
||||
#define DF_SPICLOCK 52 // SCK
|
||||
#define DF_SLAVESELECT 53 // SS (PB0)
|
||||
#define DF_RESET 31 // RESET (PC6)
|
||||
#else // normal arduino SPI pins...
|
||||
#define DF_DATAOUT 11 //MOSI
|
||||
#define DF_DATAIN 12 //MISO
|
||||
#define DF_SPICLOCK 13 //SCK
|
||||
#define DF_SLAVESELECT 10 //SS
|
||||
#endif
|
||||
|
||||
// AT45DB161D Commands (from Datasheet)
|
||||
#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53
|
||||
#define DF_TRANSFER_PAGE_TO_BUFFER_2 0x55
|
||||
#define DF_STATUS_REGISTER_READ 0xD7
|
||||
#define DF_READ_MANUFACTURER_AND_DEVICE_ID 0x9F
|
||||
#define DF_PAGE_READ 0xD2
|
||||
#define DF_BUFFER_1_READ 0xD4
|
||||
#define DF_BUFFER_2_READ 0xD6
|
||||
#define DF_BUFFER_1_WRITE 0x84
|
||||
#define DF_BUFFER_2_WRITE 0x87
|
||||
#define DF_BUFFER_1_TO_PAGE_WITH_ERASE 0x83
|
||||
#define DF_BUFFER_2_TO_PAGE_WITH_ERASE 0x86
|
||||
#define DF_PAGE_ERASE 0x81
|
||||
#define DF_BLOCK_ERASE 0x50
|
||||
#define DF_SECTOR_ERASE 0x7C
|
||||
#define DF_CHIP_ERASE_0 0xC7
|
||||
#define DF_CHIP_ERASE_1 0x94
|
||||
#define DF_CHIP_ERASE_2 0x80
|
||||
#define DF_CHIP_ERASE_3 0x9A
|
||||
|
||||
class DataFlash_Class
|
||||
{
|
||||
private:
|
||||
// DataFlash Log variables...
|
||||
unsigned char df_BufferNum;
|
||||
unsigned char df_Read_BufferNum;
|
||||
unsigned int df_BufferIdx;
|
||||
unsigned int df_Read_BufferIdx;
|
||||
unsigned int df_PageAdr;
|
||||
unsigned int df_Read_PageAdr;
|
||||
unsigned char df_Read_END;
|
||||
unsigned char df_Stop_Write;
|
||||
//Methods
|
||||
unsigned char BufferRead (unsigned char BufferNum, unsigned int IntPageAdr);
|
||||
void BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data);
|
||||
void BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait);
|
||||
void PageToBuffer(unsigned char BufferNum, unsigned int PageAdr);
|
||||
void WaitReady();
|
||||
unsigned char ReadStatus();
|
||||
|
||||
public:
|
||||
unsigned char df_manufacturer;
|
||||
unsigned char df_device_0;
|
||||
unsigned char df_device_1;
|
||||
|
||||
DataFlash_Class(); // Constructor
|
||||
void Init();
|
||||
void ReadManufacturerID();
|
||||
int GetPage();
|
||||
void PageErase (unsigned int PageAdr);
|
||||
// Write methods
|
||||
void StartWrite(int PageAdr);
|
||||
void WriteByte(unsigned char data);
|
||||
void WriteInt(int data);
|
||||
void WriteLong(long data);
|
||||
// Read methods
|
||||
void StartRead(int PageAdr);
|
||||
unsigned char ReadByte();
|
||||
int ReadInt();
|
||||
long ReadLong();
|
||||
};
|
||||
|
||||
extern DataFlash_Class DataFlash;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
/*
|
||||
Example of DataFlash library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <DataFlash.h>
|
||||
|
||||
#define HEAD_BYTE1 0xA3
|
||||
#define HEAD_BYTE2 0x95
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(57600);
|
||||
DataFlash.Init(); // DataFlash initialization
|
||||
|
||||
Serial.println("Dataflash Log Test 1.0");
|
||||
|
||||
// Test
|
||||
delay(20);
|
||||
DataFlash.ReadManufacturerID();
|
||||
delay(10);
|
||||
Serial.print("Manufacturer:");
|
||||
Serial.print(int(DataFlash.df_manufacturer));
|
||||
Serial.print(",");
|
||||
Serial.print(int(DataFlash.df_device_0));
|
||||
Serial.print(",");
|
||||
Serial.print(int(DataFlash.df_device_1));
|
||||
Serial.println();
|
||||
|
||||
// We start to write some info (sequentialy) starting from page 1
|
||||
// This is similar to what we will do...
|
||||
DataFlash.StartWrite(1);
|
||||
Serial.println("Writing to flash... wait...");
|
||||
for (int i=0;i<1000;i++) // Write 1000 packets...
|
||||
{
|
||||
// We write packets of binary data... (without worry about nothing more)
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteInt(2000+i);
|
||||
DataFlash.WriteInt(2001+i);
|
||||
DataFlash.WriteInt(2002+i);
|
||||
DataFlash.WriteInt(2003+i);
|
||||
DataFlash.WriteLong((long)i*5000);
|
||||
DataFlash.WriteLong((long)i*16268);
|
||||
DataFlash.WriteByte(0xA2); // 2 bytes of checksum (example)
|
||||
DataFlash.WriteByte(0x4E);
|
||||
delay(10);
|
||||
}
|
||||
delay(100);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int i;
|
||||
byte tmp_byte1;
|
||||
byte tmp_byte2;
|
||||
int tmp_int;
|
||||
long tmp_long;
|
||||
|
||||
Serial.println("Start reading page 1...");
|
||||
DataFlash.StartRead(1); // We start reading from page 1
|
||||
for (i=0;i<200;i++) // Read 200 packets...
|
||||
{
|
||||
tmp_byte1=DataFlash.ReadByte();
|
||||
tmp_byte2=DataFlash.ReadByte();
|
||||
Serial.print("PACKET:");
|
||||
if ((tmp_byte1==HEAD_BYTE1)&&(tmp_byte1==HEAD_BYTE1))
|
||||
{
|
||||
// Read 4 ints...
|
||||
tmp_int=DataFlash.ReadInt();
|
||||
Serial.print(tmp_int);
|
||||
Serial.print(",");
|
||||
tmp_int=DataFlash.ReadInt();
|
||||
Serial.print(tmp_int);
|
||||
Serial.print(",");
|
||||
tmp_int=DataFlash.ReadInt();
|
||||
Serial.print(tmp_int);
|
||||
Serial.print(",");
|
||||
tmp_int=DataFlash.ReadInt();
|
||||
Serial.print(tmp_int);
|
||||
Serial.print(",");
|
||||
|
||||
// Read 2 longs...
|
||||
tmp_long=DataFlash.ReadLong();
|
||||
Serial.print(tmp_long);
|
||||
Serial.print(",");
|
||||
tmp_long=DataFlash.ReadLong();
|
||||
Serial.print(tmp_long);
|
||||
Serial.print(";");
|
||||
|
||||
// Read the checksum...
|
||||
tmp_byte1=DataFlash.ReadByte();
|
||||
tmp_byte2=DataFlash.ReadByte();
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
delay(10000);
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
DataFlash KEYWORD1
|
||||
Init KEYWORD2
|
||||
ReadManufacturerID KEYWORD2
|
||||
GetPage KEYWORD2
|
||||
PageErase KEYWORD2
|
||||
StartWrite KEYWORD2
|
||||
StartRead KEYWORD2
|
||||
ReadByte KEYWORD2
|
||||
ReadInt KEYWORD2
|
||||
ReadLong KEYWORD2
|
||||
WriteByte KEYWORD2
|
||||
WriteInt KEYWORD2
|
||||
WriteLong KEYWORD2
|
||||
|
||||
|
|
@ -0,0 +1,272 @@
|
|||
/*
|
||||
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
GPS configuration : NMEA protocol
|
||||
Baud rate : 38400
|
||||
NMEA Sentences :
|
||||
$GPGGA : Global Positioning System Fix Data
|
||||
$GPVTG : Ttack and Ground Speed
|
||||
|
||||
Methods:
|
||||
Init() : GPS Initialization
|
||||
Read() : Call this funcion as often as you want to ensure you read the incomming gps data
|
||||
|
||||
Properties:
|
||||
Lattitude : Lattitude * 10000000 (long value)
|
||||
Longitude : Longitude * 10000000 (long value)
|
||||
Altitude : Altitude * 1000 (milimeters) (long value)
|
||||
Ground_speed : Speed (m/s) * 100 (long value)
|
||||
Ground_course : Course (degrees) * 100 (long value)
|
||||
Type : 2 (This indicate that we are using the Generic NMEA library)
|
||||
NewData : 1 when a new data is received.
|
||||
You need to write a 0 to NewData when you read the data
|
||||
Fix : >=1: GPS FIX, 0: No Fix (normal logic)
|
||||
Quality : 0 = No Fix
|
||||
1 = Bad (Num sats < 5)
|
||||
2 = Poor
|
||||
3 = Medium
|
||||
4 = Good
|
||||
|
||||
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
|
||||
*/
|
||||
|
||||
#include "GPS_NMEA.h"
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include "WProgram.h"
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
GPS_NMEA_Class::GPS_NMEA_Class()
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void GPS_NMEA_Class::Init(void)
|
||||
{
|
||||
Type = 2;
|
||||
GPS_checksum_calc = false;
|
||||
bufferidx = 0;
|
||||
NewData=0;
|
||||
Fix=0;
|
||||
Quality=0;
|
||||
PrintErrors=0;
|
||||
// Initialize serial port
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
Serial1.begin(38400); // Serial port 1 on ATMega1280
|
||||
#else
|
||||
Serial.begin(38400);
|
||||
#endif
|
||||
}
|
||||
|
||||
// This code don´t wait for data, only proccess the data available on serial port
|
||||
// We can call this function on the main loop (50Hz loop)
|
||||
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
|
||||
void GPS_NMEA_Class::Read(void)
|
||||
{
|
||||
char c;
|
||||
int numc;
|
||||
int i;
|
||||
|
||||
|
||||
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
|
||||
numc = Serial1.available();
|
||||
#else
|
||||
numc = Serial.available();
|
||||
#endif
|
||||
if (numc > 0)
|
||||
for (i=0;i<numc;i++){
|
||||
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
|
||||
c = Serial1.read();
|
||||
#else
|
||||
c = Serial.read();
|
||||
#endif
|
||||
if (c == '$'){ // NMEA Start
|
||||
bufferidx = 0;
|
||||
buffer[bufferidx++] = c;
|
||||
GPS_checksum = 0;
|
||||
GPS_checksum_calc = true;
|
||||
continue;
|
||||
}
|
||||
if (c == '\r'){ // NMEA End
|
||||
buffer[bufferidx++] = 0;
|
||||
parse_nmea_gps();
|
||||
}
|
||||
else {
|
||||
if (bufferidx < (GPS_BUFFERSIZE-1)){
|
||||
if (c == '*')
|
||||
GPS_checksum_calc = false; // Checksum calculation end
|
||||
buffer[bufferidx++] = c;
|
||||
if (GPS_checksum_calc)
|
||||
GPS_checksum ^= c; // XOR
|
||||
}
|
||||
else
|
||||
bufferidx=0; // Buffer overflow : restart
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
void GPS_NMEA_Class::parse_nmea_gps(void)
|
||||
{
|
||||
byte NMEA_check;
|
||||
long aux_deg;
|
||||
long aux_min;
|
||||
char *parseptr;
|
||||
|
||||
|
||||
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
|
||||
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
||||
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
|
||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||
//Serial.println("buffer");
|
||||
NewData = 1; // New GPS Data
|
||||
parseptr = strchr(buffer, ',')+1;
|
||||
//parseptr = strchr(parseptr, ',')+1;
|
||||
Time = parsenumber(parseptr,2); // GPS UTC time hhmmss.ss
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
//
|
||||
aux_deg = parsedecimal(parseptr,2); // degrees
|
||||
aux_min = parsenumber(parseptr+2,4); // minutes (sexagesimal) => Convert to decimal
|
||||
Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5)
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
//
|
||||
if (*parseptr=='S')
|
||||
Lattitude = -1*Lattitude; // South Lattitudes are negative
|
||||
//
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
// W Longitudes are Negative
|
||||
aux_deg = parsedecimal(parseptr,3); // degrees
|
||||
aux_min = parsenumber(parseptr+3,4); // minutes (sexagesimal)
|
||||
Longitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000)
|
||||
//Longitude = -1*Longitude; // This Assumes that we are in W longitudes...
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
//
|
||||
if (*parseptr=='W')
|
||||
Longitude = -1*Longitude; // West Longitudes are negative
|
||||
//
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
Fix = parsedecimal(parseptr,1);
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
NumSats = parsedecimal(parseptr,2);
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
HDOP = parsenumber(parseptr,1); // HDOP * 10
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
Altitude = parsenumber(parseptr,1)*100; // Altitude in decimeters*100 = milimeters
|
||||
if (Fix < 1)
|
||||
Quality = 0; // No FIX
|
||||
else if(NumSats<5)
|
||||
Quality = 1; // Bad (Num sats < 5)
|
||||
else if(HDOP>30)
|
||||
Quality = 2; // Poor (HDOP > 30)
|
||||
else if(HDOP>25)
|
||||
Quality = 3; // Medium (HDOP > 25)
|
||||
else
|
||||
Quality = 4; // Good (HDOP < 25)
|
||||
}
|
||||
else
|
||||
{
|
||||
if (PrintErrors)
|
||||
Serial.println("GPSERR: Checksum error!!");
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
|
||||
//Serial.println(buffer);
|
||||
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
||||
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
|
||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||
parseptr = strchr(buffer, ',')+1;
|
||||
Ground_Course = parsenumber(parseptr,2); // Ground course in degrees * 100
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100)
|
||||
//GPS_line = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (PrintErrors)
|
||||
Serial.println("GPSERR: Checksum error!!");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bufferidx = 0;
|
||||
if (PrintErrors)
|
||||
Serial.println("GPSERR: Bad sentence!!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Parse hexadecimal numbers
|
||||
byte GPS_NMEA_Class::parseHex(char c) {
|
||||
if (c < '0')
|
||||
return (0);
|
||||
if (c <= '9')
|
||||
return (c - '0');
|
||||
if (c < 'A')
|
||||
return (0);
|
||||
if (c <= 'F')
|
||||
return ((c - 'A')+10);
|
||||
}
|
||||
|
||||
// Decimal number parser
|
||||
long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) {
|
||||
long d = 0;
|
||||
byte i;
|
||||
|
||||
i = num_car;
|
||||
while ((str[0] != 0)&&(i>0)) {
|
||||
if ((str[0] > '9') || (str[0] < '0'))
|
||||
return d;
|
||||
d *= 10;
|
||||
d += str[0] - '0';
|
||||
str++;
|
||||
i--;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
// Function to parse fixed point numbers (numdec=number of decimals)
|
||||
long GPS_NMEA_Class::parsenumber(char *str,byte numdec) {
|
||||
long d = 0;
|
||||
byte ndec = 0;
|
||||
|
||||
while (str[0] != 0) {
|
||||
if (str[0] == '.'){
|
||||
ndec = 1;
|
||||
}
|
||||
else {
|
||||
if ((str[0] > '9') || (str[0] < '0'))
|
||||
return d;
|
||||
d *= 10;
|
||||
d += str[0] - '0';
|
||||
if (ndec > 0)
|
||||
ndec++;
|
||||
if (ndec > numdec) // we reach the number of decimals...
|
||||
return d;
|
||||
}
|
||||
str++;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
GPS_NMEA_Class GPS;
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
#ifndef GPS_NMEA_h
|
||||
#define GPS_NMEA_h
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define GPS_BUFFERSIZE 120
|
||||
|
||||
class GPS_NMEA_Class
|
||||
{
|
||||
private:
|
||||
// Internal variables
|
||||
uint8_t GPS_checksum;
|
||||
uint8_t GPS_checksum_calc;
|
||||
char buffer[GPS_BUFFERSIZE];
|
||||
int bufferidx;
|
||||
|
||||
void parse_nmea_gps(void);
|
||||
uint8_t parseHex(char c);
|
||||
long parsedecimal(char *str,uint8_t num_car);
|
||||
long parsenumber(char *str,uint8_t numdec);
|
||||
|
||||
public:
|
||||
// Methods
|
||||
GPS_NMEA_Class();
|
||||
void Init();
|
||||
void Read();
|
||||
// Properties
|
||||
long Time; //GPS Millisecond Time of Week
|
||||
long Lattitude; // Geographic coordinates
|
||||
long Longitude;
|
||||
long Altitude;
|
||||
long Ground_Speed;
|
||||
long Speed_3d; //Speed (3-D)
|
||||
long Ground_Course;
|
||||
uint8_t Type; // Type of GPS (library used)
|
||||
uint8_t NumSats; // Number of visible satelites
|
||||
uint8_t Fix; // >=1:GPS FIX 0:No FIX (normal logic)
|
||||
uint8_t Quality; // GPS Signal quality
|
||||
uint8_t NewData; // 1:New GPS Data
|
||||
uint8_t PrintErrors; // 1: To Print GPS Errors (for debug)
|
||||
int HDOP; // HDOP
|
||||
};
|
||||
|
||||
extern GPS_NMEA_Class GPS;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
/*
|
||||
Example of GPS NMEA library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
|
||||
and with standard ATMega168 and ATMega328 on Serial Port 0
|
||||
*/
|
||||
|
||||
#include <GPS_NMEA.h> // NMEA GPS Library
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(57600);
|
||||
Serial.println("GPS NMEA library test");
|
||||
GPS.Init(); // GPS Initialization
|
||||
delay(1000);
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
GPS.Read();
|
||||
if (GPS.NewData) // New GPS data?
|
||||
{
|
||||
Serial.print("GPS:");
|
||||
Serial.print(" Time:");
|
||||
Serial.print(GPS.Time);
|
||||
Serial.print(" Fix:");
|
||||
Serial.print((int)GPS.Fix);
|
||||
Serial.print(" Lat:");
|
||||
Serial.print(GPS.Lattitude);
|
||||
Serial.print(" Lon:");
|
||||
Serial.print(GPS.Longitude);
|
||||
Serial.print(" Alt:");
|
||||
Serial.print(GPS.Altitude/1000.0);
|
||||
Serial.print(" Speed:");
|
||||
Serial.print(GPS.Ground_Speed/100.0);
|
||||
Serial.print(" Course:");
|
||||
Serial.print(GPS.Ground_Course/100.0);
|
||||
Serial.println();
|
||||
GPS.NewData = 0; // We have readed the data
|
||||
}
|
||||
delay(20);
|
||||
}
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
GPS KEYWORD1
|
||||
GPS_NMEA KEYWORD1
|
||||
Init KEYWORD2
|
||||
Read KEYWORD2
|
||||
Type KEYWORD2
|
||||
Time KEYWORD2
|
||||
Lattitude KEYWORD2
|
||||
Longitude KEYWORD2
|
||||
Altitude KEYWORD2
|
||||
Ground_Speed KETWORD2
|
||||
Ground_Course KEYWORD2
|
||||
Speed_3d KEYWORD2
|
||||
NumSats KEYWORD2
|
||||
Fix KEYWORD2
|
||||
NewData KEYWORD2
|
||||
Quality KEYWORD2
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,270 @@
|
|||
/*
|
||||
GPS_UBLOX.cpp - Ublox GPS library for Arduino
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
GPS configuration : Ublox protocol
|
||||
Baud rate : 38400
|
||||
Active messages :
|
||||
NAV-POSLLH Geodetic Position Solution, PAGE 66 of datasheet
|
||||
NAV-VELNED Velocity Solution in NED, PAGE 71 of datasheet
|
||||
NAV-STATUS Receiver Navigation Status
|
||||
or
|
||||
NAV-SOL Navigation Solution Information
|
||||
|
||||
Methods:
|
||||
Init() : GPS Initialization
|
||||
Read() : Call this funcion as often as you want to ensure you read the incomming gps data
|
||||
|
||||
Properties:
|
||||
Lattitude : Lattitude * 10000000 (long value)
|
||||
Longitude : Longitude * 10000000 (long value)
|
||||
Altitude : Altitude * 100 (meters) (long value)
|
||||
Ground_speed : Speed (m/s) * 100 (long value)
|
||||
Ground_course : Course (degrees) * 100 (long value)
|
||||
NewData : 1 when a new data is received.
|
||||
You need to write a 0 to NewData when you read the data
|
||||
Fix : 1: GPS FIX, 0: No Fix (normal logic)
|
||||
|
||||
*/
|
||||
|
||||
#include "GPS_UBLOX.h"
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include "WProgram.h"
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
GPS_UBLOX_Class::GPS_UBLOX_Class()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void GPS_UBLOX_Class::Init(void)
|
||||
{
|
||||
ck_a=0;
|
||||
ck_b=0;
|
||||
UBX_step=0;
|
||||
NewData=0;
|
||||
Fix=0;
|
||||
PrintErrors=0;
|
||||
GPS_timer=millis(); //Restarting timer...
|
||||
// Initialize serial port
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
Serial1.begin(38400); // Serial port 1 on ATMega1280
|
||||
#else
|
||||
Serial.begin(38400);
|
||||
#endif
|
||||
}
|
||||
|
||||
// optimization : This code don´t wait for data, only proccess the data available
|
||||
// We can call this function on the main loop (50Hz loop)
|
||||
// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
|
||||
void GPS_UBLOX_Class::Read(void)
|
||||
{
|
||||
static unsigned long GPS_timer=0;
|
||||
byte data;
|
||||
int numc;
|
||||
|
||||
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
|
||||
numc = Serial1.available();
|
||||
#else
|
||||
numc = Serial.available();
|
||||
#endif
|
||||
if (numc > 0)
|
||||
for (int i=0;i<numc;i++) // Process bytes received
|
||||
{
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
data = Serial1.read();
|
||||
#else
|
||||
data = Serial.read();
|
||||
#endif
|
||||
switch(UBX_step) //Normally we start from zero. This is a state machine
|
||||
{
|
||||
case 0:
|
||||
if(data==0xB5) // UBX sync char 1
|
||||
UBX_step++; //OH first data packet is correct, so jump to the next step
|
||||
break;
|
||||
case 1:
|
||||
if(data==0x62) // UBX sync char 2
|
||||
UBX_step++; //ooh! The second data packet is correct, jump to the step 2
|
||||
else
|
||||
UBX_step=0; //Nop, is not correct so restart to step zero and try again.
|
||||
break;
|
||||
case 2:
|
||||
UBX_class=data;
|
||||
ubx_checksum(UBX_class);
|
||||
UBX_step++;
|
||||
break;
|
||||
case 3:
|
||||
UBX_id=data;
|
||||
ubx_checksum(UBX_id);
|
||||
UBX_step++;
|
||||
break;
|
||||
case 4:
|
||||
UBX_payload_length_hi=data;
|
||||
ubx_checksum(UBX_payload_length_hi);
|
||||
UBX_step++;
|
||||
// We check if the payload lenght is valid...
|
||||
if (UBX_payload_length_hi>=UBX_MAXPAYLOAD)
|
||||
{
|
||||
if (PrintErrors)
|
||||
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
|
||||
UBX_step=0; //Bad data, so restart to step zero and try again.
|
||||
ck_a=0;
|
||||
ck_b=0;
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
UBX_payload_length_lo=data;
|
||||
ubx_checksum(UBX_payload_length_lo);
|
||||
UBX_step++;
|
||||
UBX_payload_counter=0;
|
||||
break;
|
||||
case 6: // Payload data read...
|
||||
if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length
|
||||
{
|
||||
UBX_buffer[UBX_payload_counter] = data;
|
||||
ubx_checksum(data);
|
||||
UBX_payload_counter++;
|
||||
if (UBX_payload_counter==UBX_payload_length_hi)
|
||||
UBX_step++;
|
||||
}
|
||||
break;
|
||||
case 7:
|
||||
UBX_ck_a=data; // First checksum byte
|
||||
UBX_step++;
|
||||
break;
|
||||
case 8:
|
||||
UBX_ck_b=data; // Second checksum byte
|
||||
|
||||
// We end the GPS read...
|
||||
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b)) // Verify the received checksum with the generated checksum..
|
||||
parse_ubx_gps(); // Parse the new GPS packet
|
||||
else
|
||||
{
|
||||
if (PrintErrors)
|
||||
Serial.println("ERR:GPS_CHK!!");
|
||||
}
|
||||
// Variable initialization
|
||||
UBX_step=0;
|
||||
ck_a=0;
|
||||
ck_b=0;
|
||||
GPS_timer=millis(); //Restarting timer...
|
||||
break;
|
||||
}
|
||||
} // End for...
|
||||
// If we don´t receive GPS packets in 2 seconds => Bad FIX state
|
||||
if ((millis() - GPS_timer)>2000)
|
||||
{
|
||||
Fix = 0;
|
||||
if (PrintErrors)
|
||||
Serial.println("ERR:GPS_TIMEOUT!!");
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
void GPS_UBLOX_Class::parse_ubx_gps(void)
|
||||
{
|
||||
int j;
|
||||
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
|
||||
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
|
||||
if(UBX_class==0x01)
|
||||
{
|
||||
switch(UBX_id)//Checking the UBX ID
|
||||
{
|
||||
case 0x02: //ID NAV-POSLLH
|
||||
j=0;
|
||||
Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week
|
||||
j+=4;
|
||||
Longitude = join_4_bytes(&UBX_buffer[j]); // lon*10000000
|
||||
j+=4;
|
||||
Lattitude = join_4_bytes(&UBX_buffer[j]); // lat*10000000
|
||||
j+=4;
|
||||
//Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm
|
||||
j+=4;
|
||||
Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm
|
||||
//j+=4;
|
||||
/*
|
||||
hacc = (float)join_4_bytes(&UBX_buffer[j])/(float)1000;
|
||||
j+=4;
|
||||
vacc = (float)join_4_bytes(&UBX_buffer[j])/(float)1000;
|
||||
j+=4;
|
||||
*/
|
||||
NewData=1;
|
||||
break;
|
||||
case 0x03://ID NAV-STATUS
|
||||
//if(UBX_buffer[4] >= 0x03)
|
||||
if((UBX_buffer[4] >= 0x03)&&(UBX_buffer[5]&0x01))
|
||||
Fix=1; //valid position
|
||||
else
|
||||
Fix=0; //invalid position
|
||||
break;
|
||||
|
||||
case 0x06://ID NAV-SOL
|
||||
if((UBX_buffer[10] >= 0x03)&&(UBX_buffer[11]&0x01))
|
||||
Fix=1; //valid position
|
||||
else
|
||||
Fix=0; //invalid position
|
||||
UBX_ecefVZ=join_4_bytes(&UBX_buffer[36]); //Vertical Speed in cm/s
|
||||
NumSats=UBX_buffer[47]; //Number of sats...
|
||||
break;
|
||||
|
||||
case 0x12:// ID NAV-VELNED
|
||||
j=16;
|
||||
Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm/s
|
||||
j+=4;
|
||||
Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm/s
|
||||
j+=4;
|
||||
Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg*100000
|
||||
j+=4;
|
||||
/*
|
||||
sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy
|
||||
j+=4;
|
||||
headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy
|
||||
j+=4;
|
||||
*/
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Join 4 bytes into a long
|
||||
long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[])
|
||||
{
|
||||
union long_union {
|
||||
int32_t dword;
|
||||
uint8_t byte[4];
|
||||
} longUnion;
|
||||
|
||||
longUnion.byte[0] = *Buffer;
|
||||
longUnion.byte[1] = *(Buffer+1);
|
||||
longUnion.byte[2] = *(Buffer+2);
|
||||
longUnion.byte[3] = *(Buffer+3);
|
||||
return(longUnion.dword);
|
||||
}
|
||||
|
||||
/****************************************************************
|
||||
*
|
||||
****************************************************************/
|
||||
// Ublox checksum algorithm
|
||||
void GPS_UBLOX_Class::ubx_checksum(byte ubx_data)
|
||||
{
|
||||
ck_a+=ubx_data;
|
||||
ck_b+=ck_a;
|
||||
}
|
||||
|
||||
GPS_UBLOX_Class GPS;
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
#ifndef GPS_UBLOX_h
|
||||
#define GPS_UBLOX_h
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define UBX_MAXPAYLOAD 60
|
||||
|
||||
class GPS_UBLOX_Class
|
||||
{
|
||||
private:
|
||||
// Internal variables
|
||||
uint8_t ck_a; // Packet checksum
|
||||
uint8_t ck_b;
|
||||
uint8_t UBX_step;
|
||||
uint8_t UBX_class;
|
||||
uint8_t UBX_id;
|
||||
uint8_t UBX_payload_length_hi;
|
||||
uint8_t UBX_payload_length_lo;
|
||||
uint8_t UBX_payload_counter;
|
||||
uint8_t UBX_buffer[UBX_MAXPAYLOAD];
|
||||
uint8_t UBX_ck_a;
|
||||
uint8_t UBX_ck_b;
|
||||
long GPS_timer;
|
||||
long UBX_ecefVZ;
|
||||
void parse_ubx_gps();
|
||||
void ubx_checksum(unsigned char ubx_data);
|
||||
long join_4_bytes(unsigned char Buffer[]);
|
||||
|
||||
public:
|
||||
// Methods
|
||||
GPS_UBLOX_Class();
|
||||
void Init();
|
||||
void Read();
|
||||
// Properties
|
||||
long Time; //GPS Millisecond Time of Week
|
||||
long Lattitude; // Geographic coordinates
|
||||
long Longitude;
|
||||
long Altitude;
|
||||
long Ground_Speed;
|
||||
long Speed_3d; //Speed (3-D)
|
||||
long Ground_Course;
|
||||
uint8_t NumSats; // Number of visible satelites
|
||||
uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic)
|
||||
uint8_t NewData; // 1:New GPS Data
|
||||
uint8_t PrintErrors; // 1: To Print GPS Errors (for debug)
|
||||
};
|
||||
|
||||
extern GPS_UBLOX_Class GPS;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
/*
|
||||
Example of GPS UBLOX library.
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
|
||||
and with standard ATMega168 and ATMega328 on Serial Port 0
|
||||
*/
|
||||
|
||||
#include <GPS_UBLOX.h> // UBLOX GPS Library
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(57600);
|
||||
Serial.println("GPS UBLOX library test");
|
||||
GPS.Init(); // GPS Initialization
|
||||
delay(1000);
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
GPS.Read();
|
||||
if (GPS.NewData) // New GPS data?
|
||||
{
|
||||
Serial.print("GPS:");
|
||||
Serial.print(" Time:");
|
||||
Serial.print(GPS.Time);
|
||||
Serial.print(" Fix:");
|
||||
Serial.print((int)GPS.Fix);
|
||||
Serial.print(" Lat:");
|
||||
Serial.print(GPS.Lattitude);
|
||||
Serial.print(" Lon:");
|
||||
Serial.print(GPS.Longitude);
|
||||
Serial.print(" Alt:");
|
||||
Serial.print(GPS.Altitude/1000.0);
|
||||
Serial.print(" Speed:");
|
||||
Serial.print(GPS.Ground_Speed/100.0);
|
||||
Serial.print(" Course:");
|
||||
Serial.print(GPS.Ground_Course/100000.0);
|
||||
Serial.println();
|
||||
GPS.NewData = 0; // We have readed the data
|
||||
}
|
||||
delay(20);
|
||||
}
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
GPS KEYWORD1
|
||||
GPS_UBLOX KEYWORD1
|
||||
Init KEYWORD2
|
||||
Read KEYWORD2
|
||||
Time KEYWORD2
|
||||
Lattitude KEYWORD2
|
||||
Longitude KEYWORD2
|
||||
Altitude KEYWORD2
|
||||
Ground_Speed KETWORD2
|
||||
Ground_Course KEYWORD2
|
||||
Speed_3d KEYWORD2
|
||||
NumSats KEYWORD2
|
||||
Fix KEYWORD2
|
||||
NewData KEYWORD2
|
||||
|
||||
|
||||
Loading…
Reference in New Issue