Darrien Galid
Self-navigating boat program with sensors and control system fused
Diff: main.cpp
- Revision:
- 1:736ae4695570
- Parent:
- 0:cf5854b3296f
--- a/main.cpp Fri May 10 08:45:41 2019 +0000
+++ b/main.cpp Fri May 24 16:12:40 2019 +0000
@@ -2,47 +2,46 @@
#include "MPU9250.h"
#include "math.h"
#include "TinyGPSplus.h"
+#include "Servo.h"
+#include "PID.h"
+
+Servo motor_servo(PTB1); // Boat throttle servo PWM
+Servo rudder_servo(PTB0); // Boat rudder servo PWM
+DigitalOut led(PTD3);
+MPU9250 mpu9250; // Instantiate MPU9250 class
+Timer t; // setup a timer
+Serial device(PTE0, PTE1); //RF Device tx and rx at PTE0 and PTE1
+Serial pc(USBTX, USBRX); // Serial communication to display resuslts on PC
+TinyGPSPlus tgps; // Instantiate TinyGPS class
+PID velocity_controller(5.0, 1.0, 0.4, 0.1);
+PID heading_controller(5.0, 2.0, 0.4, 0.1);
#define TX5 PTD3
#define RX5 PTD2
#define GPSBaud 9600
-#define destinate_lat 1.533166
-#define destinate_lng 110.357162
+#define destinate_lat 1.532277
+#define destinate_lng 110.357754
-DigitalOut led(PTD3);
- MPU9250 mpu9250; // Instantiate MPU9250 class
-
- Timer t; // setup a timer
-
- Serial pc(USBTX, USBRX); // Serial communication to display resuslts on PC
- Serial GPSSerial(TX5, RX5); //Serial communication to read data from GPS
- TinyGPSPlus tgps; // Instantiate TinyGPS class
-
-
+Serial GPSSerial(TX5, RX5); //Serial communication to read data from GPS
float p1,p2,p3,p4,a1,a2,a3,a4,q1i,q2i,q3i,q4i,qnorm,a2o=0,a3o=0,a4o=0,axf=0,ayf=0,azf=0,l=1.0,now_vel=0,last_vel=0;
uint32_t movwind=0, ready = 0,GPS_FLAG=0,axzerocount=0,ayzerocount=0,start_time=90,stop_avg=0,count_gps_reads=0,onetime=0,print_count=0;
char buffer[14];
uint8_t whoami = 0;
float deltat = 0.0f,ax=0.0f,ay=0.0f,az=0.0f,mx=0.0f,my=0.0f,mz=0.0f,gx=0.0f,gy=0.0f,gz=0.0f;
-double D=0,Do = 0, H=0, prev_lat=0,perev_lng=0,GPSPNO=0,GPSPEO=0,GPSPN,GPSPNintial,GPSPEintial,GpsstartingptN,GpsstartingptE,GPSPNcurrent,GPSPEcurrent,GPSPE,GPSVNO,GPSVEO,GPSVN,GPSVE,accel_var=0.0015,GPS_VAR = 2;
+double D=0,Do = 0, H=0, prev_lat=0,perev_lng=0,GPSPNO=0,GPSPEO=0,GPSPN,GPSPNintial,GPSPEintial,GpsstartingptN,GpsstartingptE,GPSPNcurrent,GPSPEcurrent,GPSPE,GPSVNO,GPSVEO,GPSVN,GPSVE,accel_var=0.0023,accel_varb=0.0023,GPS_VAR = 500;
double gps_lato=0,gps_longo=0,gps_lat=0,gps_long=0,position_filt_N,position_unfilt_N,position_filt_E,position_unfilt_E,position_to_dest_N,position_to_dest_E,dest_P_N,dest_P_E,unposition_to_dest_N ,unposition_to_dest_E;
double actual_pk1N = 1,actual_pk2N = 0,actual_pk3N = 0,actual_pk4N = 1,actual_pk1E = 1,actual_pk2E = 0,actual_pk3E = 0,actual_pk4E = 1,predicted_lat = 0, predicted_lng = 0,dest = 0,corrected_ang,sum_avg_lng=0,sum_avg_lat=0;
+void ManualOverride(); // Switch the boat into manual control mode
void setup(); // used in calibration of MPU9250 and setting baud rates for serial com.
-
void Read_MPU9250_Raw();// used in reading raw accel, gyro, magneto in 2 axis from the MPU9250 (IMU)
// used to fuse the accelerometer with gyro and magnetometer readings to determine the orientation in quaternion form
void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat);
-
void compute_accel_world_ref();//Used to rotate the acceleration from the sensor fram to the world frame (NED) using quaternion
-
void compute_orientation();// used in computing the roll, Pitch and yaw from quaternion
-
void SerialInterruptHandler(void);// Interupt function where GPS data is read
-
double latitudetometers(double latitudetom);// convert long to meters (North) on Earth
-
double longitudetometers(double longitudetom);// convert long to meters (East) on Earth
// used to perform deadreckoning
@@ -50,73 +49,198 @@
// used to update the accumilative (IMU) readings with absolute GPS readings
void kalmanfilterupdate(double GPS_POS, double GPS_VEL, double &predicted_pos, double &predicted_vel, double &actual_pk1, double &actual_pk2, double &actual_pk3, double &actual_pk4);
-
void IMU_GPS_Fusion();// used in impleminting the Kalman filter to output predicted velocity and position.
-
float invSqrt(float x);// inverse square root function
-
+class USV {
+ private:
+
+ public:
+ float u; // Surge velocity
+ float v; // Sway velocity
+ float r; // Yaw velocity
+ float x; // Position in East
+ float y; // Position in North
+ float psi; // Yaw angle
+};
+
+class Setpoint {
+ private:
+
+ public:
+ float vel;
+ float north;
+ float east;
+ float psi;
+};
+
int main()
{
+ //
+ motor_servo = 0.0;
+ wait(0.5);
+ motor_servo = 1.0;
+ wait(1.0);
+ motor_servo = 0.0;
+ rudder_servo = 0.5;
+ float motor_out = 0.0;
+ float rudder_out = 0.0;
+ float dist = 0.0;
+ char command;
+ bool start_flag = false;
+ bool destination_flag = false;
+ int read_count = 0;
+ USV boat;
+ Setpoint sp;
+ device.baud(9600);
+ device.printf("Wireless UART connection initialised.\n\r");
- setup();// code to setup GPS/MPU9250
-
- /* Main Loop*/
- while(1)
- {
- Read_MPU9250_Raw();
- MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
- compute_accel_world_ref();
- compute_orientation();
-
- // GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
- //if(t.read_ms()/1000>(start_time-2))
- //stop_avg=1;
- if(t.read_ms()/1000>start_time)//90 seconds has passed in order to start taking reliable acceleration readings from the filter 90 sec = 60 calibration+30 filter stabilization
- {
-
- GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
- if(ready == 1)
- {
- IMU_GPS_Fusion();
-
- //**************************************
- //a2 //This variable is acceleration North
- //a3 //This variable is acceleration East
- //yaw //This variable is the yaw (Heading)
- //corrected_ang //This variable returns the angle needed to adjust in order to reach the destination (-VE need to rotate clockwise to reach dest, +ve need to rotate C.C.W to reach dest)
- //position_to_dest_N//This variable returns distance needed to reach the destination in North (meters).
- //position_to_dest_E//This variable returns distance needed to reach the destination in East.
- //position_filt_N //This variable returns how much you have moved in North (meters)
- //position_filt_E //This variable return how much you have moved in East (meters)
- //GPSVNO // This variable returns your velocity in North
- //GPSEN0 //This variable returns your velocity in East
- //*****************************************
-
- //a4 //This variable is acceleration Down
- //roll //This variable is the roll
- //Pitch // This variable is the Pitch
- //position_filt_N //This variable returns how much you have moved in North
- //position_filt_E //This variable return how much you have moved in East
- //GPSPNO //This variable returns your current position in the world in term of meters (North)
- //GPSPEO //This variable returns your current position in the world in term of meters (East
- //GpsstartingptN; //This variable returns your starting North position in terms of meters (Measured from latitude of GPS)
- //GpsstartingptE; //This variable returns your starting position East in terms of meters (Measured from latitude of GPS)
-
- if (print_count ==6)
- {
- printf ("D(VH) = %f, Angle = %f, vel = %f\n\r",sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E)),corrected_ang,sqrt((GPSVNO*GPSVNO)+(GPSVEO*GPSVEO)));
- //printf ("Fd = %.2f\n\r",sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E)));
- print_count = 0;
- }
- print_count++;
- //printf("%i\n\r",count_gps_reads);
- }
+ // Sensor setup
+ device.printf("Press 'r' to calibrate sensors.\n\r");
+ while (!start_flag) {
+ if (device.readable() != 0) command = device.getc();
+ if (command == 'r') start_flag = true;
+ }
+
+ device.printf("Calibrate command accepted.\n\r");
+ command = ' ';
+ start_flag = false;
+ setup();// code to setup GPS/MPU9250
+
+ while(t.read_ms()/1000<start_time) {
+ Read_MPU9250_Raw();
+ MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
+ compute_accel_world_ref();
+ compute_orientation();
+ }
+ device.printf("Sensor calibration done.\n\r");
+
+ // Wait for start command
+ device.printf("Press 'r' to set setpoint and start.\n\r");
+ while (!start_flag) {
+ if (device.readable() != 0) command = device.getc();
+ if (command == 'r') start_flag = true;
+ }
+
+ device.printf("Start command accepted.\n\r");
+ command = ' ';
+ start_flag = false;
+
+ // Declare setpoints
+ sp.vel = 1.0;
+ sp.north = destinate_lat;
+ sp.east = destinate_lng;
+ device.printf("Velocity setpoint is %f.\n\r", sp.vel);
+ device.printf("North setpoint is %f.\n\r", sp.north);
+ device.printf("East setpoint is %f.\n\r", sp.east);
+
+ velocity_controller.setOutputLimits(0.0, 0.12);
+ velocity_controller.setMode(AUTO_MODE);
+ velocity_controller.setSetPoint(sp.vel);
+ device.printf("Velocity controller initialised.\n\r");
+
+ heading_controller.setOutputLimits(-0.3, 0.3);
+ heading_controller.setMode(AUTO_MODE);
+ device.printf("Heading controller initialised.\n\r");
+
+ device.printf("Moving boat to destination.\n\r");
+
+ while(!destination_flag) {
+ // Update boat states from sensor
+ Read_MPU9250_Raw();
+ MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);
+ compute_accel_world_ref();
+ compute_orientation();
+ GPSSerial.attach(&SerialInterruptHandler, Serial::RxIrq);// Interrupt trigerrer
+ if (ready == 1) {
+
+ IMU_GPS_Fusion();
+ boat.psi = yaw*DEG_TO_RAD;
+ boat.u = sqrt((GPSVNO*GPSVNO) + (GPSVEO*GPSVEO));
+ boat.x = GPSPEO;
+ boat.y = GPSPNO;
- }
+ // Update heading setpoint
+ sp.psi = dest*DEG_TO_RAD;
+ heading_controller.setSetPoint(sp.psi);
+
+ //Update the process variable.
+ velocity_controller.setProcessValue(boat.u);
+ heading_controller.setProcessValue(boat.psi);
+
+ //Set the new output.
+ motor_out = velocity_controller.compute();
+ rudder_out = heading_controller.compute();
+ dist = sqrt((position_to_dest_N*position_to_dest_N)+(position_to_dest_E*position_to_dest_E));
+ motor_servo = motor_out;
+ rudder_servo = 0.5 + rudder_out;
+ //device.printf("Error = %f, Rudder output = %f\n\rRudder angle = %f\n\r", corrected_ang, rudder_out, 0.5+rudder_out);
+ //device.printf("Motor_out = %f\n\r",motor_out);
+
+ // Check if destination reached
+ if(dist < 5.0) destination_flag = true;
+
+ // Check if manual override commanded
+ if (device.readable() != 0) command = device.getc();
+ if (command == 'r') ManualOverride();
+
+ if(read_count == 10) {
+ device.printf("Dist = %f\n\r", dist);
+ read_count = 0;
+ }
+
+ read_count++;
+ //wait(0.1);
+ }
+ }
- }
-
+ device.printf("Destination reached. Switching to manual mode.\n\r");
+ ManualOverride();
+
+ return 0;
+}
+
+void ManualOverride() {
+ device.printf("Manual mode entered.\n\r");
+ char command;
+ motor_servo = 0.0;
+ rudder_servo = 0.5;
+ device.printf("Ready to accept commands.\n\r");
+
+ while(1) {
+ while (device.readable() != 0) {
+
+ command = device.getc();
+
+ switch (command){
+ case 'w':
+ if (motor_servo < 1.0) motor_servo = motor_servo + 0.01;
+ device.printf("Motor speed increased\n\r");
+ break;
+
+ case 's':
+ if (motor_servo > 0.0) motor_servo = motor_servo - 0.01;
+ device.printf("Motor speed decreased\n\r");
+ break;
+
+ case 'a':
+ if (rudder_servo > 0.3) rudder_servo = rudder_servo - 0.1;
+ device.printf("Rudder tilted right\n\r");
+ break;
+
+ case 'd':
+ if (rudder_servo < 0.8) rudder_servo = rudder_servo + 0.1;
+ device.printf("Rudder tilted left\n\r");
+ break;
+
+ case 'r':
+ motor_servo = 0.0;
+ rudder_servo = 0.5;
+ device.printf("Reset\n\r");
+ break;
+ }
+ }
+ }
}
void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat)
@@ -350,61 +474,61 @@
// Read the WHO_AM_I register, this is a good test of communication
whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
- pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x73\n\r");
+ device.printf("I AM 0x%x\n\r", whoami); device.printf("I SHOULD BE 0x73\n\r");
myled = 1;
if (whoami == 0x73) // WHO_AM_I should always be 0x73
{
- pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
- pc.printf("MPU9250 is online...\n\r");
+ device.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+ device.printf("MPU9250 is online...\n\r");
wait(1);
mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
- pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
- pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
- pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
- pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
- pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
- pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
+ device.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);
+ device.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
+ device.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);
+ device.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);
+ device.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);
+ device.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);
mpu9250.getAres(); // Get accelerometer sensitivity
mpu9250.getGres(); // Get gyro sensitivity
mpu9250.getMres(); // Get magnetometer sensitivity
- pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
- pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
- pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+ device.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+ device.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+ device.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
- pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
- pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
- pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
- pc.printf("x accel bias = %f\n\r", accelBias[1]);
- pc.printf("y accel bias = %f\n\r", accelBias[0]);
- pc.printf("z accel bias = %f\n\r", accelBias[2]);
+ device.printf("x gyro bias = %f\n\r", gyroBias[0]);
+ device.printf("y gyro bias = %f\n\r", gyroBias[1]);
+ device.printf("z gyro bias = %f\n\r", gyroBias[2]);
+ device.printf("x accel bias = %f\n\r", accelBias[1]);
+ device.printf("y accel bias = %f\n\r", accelBias[0]);
+ device.printf("z accel bias = %f\n\r", accelBias[2]);
wait(2);
mpu9250.initMPU9250();
- pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ device.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
wait(1);
mpu9250.initAK8963(magCalibration);
- pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
- pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
- pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
- if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
- if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
- if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
- if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+ device.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ device.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ device.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+ if(Mscale == 0) device.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) device.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) device.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) device.printf("Magnetometer ODR = 100 Hz\n\r");
- pc.printf("Mag Calibration: Wave device in a figure eight until done!");
+ device.printf("Mag Calibration: Wave device in a figure eight until done!");
wait(4);
mpu9250.magcalMPU9250(magBias, magScale);
- pc.printf("Mag Calibration done!\n\r");
- pc.printf("x mag bias = %f\n\r", magBias[0]);
- pc.printf("y mag bias = %f\n\r", magBias[1]);
- pc.printf("z mag bias = %f\n\r", magBias[2]);
+ device.printf("Mag Calibration done!\n\r");
+ device.printf("x mag bias = %f\n\r", magBias[0]);
+ device.printf("y mag bias = %f\n\r", magBias[1]);
+ device.printf("z mag bias = %f\n\r", magBias[2]);
//predicted_pk = A*actual_pk;
//predicted_pk.print();
wait(2);
@@ -413,8 +537,8 @@
else
{
- pc.printf("Could not connect to MPU9250: \n\r");
- pc.printf("%#x \n", whoami);
+ device.printf("Could not connect to MPU9250: \n\r");
+ device.printf("%#x \n", whoami);
myled = 0;
while(1) ; // Loop forever if communication doesn't happen
@@ -503,7 +627,7 @@
}
else
{
- accel_var=0.001;
+ accel_var=accel_varb;
}
if((a3<=0.042)&&a3>=-0.042)
{
@@ -512,7 +636,7 @@
}
else
{
- accel_var=0.001;
+ accel_var=accel_varb;
}
if((a4<=0.035)&&a4>=-0.035)
{
@@ -521,7 +645,7 @@
}
else
{
- accel_var=0.001;
+ accel_var=accel_varb;
}
}