Revision 4:e89234ca9d4e, committed 2020-11-12

Comitter:

pmmccorkell

Date:

Thu Nov 12 20:25:07 2020 +0000

Parent:

3:d6471216e378

Commit message:

asf

Changed in this revision

--- a/AUV_PID.cpp	Mon Feb 03 18:37:11 2020 +0000
+++ b/AUV_PID.cpp	Thu Nov 12 20:25:07 2020 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "BNO055.h"     //imu
-#include "MS5837.h"     //pressure sensor
 #include "PID.h"
-//asdf
+#define PI_math           3.14159265358979323846
+
 //Setup USB Serial
 Serial pc(USBTX, USBRX);
 int baudrate = 115200;
@@ -12,12 +12,6 @@
 DigitalOut pwr_on(p30);
 BNO055 imu(i2c,p8);
 
-//instantiate globals for press sensor
-MS5837 press_sensor(I2C_SDA,I2C_SCL,ms5837_addr_no_CS);
-int press_sensor_type=1;    //0 for 02BA, 1 for 30BA
-int sensor_rate=512;        //Oversampling Rate, see data sheet
-float depth=0;     //cm
-
 int wait_main=20;       //ms to wait in main, and associated, loops
 
 /*
@@ -44,7 +38,6 @@
 double min_thruster_bias=0.028;  //deadzone around 1.5ms where ESC will not operate
 double pw_tolerance=0.00001;
 
-
 volatile float ticker_rate= 0.02;   //Time between ticker calls in seconds.
 float target_time=5;    //Ideal timeframe to reach desired heading, depth, pitch using only Kp.
 float target_time_multiple=2;   //Ki will max out PWM in "factor * target_time".
@@ -56,7 +49,7 @@
 //degrees of acceptable heading tolerance
 double az_tolerance=2;
 // PID gains for heading
-volatile float heading_Kp =0.0008333f;
+volatile float heading_Kp = 0.0003333f;  //0.0008333f;
 volatile float heading_Ki =0.0f;
 volatile float heading_Kd =0.0f;        
 PID pid_heading(heading_Kp,heading_Ki,heading_Kd,ticker_rate,az_tolerance);
@@ -66,37 +59,6 @@
 // volatile float speed_Kd=0.0;
 // PID pid_speed(speed_Kp, speed_Ki, speed_Kd, ticker_rate, speed_tolerance);
 
-//
-//Elevation controllers
-//
-Ticker tickerElThrusters;
-//acceptable depth tolerance in cm
-float depth_tolerance=0.2;
-// PID gains for depth
-volatile float depth_Kp =6.0f;
-volatile float depth_Ki =10.0f;
-volatile float depth_Kd =0.0;
-PID pid_depth(depth_Kp, depth_Ki, depth_Kd, ticker_rate, depth_tolerance);
-//acceptable pitch tolerance in degrees
-double pitch_tolerance=1;
-// PID gains for pitch
-volatile float pitch_Kp = 6.0f; //1.0f;
-volatile float pitch_Ki = 10.0f; //10.0f;
-volatile float pitch_Kd = 0.00001;  //0.0;
-PID pid_pitch(pitch_Kp, pitch_Ki, pitch_Kd, ticker_rate, pitch_tolerance);
-
-/*
-Ticker tickerRollThrusters;
-volatile float roll_Pk = .50f;
-volatile float roll_Ik = .10f;
-volatile float roll_Dk=0.0;
-PID pid_roll(roll_Pk, roll_Ik, roll_Dk, ticker_rate);
-volatile float strafe_Pk=.50f;
-volatile float strafe_Ik=.10f;
-volatile float strafe_Dk=0.0;
-PID pid_strafe(roll_Pk, roll_Ik, roll_Dk, ticker_rate);
-*/
-
 //instantiate globals for sensor updates and data stream
 uint16_t ready_prefix = 0x0000;
 uint16_t horizon_prefix=0xff00;
@@ -118,16 +80,22 @@
 //-1 indicates no setting
 int persistent_heading=-1;
 int persistent_speed=-1;
-int persistent_depth=-1;
-int persistent_pitch=-1;
+int persistent_offset=-1;
 
 BNO055_ID_INF_TypeDef       bno055_id_inf;
 BNO055_EULER_TypeDef        euler_angles;
-//BNO055_QUATERNION_TypeDef   quaternion;
+BNO055_QUATERNION_TypeDef   quaternion;
 //BNO055_LIN_ACC_TypeDef      linear_acc;
 //BNO055_GRAVITY_TypeDef      gravity;
 //BNO055_TEMPERATURE_TypeDef  chip_temp;
 
+typedef struct {
+    uint16_t h;
+    uint16_t r;
+    uint16_t p;
+} CONVERTED_QUATERNION_TypeDef;
+
+CONVERTED_QUATERNION_TypeDef    conversion;
 
 //-----THRUSTER CLASS BEGIN-----//
 //Thruster class to instantiate individual thrusters.
@@ -238,11 +206,11 @@
 //-----THRUSTER CLASS END-----//
 
 // Instantiate thrusters.
-Thruster port_thrust(p21,-1);
-Thruster starboard_thrust(p22,1);
+Thruster fwd_port(p21,1);
+Thruster fwd_star(p22,-1);
 //Thruster steadystate(p23,1); //for test purposes, to keep ESC from making LOUD NOISES
-Thruster fore_thrust(p24,1);
-Thruster aft_thrust(p25,-1);
+Thruster aft_port(p24,-1);
+Thruster aft_star(p25,1);
 
 //Function to check for water leak. Open is good, short is bad.
 void leak_data() {
@@ -253,18 +221,6 @@
     leak_light=leak_detect;
 }
 
-//Function to get elevation data and send to RasPi.
-uint32_t el_data() {
-    //Run Barometric equations from pressure sensor.
-    press_sensor.calculate();
-    depth=press_sensor.depth();
-    uint32_t depth_data=(depth*0x20);
-    //0xb0 acts as prefix to identify Barometer Pressure.
-    //Pressure sensor sends pressure in range [0x3e8,0x1d4c0]. Divide by 100 for mbar.
-    uint32_t el_data_comp=(0xb1000000|depth_data);
-    return el_data_comp;
-}
-
 //Function to format Gain values into 3 significant digits and exponent value.
 // ie 0x abc * 10^(d-9) -> 0x abcd
 uint16_t k_data(float k_val) {
@@ -294,6 +250,30 @@
     return return_val;
 }
 
+
+//Convert Quaternion to Euler angles
+//https://forums.adafruit.com/viewtopic.php?f=19&t=95266
+void q_to_E(double qw, double qx, double qy, double qz, CONVERTED_QUATERNION_TypeDef *conv) {
+    double qnorm = 1 / sqrt((double)(qw*qw + qx*qx + qy*qy + qz*qz));  // normalize the quaternion
+    qw *= qnorm;
+    qx *= qnorm;
+    qy *= qnorm;
+    qz *= qnorm;
+    double factor = 180.0/PI_math;
+    double roll_conversion = factor * atan2((double)(qw*qx + qy*qz), (double)(0.5 - qx*qx - qy*qy));
+    //double pitch = factor * asin((double)(2 * (qw*qy - qx*qz)));      //reversed
+    double pitch_conversion = factor * asin((double)(2 * (qx*qz - qw*qy)));
+    double heading_conversion = factor * atan2((double)(qw*qz + qx*qy), (double)(0.5 - qy*qy - qz*qz));
+    heading_conversion=(-1*heading_conversion);
+    if (heading_conversion<0) {
+        heading_conversion+=360;   //quaternion reads heading in CCW, flip to CW
+    }
+    //pc.printf("log: Q-derived h: %lf, r: %lf, P: %lf\n\r", heading_conversion, roll_conversion, pitch_conversion);
+    conv->r = int(roll_conversion*16);
+    conv->h = int(heading_conversion*16);
+    conv->p = int(pitch_conversion*16);
+}
+
 //Data function pulls data from BNO and sends over serial
 //Timed using DigitalOut and Oscope. 
 //  At baud 115200, averaged 5 times over 256, +pulsewidth 11.1 - 13.3ms.
@@ -337,11 +317,18 @@
     status[2]=0xc0000000+imu.read_calib_status();
 
     //Get Euler data from BNO.
-    imu.get_Euler_Angles(&euler_angles);
+//    imu.get_Euler_Angles(&euler_angles);
+    imu.get_quaternion(&quaternion);
+    double qw=(double)quaternion.w;
+    double qx=(double)quaternion.x;
+    double qy=(double)quaternion.y;
+    double qz=(double)quaternion.z;
+    q_to_E(qw,qx,qy,qz,&conversion);
     
     //word 3 is Heading.
         //0xc100 acts as prefix to identify Heading for RasPi. 
-    uint16_t h = euler_angles.h;
+    //uint16_t h = euler_angles.h;
+    uint16_t h = conversion.h;
     heading=h;
     status[3]=0xc1000000+h;
 
@@ -351,43 +338,35 @@
     //word 4 is Roll.
         //0xc300 acts as prefix to identify Roll for RasPi.
         //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
-    uint16_t r = offset + euler_angles.r;
+    //uint16_t r = offset + euler_angles.r;
+    uint16_t r = offset + conversion.r;
     status[4]=0xc3000000+r;
 
     //word 5 is Pitch.
         //0xc500 acts as prefix to identify Pitch for RasPi.
         //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
-    uint16_t p = offset + euler_angles.p;
+    //uint16_t p = offset + euler_angles.p;
+    uint16_t p = offset + conversion.p;
     pitch=(p - (offset/2)); //only want 180deg offset
     status[5]=0xc5000000+p;
     
     //word 6 gets Depth from el_data() function.
         //0xb0 acts as prefix to identify Barometer Pressure.
-    status[6]=el_data();
+    // status[6]=el_data();
     
     //Thruster speeds in 10us resolution.
-    status[7]=((port_thrust.thruster_data() & 0x00ffffff) | 0xf1000000);
-    status[8]=((starboard_thrust.thruster_data() &0x00ffffff) | 0xf2000000);
-    status[9]=((fore_thrust.thruster_data() & 0x00ffffff) | 0xf3000000);
-    status[10]=((aft_thrust.thruster_data() & 0x00ffffff) | 0xf4000000);
+    status[6]=((fwd_port.thruster_data() & 0x00ffffff) | 0xf1000000);
+    status[7]=((fwd_star.thruster_data() &0x00ffffff) | 0xf2000000);
+    status[8]=((aft_port.thruster_data() & 0x00ffffff) | 0xf3000000);
+    status[9]=((aft_star.thruster_data() & 0x00ffffff) | 0xf4000000);
 
     //Gains in format: ABC * 10^(D-9), similar to resistor color bands but starting at 10^-9 rather than 10^0.
     //Value is sent in Hex, but the 3bit scalar, ABC, is decimal [0,999]. Exponent component, D, may be hex to obtain range of [10^-9,10^6]
     
     //Heading Gains
-    status[11]= 0xd1100000 + k_data(heading_Kp);    //Kp
-    // status[12]= 0xd1200000 + k_data(heading_Ki); //Ki
-    // status[13]= 0xd1300000 + k_data(heading_Kd); //Kd
-
-    //Pitch Gains
-    status[12]= 0xd1200000 + k_data(pitch_Kp);  //Kp
-    // status[15]= 0xd5200000 + k_data(pitch_Ki);   //Ki
-    // status[16]= 0xd5300000 + k_data(pitch_Kd);   //Kd
-
-    //Depth Gains
-    status[13]= 0xd1300000 + k_data(depth_Kp);  //Kp
-    // status[18]= 0xd7200000 + k_data(depth_Ki);   //Ki
-    // status[19]= 0xd7300000 + k_data(depth_Kd);   //Kd
+    status[10]= 0xd1100000 + k_data(heading_Kp); //Kp
+    status[11]= 0xd1200000 + k_data(heading_Ki); //Ki
+    status[12]= 0xd1300000 + k_data(heading_Kd); //Kd
 
     //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
     int i;
@@ -402,117 +381,10 @@
 void stop_all_persistent() {
     persistent_heading=-1;
     persistent_speed=-1;
-    persistent_depth=-1;
-    persistent_pitch=-1;
-    pid_pitch.clear();
-    pid_depth.clear();
     pid_heading.clear();
     // pid_speed.reset();
 }
 
-//Mostly deprecated by xyzControllers
-//Function to create new threads for motor logic
-void call_threads(int select, double dir=1, double speed=0) {
-    call_threads_available=0;
-    double rev_speed=(-1*speed);
-    //Masking for port and starboard thruster status.
-    uint16_t ready_mask_ps=0xff3f;
-    //Masking for fore and aft thruster status.
-    uint16_t ready_mask_fa=0xffcf;
-    switch (select) {
-        //case 1, forwards or backwards
-        case 1:
-            ready_data=(ready_data&ready_mask_ps)|0x00c0;
-            if (dir==1) {
-                pc.printf("log: call_threads Fwd, %f\r\n",speed);
-                starboard_thrust.set_speed(speed);
-                port_thrust.set_speed(speed);
-            }
-            if (dir==-1) {
-                pc.printf("log: call_threads Rev, %f\r\n",speed);
-                starboard_thrust.set_speed(rev_speed);
-                port_thrust.set_speed(rev_speed);
-            }
-            break;
-
-        //case 2, turn left or right
-        case 2:
-            ready_data=(ready_data&ready_mask_ps)|0x00c0;
-            if (dir==1) {
-                pc.printf("log: call_threads Turn R, %f\r\n",speed);
-                starboard_thrust.set_speed(speed);
-                port_thrust.set_speed(rev_speed);
-            }
-            if (dir==-1) {
-                pc.printf("log: call_threads Turn L, %f\r\n",speed);
-                starboard_thrust.set_speed(rev_speed);
-                port_thrust.set_speed(speed);
-            }
-            break;
-
-        //case 3, Up and Down
-        case 3:
-            ready_data=(ready_data&ready_mask_fa)|0x0030;
-            if (dir==1) {
-                pc.printf("log: call_threads Up, %f\r\n",speed);
-                fore_thrust.set_speed(speed);
-                aft_thrust.set_speed(speed);
-            }
-            if (dir==-1) {
-                pc.printf("log:call_threads Down,%f\r\n",speed);
-                fore_thrust.set_speed(rev_speed);
-                aft_thrust.set_speed(rev_speed);
-            }
-            break;
-
-        //case 4, pitch up/down
-        case 4:
-            ready_data=(ready_data&ready_mask_fa)|0x0030;
-            if (dir==1) {
-                pc.printf("log: call_threads Pitch Up,%f\r\n",speed);
-                fore_thrust.set_speed(speed);
-                aft_thrust.set_speed(rev_speed);
-            }
-            if (dir==-1) {
-                pc.printf("log: call_threads Pitch Down,%f\r\n",speed);
-                fore_thrust.set_speed(rev_speed);
-                aft_thrust.set_speed(speed);
-            }
-            break;
-
-        //cases 5 and 6 reserved for roll should we add more thrusters.
-
-        //case 77, Emergency Surface
-        case 77:
-            pc.printf("log: call_threads Emergency Surface,%f\r\n",speed);
-            //Fore and Aft up: 111
-            ready_data=(ready_data&ready_mask_fa)|0x0030;
-            fore_thrust.set_speed(esc_range_spec);
-            aft_thrust.set_speed(esc_range_spec);
-            break;
-
-        //case 99, Stop Fore and Aft thrusters.
-        case 99:
-            pc.printf("log: Stop el thrusters,%f\r\n",null_pw);
-            //fore and aft thrusters stopped: 000
-            ready_data=(ready_data&ready_mask_fa);
-            fore_thrust.set_speed(null_pw);
-            aft_thrust.set_speed(null_pw);
-            break;
-
-        //case 0, stop az thrusters
-        default:
-        case 0:
-            pc.printf("log: Stop az thrusters,%f\r\n",null_pw);
-            //starboard and port thrusters stopped: 000
-            ready_data=(ready_data&ready_mask_ps);
-            starboard_thrust.set_speed(null_pw);
-            port_thrust.set_speed(null_pw);
-            break;
-    }
-    call_threads_available=1;
-}
-
 //When bad things are happening.
 void EventHorizon() {
     event_horizon_flag=1;
@@ -521,10 +393,10 @@
     pc.printf("log: EventHorizon called, count: %i\r\n",horizon_count);
     //setEvent() method locks out Thruster class set_speed() function
     //  and sets PWM to 1.5ms.
-    port_thrust.setEvent();
-    starboard_thrust.setEvent();
-    fore_thrust.setEvent();
-    aft_thrust.setEvent();
+    fwd_port.setEvent();
+    fwd_star.setEvent();
+    aft_port.setEvent();
+    aft_star.setEvent();
     pc.printf("log: Thruster events successfully set\r\n");
     //Tells Raspi that Emergency state has been initiated.
     ready_prefix=(horizon_prefix+horizon_count);
@@ -535,16 +407,14 @@
         wait_ms(wait_main);
     }
     //Clear emergency situation.
-    port_thrust.clearEvent();
-    starboard_thrust.clearEvent();
-    fore_thrust.clearEvent();
-    aft_thrust.clearEvent();
+    fwd_port.clearEvent();
+    fwd_star.clearEvent();
+    aft_port.clearEvent();
+    aft_star.clearEvent();
     pc.printf("log: Thruster events successfully cleared\r\n");
     //Set PWM to 1.5ms after emergency situation. Should have been set to 1.5ms, but double checking.
     //  For extra precaution.
     stop_all_persistent();
-    call_threads(0);
-    call_threads(99);
     //Lets Raspi know that Mbed is ready for commands again.
     ready_prefix=0xffff;
     pc.printf("log: ready status reset, mbed may resume\r\n");
@@ -581,120 +451,9 @@
     return returnval;
 }
 
-//Todo: Update for edge case around 90deg rather than 0-360. 
-//Observed: BNO never actually reaches +/-90deg, flips around 84deg. 
-//  Arguably Quarternion may be better, however do not anticipate using more than 45deg.
-double pitchController(void){
-    // call_threads_available=0;
-    double speed=0;
-    double desired_pitch=persistent_pitch;
-    //Ensure there is a valid pitch set point before continuing
-    if (desired_pitch!=-1) {
-        double diff=0;
-        double current_pitch=pitch;
-        //Compensate for BNO having 16bit resolution per degree
-        current_pitch=(current_pitch/16);
-
-        //Calculate how far to turn in degrees.
-        diff = (desired_pitch-current_pitch);
-        //Send error to PID.
-        speed=pid_pitch.process(diff);
-
-        //Necessary to overcome ESC' 25us deadzone around 1.5ms.
-        if ((fabs(speed)<min_thruster_bias) and (diff!=0)) {
-            if (speed<0) speed=-1*(min_thruster_bias);
-            else speed=min_thruster_bias;
-        }
-        //Serial log statements
-        //Commented out to reduce serial lag induced at 115200 baud
-        //pc.printf("log: Pcntrl, factor: %f, speed: %f\r\n",factor,speed);
-        //pc.printf("log: Pcntrl, desired: %f, current: %f, diff: %f\r\n",desired_pitch,current_pitch,diff);
-    }
-    // call_threads_available=1;
-    return (speed);
-}
-
-double depthController(void){
-    // call_threads_available=0;
-    double speed=0;
-    double desired_depth=persistent_depth;
-    double current_depth=depth;
-    // double diff=abs(desired_depth-current_depth);
-    double diff = desired_depth-current_depth;
-
-    //Send the error to PID.
-    speed = pid_depth.process(diff);
-
-    //Necessary to overcome ESC' 25us deadzone around 1.5ms.
-    if ((speed<min_thruster_bias) and (diff!=0)) speed=min_thruster_bias;
-
-    //Serial log statements
-    //Commented out to reduce serial lag induced at 115200 baud
-    //pc.printf("log: Dcntrl, factor: %f, speed: %f\r\n",factor,speed);
-    //pc.printf("log: Dcntrl, desired: %f, current: %f, diff: %f\r\n",desired_depth,current_depth,diff);
-
-    // call_threads_available=1;
-    return speed;
-}
-
-//Function to handle vertical motor logic.
-void el_thruster_logic() {
-    if (manual_mode==0) {
-        //Internal debugging and time measurements.
-        //function_timer=1;
-        // logic_available=0;
-
-        //Instantiate controllers at 0.
-        double depth_speed=0;
-        double pitch_speed=0;
-        double aft_speed=0;
-        double fore_speed=0;
-        
-        //Call Depth and/or Pitch controllers only if they have a valid set point.
-        if (persistent_depth!=-1) {
-            depth_speed=depthController();
-        }
-        if (persistent_pitch!=-1) {
-            pitch_speed=pitchController();
-        }
-        
-        //If both Controllers are active, it's easy to saturate the ESCs at +/-400ms.
-        //Arbitrarily chose 75% for creating superposition of Depth and Pitch, this could be refined.
-        if ((persistent_pitch!=-1) and (persistent_depth!=-1)) {
-            depth_speed=depth_speed*.75;
-            pitch_speed=pitch_speed*.75;
-        }
-
-        //Form superposition independently for Aft and Fore ESCs 
-        //      from depth and pitch controllers.
-        aft_speed=(depth_speed - pitch_speed);
-        fore_speed=(depth_speed + pitch_speed);
-
-        //Ensure values don't exceed ESC operational range: ([-400,400]ms + 1.5ms).
-        if (aft_speed<(-1*esc_range)) aft_speed=(-1*esc_range);
-        if (fore_speed<(-1*esc_range)) fore_speed=(-1*esc_range);
-        if (aft_speed>esc_range) aft_speed=esc_range;
-        if (fore_speed>esc_range) fore_speed=esc_range;
-
-        //Only update PWM values if they're appreciably different from current state.
-        //Log print statements commented out to reduce serial lag at 115200 baud rate.
-        double current_aft_pw = aft_thrust.get_pw();
-        double current_fore_pw = fore_thrust.get_pw();
-        double compare_aft_pw=fabs((1.5+aft_speed)-current_aft_pw);
-        double compare_fore_pw=fabs((1.5+fore_speed)-current_fore_pw);
-        if (compare_aft_pw > pw_tolerance) {
-            aft_thrust.set_speed(aft_speed);
-            //pc.printf("log: aft %f\r\n",aft_speed);
-        }
-        if (compare_fore_pw > pw_tolerance) {
-            fore_thrust.set_speed(fore_speed);
-            //pc.printf("log: fore %f\r\n",fore_speed);
-        }
-        //Internal debugging and time measurements.
-        // logic_available=1;
-        //function_timer=0;
-    }
-}
+//TODO: for lateral movement
+//double offsetController() {
+//}
 
 //Function to go to set heading.
 //Timed with DigitalOut on Oscope.
@@ -719,43 +478,54 @@
         if (diff>180.0) {
             //pc.printf("log: diff>180: %f\r\n",diff);
             if (desired_heading>180) {
-                //dir=(-1*dir);
                 current_heading=current_heading+180;
                 desired_heading=desired_heading-180;
-                diff=current_heading-desired_heading;
                 pc.printf("log: desired>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff);
             }
-            if (current_heading>180) {
-                dir=(-1*dir);
+            else {  //current_heading>180
                 current_heading=current_heading-180;
                 desired_heading=desired_heading+180;
-                diff=desired_heading-current_heading;
+                pc.printf("log: current>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff);
             }
         }
-
-        speed = pid_heading.process(diff);
+        speed = pid_heading.process(desired_heading,current_heading);
+        // pc.printf("log: des %f, cur %f, pr speed %f\r\n",desired_heading,current_heading,speed);
 
         //Necessary to overcome 25us deadzone around 1.5ms
-        if ((abs(speed)<min_thruster_bias) and (diff!=0)) {
+        if ((abs(speed)<min_thruster_bias) and (abs(diff)>az_tolerance)) {
             if (speed<0) speed=(-1*min_thruster_bias);
             else speed=min_thruster_bias;
         }
-        //Serial log statements
-        //Commented out to reduce serial lag induced at 115200 baud
-        // pc.printf("log: Hcntrl, factor: %f, speed: %f\r\n",factor,speed);
-        // pc.printf("log: Hcntrl, desired: %f, current: %f, diff: %f\r\n",desired_heading,current_heading,diff);
     }
     // call_threads_available=1;
     return (speed*dir);
 }
 
-//Controller to maintain the scalar component of velocity vector.
-double speedController() {
+//Controller to maintain the scalar component of velocity vector for starboard front and port aft thrusters.
+double cos_speedController() {
     // call_threads_available=0;
-    double desired_speed=persistent_speed;
+    double offset_factor=0;
+    if (persistent_offset!=-1) {
+        offset_factor=(((2*PI_math)/360)*persistent_offset);
+    }
+    offset_factor+=(PI_math/4);
+    double desired_speed=persistent_speed*cos(offset_factor);
     desired_speed=(desired_speed/1000);     //convert int us to ms
 
-    // ENTER PID CALCS HERE //
+    // call_threads_available=1;
+    return (desired_speed);
+}
+
+//Controller to maintain the scalar component of velocity vector for port front and starboard aft thrusters.
+double sin_speedController() {
+    // call_threads_available=0;
+    double offset_factor=0;
+    if (persistent_offset!=-1) {
+        offset_factor=(((2*PI_math)/360)*persistent_offset);
+    }
+    offset_factor+=(PI_math/4);
+    double desired_speed=persistent_speed*sin(offset_factor);
+    desired_speed=(desired_speed/1000);     //convert int us to ms
 
     // call_threads_available=1;
     return (desired_speed);
@@ -769,55 +539,56 @@
 //        function_timer3=1;
         // logic_available=0;
         double heading_speed=0;
-        double sp_speed=0;
-        double starboard_speed=0;
-        double port_speed=0;
-        if (persistent_speed==-1) sp_speed=0;
+        double sin_speed=0;
+        double cos_speed=0;
+        double fwd_star_speed=0;
+        double fwd_port_speed=0;
+        double aft_star_speed=0;
+        double aft_port_speed=0;
+        if (persistent_speed==-1) {
+            cos_speed=0;
+            sin_speed=0;
+        }
         if (persistent_heading==-1) heading_speed=0;
         if (persistent_heading!=-1) {
             heading_speed=headingController();
         }
         if (persistent_speed!=-1) {
-            sp_speed=speedController();
+            cos_speed=cos_speedController();
+            sin_speed=sin_speedController();
         }
-        /*
-        if ((persistent_heading!=-1) and (persistent_speed!=-1)) {
-            heading_speed=heading_speed*.75;
-            sp_speed=sp_speed*.75;
-        }
-        */
 
         //Create Superposition of Heading and Speed
-        //May need to divide these by 2 ?
-        starboard_speed=(sp_speed - heading_speed);
-        port_speed=(sp_speed + heading_speed);
+        fwd_star_speed=(cos_speed - heading_speed);
+        aft_port_speed=(cos_speed + heading_speed);
+        fwd_port_speed=(sin_speed + heading_speed);
+        aft_star_speed=(sin_speed - heading_speed);
 
         //Error checking to ensure PWM doesn't escape ESC parameters
-        if (port_speed<(-1*esc_range)) port_speed=(-1*esc_range);
-        if (starboard_speed<(-1*esc_range)) starboard_speed=(-1*esc_range);
-        if (port_speed>esc_range) port_speed=esc_range;
-        if (starboard_speed>esc_range) starboard_speed=esc_range;
+        if (fwd_port_speed<(-1*esc_range)) fwd_port_speed=(-1*esc_range);
+        if (fwd_star_speed<(-1*esc_range)) fwd_star_speed=(-1*esc_range);
+        if (aft_star_speed<(-1*esc_range)) aft_star_speed=(-1*esc_range);
+        if (aft_port_speed<(-1*esc_range)) aft_port_speed=(-1*esc_range);
+
+        if (fwd_port_speed>esc_range) fwd_port_speed=esc_range;
+        if (fwd_star_speed>esc_range) fwd_star_speed=esc_range;
+        if (aft_star_speed>esc_range) aft_star_speed=esc_range;
+        if (aft_port_speed>esc_range) aft_port_speed=esc_range;
 
-        //Only write PWM if PW is appreciably different
-        double current_starboard_pw = starboard_thrust.get_pw();
-        double current_port_pw = port_thrust.get_pw();
-        double compare_port_pw=fabs((1.5+port_speed)-current_port_pw);
-        double compare_starboard_pw=fabs((1.5+starboard_speed)-current_starboard_pw);
-        if (compare_port_pw > pw_tolerance) {
-            port_thrust.set_speed(port_speed);
-            //pc.printf("log: port %f\r\n",port_speed);
-        }
-        if (compare_starboard_pw > pw_tolerance) {
-            starboard_thrust.set_speed(starboard_speed);
-            //pc.printf("log: stbd %f\r\n",starboard_speed);
-        }
-        // logic_available=1;
-//        function_timer3=0;
+        //Write PWM values.
+        fwd_port.set_speed(fwd_port_speed);
+        aft_port.set_speed(aft_port_speed);
+        //pc.printf("log: port %f\r\n",port_speed);
+        fwd_star.set_speed(fwd_star_speed);
+        aft_star.set_speed(aft_star_speed);
+        //pc.printf("log: stbd %f\r\n",starboard_speed);
+        //logic_available=1;
+        //function_timer3=0;
     }
 }
 
 
-//Function to change BNO position
+//Function to change BNO position.
 void switch_pos(int position) {
     uint16_t ready_mask=0xfff8;
     if (position>=0 and position<8) {
@@ -859,72 +630,30 @@
     }
 }
 
-//Mostly deprecated.
-//Manual direction function allows manual control of AUV.
-void manual_direction(int direction,int mode) {
-    // double speed_rate=0.25;         //25% speed of global max.
-    //double speed=(esc_range*speed_rate);    //This rate% of global% set in globals. Expected (70-90%).
-    double speed=min_thruster_bias;
-    double dir;
-    if (direction==1) dir = 1.0;
-    else if (direction==2) dir=(-1);
-    else mode=0.0;
-    if (mode==0) {
-        //stop az Thrusters
-        call_threads(0);
-        //stop el Thrusters
-        call_threads(99);
-    }
-    else {
-        call_threads(mode,dir,speed);
-    }
-}
-
-//Mostly deprecated.
-//Tests +/- direction of heading, speed, depth, and pitch for demonstration.
-void test_direction(int mode) {
-    //pc.printf("test_dir thread, mode:%i\r\n",mode);
-    int count=0;
-    int time=450;
-    int length=0;
-    manual_direction(1,mode);
-    while(count<time) {
-        int check=0;
-        if (count==200) {
-            //pc.printf("stop in middle of test/r/n");
-            if ((mode==1) or (mode==2)) {
-                call_threads(0);
-            }
-            if ((mode==3) or (mode==4)) {
-                call_threads(99);
-            }
-        }
-        if (count==250) {
-            //pc.printf("reverse direction\r\n");
-            if (event_horizon_flag==0) manual_direction(2,mode);
-        }
-        //pc.printf("log: cnt: %i\r\n",count);
-        if (pc.readable()) {
-            length=read_serial();
-            if (length==4) {
-                check=look_horizon();
-            }
-        }
-        if (check==1) count=time;
-        else {
-            count++;
-            wait_ms(wait_main);
-            az_data();
-        }
-    }
-    //stop az thrusters
-    if ((mode==1) or (mode==2)) {
-        call_threads(0);
-    }
-    //stop el thrusters
-    if ((mode==3) or (mode==4)) {
-        call_threads(99);
-    }
+//Function to change BNO Fusion Modes.
+void switch_mode(uint8_t new_mode) {
+    imu.change_fusion_mode(new_mode);
+    pc.printf("log: change BNO mod to %u\r\n",new_mode);
+    // int switch_int = (int)new_mode;
+    // switch (switch_int) {
+        // case 1:
+            // imu.change_fusion_mode(MODE_M4G);
+            // pc.printf("log: change BNO mode to M4G\r\n");
+            // break;
+        // case 2:
+            // imu.change_fusion_mode(MODE_COMPASS);
+            // pc.printf("log: change BNO mode to COMPASS\r\n");
+            // break;
+        // case 3:
+            // imu.change_fusion_mode(MODE_IMU);
+            // pc.printf("log: change BNO mode to IMU\r\n");
+            // break;
+        // case 12:
+        // default:
+            // imu.change_fusion_mode(MODE_NDOF);
+            // pc.printf("log: change BNO mode to NDOF\r\n");
+            // break;
+    // }
 }
 
 //Function for manual up/down, left/right controls.
@@ -932,23 +661,9 @@
 //      to new set point, incrementally driving vehicle.
 //      ie current heading 10deg, function(3,-1), new heading set point of 10+(-1*3) = 7deg
 void increment_persistent(int select, int magnitude) {
-    int pitch_resolution=3;     //degrees
-    int depth_resolution=3;     //cm
     int heading_resolution=3;   //degrees
     int speed_resolution=27;     //us
     switch (select) {
-        //pitch
-        case 1:
-            persistent_pitch=((pitch/16)+(magnitude*pitch_resolution));
-            pid_pitch.clear();
-            break;
-        
-        //depth
-        case 2:
-            persistent_depth=(depth+(magnitude*depth_resolution));
-            pid_depth.clear();
-            break;
-            
         //heading
         case 3:
             persistent_heading=((heading/16)+(magnitude*heading_resolution));
@@ -957,12 +672,28 @@
         
         //speed
         case 4:
-            persistent_speed=((1000*starboard_thrust.get_speed())+(magnitude*speed_resolution));
+            persistent_speed=((1000*fwd_star.get_speed())+(magnitude*speed_resolution));
             // pid_speed.clear();
             break;
+        
+        //offset
+        case 5:
+            persistent_offset=(persistent_offset+(magnitude*heading_resolution));
+            break;
     }
 }
 
+float k_commands(int value, int power) {
+    static float pow[10] = {
+        0.00001, 0.0001, 0.001, 0.01, 0.1,  //[0,4]
+        1, 10, 100, 1000, 10000 //[5,9]
+    };
+    float return_val=0;
+    return_val = value*pow[power];
+    pc.printf("log: return %f, pow %lf\r\n",return_val,pow[power]);
+    return return_val;
+}
+
 //Function intakes serial commands.
 void commands() {
     //pc.printf("log: commands called\r\n");
@@ -979,17 +710,24 @@
         }
         //check_pos char array used to filter Position commands.
         char check_pos[5]="pos:";
-        //check_hea char array used to filter Go to Heading commands.
+        //check_mod char array used to filter Fusion modes on BNO.
+        char check_mod[5]="mod:";
+        //Heading commands.
         char check_hea[5]="hea:";
-        //check_dep char array used to filter Depth commands.
-        char check_dep[5]="dep:";
-        char check_zer[5]="zer:";
-        char check_pit[5]="pit:";
-        char check_tst[5]="tst:";
+        //Speed commands.
+        char check_vel[5]="vel:";
+        //Offset commands.
+        char check_off[5]="off:";
+        //Stop all commands.
         char check_sto[5]="sto:";
+        //Reset Mbed command.
         char check_res[5]="res:";
-        char check_vel[5]="vel:";
 
+        //Heading PID gains
+        char check_hkp[5]="hkp:";
+        char check_hki[5]="hki:";
+        char check_hkd[5]="hkd:";
+        
         //While loop reads Serial input into input buffer.
 
         //Only continue if input buffer has 7 entries, otherwise ignore input buffer. 
@@ -998,14 +736,17 @@
     //    if (i==7) {
             //Instantiate counters that will be used to verify known commands.
         int verified_pos=0;
+        int verified_mod=0;
         int verified_hea=0;
-        int verified_dep=0;
-        int verified_zer=0;
-        int verified_pit=0;
-        int verified_tst=0;
         int verified_sto=0;
         int verified_res=0;
         int verified_vel=0;
+        int verified_off=0;
+
+        int verified_hkp=0;
+        int verified_hki=0;
+        int verified_hkd=0;
+        
         //While loop checks first 4 characters of input buffer and attempts to match
         //      against known commands.
         for (int q=0; q<3; q++) {
@@ -1013,7 +754,13 @@
             //      and Position command format.
             if (input[q]==check_pos[q]) {
                 verified_pos++;
-                pc.printf("pos %c at %i\r\n",input[q],q);
+                //pc.printf("pos %c at %i\r\n",input[q],q);
+            }
+            //Increment verified_mod if a match is found between Serial input buffer
+            //      and Fusion Modde command format.
+            if (input[q]==check_mod[q]) {
+                verified_mod++;
+                //pc.printf("mod %c at %i\r\n",inputtt[q],q);
             }
             //Increment verified_hea if a match is found between Serial input buffer
             //      and Heading command format.
@@ -1021,22 +768,6 @@
                 //pc.printf("hea %c at %i\r\n",input[q],q);
                 verified_hea++;
             }
-            if (input[q]==check_dep[q]) {
-                //pc.printf("dep %c at %i\r\n",input[q],q);
-                verified_dep++;
-            }
-            if (input[q]==check_zer[q]) {
-                //pc.printf("zer %c at %i\r\n",input[q],q);
-                verified_zer++;
-            }
-            if (input[q]==check_pit[q]) {
-               //pc.printf("pit %c at %i\r\n",input[q],q);
-                verified_pit++;
-            }
-            if (input[q]==check_tst[q]) {
-                //pc.printf("tst %c at %i\r\n",input[q],q);
-                verified_tst++;
-            }
             if (input[q]==check_sto[q]) {
                 //pc.printf("sto %c at %i\r\n",input[q],q);
                 verified_sto++;
@@ -1049,6 +780,18 @@
                 verified_vel++;
                 //pc.printf("vel %c at %i\r\n",input[q],q);
             }
+            if (input[q]==check_off[q]) {
+                verified_off++;
+            }
+            if (input[q]==check_hkp[q]) {
+                verified_hkp++;
+            }
+            if (input[q]==check_hki[q]) {
+                verified_hki++;
+            }
+            if (input[q]==check_hkd[q]) {
+                verified_hkd++;
+            }
         }
 
         //If first 4 characters from Serial input buffer match Position command format,
@@ -1057,6 +800,12 @@
             int change=(input[6]-'0');
             switch_pos(change);
         }
+        if (verified_mod==3) {
+            uint8_t mode=(input[6]-'0');
+            if ((mode>0x0) and (mode<0x0d)) {
+                switch_mode(mode);
+            }
+        }
         if (verified_sto==3) {
             //pc.printf("log: stop command received\r\n");
             stop_all_persistent();
@@ -1083,67 +832,6 @@
                 if (event_horizon_flag==0) persistent_heading=hea;
             }
         }   
-        //If first 4 characters from Serial input buffer match Depth command format,
-        //      execute "depth" function.
-        if (verified_dep==3) {
-            //Correct for ascii '0', and reform 3digit decimal number
-            int dep100=(input[4]-'0');
-            int dep10=(input[5]-'0');
-            int dep1=(input[6]-'0');
-            int dep=(dep100*100)+(dep10*10)+(dep1*1);
-            pc.printf("log: depth rx: %i\r\n",dep);
-            if (dep==999) {
-                persistent_depth=-1;
-                pid_depth.clear();
-            }
-            if ((dep>=821) and (dep<=827)) {
-                increment_persistent(dep10,(dep1-4));
-            }
-            if (dep<=820) {
-                pid_depth.clear();
-                if (event_horizon_flag==0) persistent_depth=dep;
-            }
-        }
-        if (verified_pit==3) {
-            //Correct for ascii '0', and reform 3digit decimal number
-            int pit100=(input[4]-'0');
-            int pit10=(input[5]-'0');
-            int pit1=(input[6]-'0');
-            int pit=(pit100*100)+(pit10*10)+(pit1*1);
-            pc.printf("log: pitch rx: %i\r\n",pit);
-            if (pit==999) {
-                persistent_pitch=-1;
-                pid_pitch.clear();
-            }
-            if ((pit>=811) and (pit<=817)) {
-                increment_persistent(pit10,(pit1-4));
-            }
-            if ((pit>=0) and (pit<=360)) {
-                pid_pitch.clear();
-                if (event_horizon_flag==0) persistent_pitch=pit;
-            }
-        }
-        if (verified_tst==3) {
-            stop_all_persistent();
-            int tst_mode=(input[4]-'0');
-            int tst_data_dir=(input[5]-'0');
-            int tst_data_move=(input[6]-'0');
-            //For both tst_modes, tst_data_msb=0 stops all thrusters.
-            //tst_mode 0 is for automated testing of 4 movements (tst_data_msb[0,4]) for fixed 3s.
-            if (tst_mode==0) {
-                pc.printf("log: tst, mode: %i, dir: %i, move: %i\r\n",tst_mode,tst_data_dir,tst_data_move);
-                manual_mode=1;
-                test_direction(tst_data_move);
-                manual_mode=0;
-            }
-            //tst_mode 1 is for manual commands of 4 movements for indefinite periods.
-            if (tst_mode==1) {
-                //Need to create way to exit manual mode
-                //  presently, have to cheese a tst:0xx command
-                manual_mode=1; 
-                manual_direction(tst_data_dir,tst_data_move);
-            }
-        }
         if (verified_res==3) {
             pc.printf("log: Reset mbed received. See you on the other side.\r\n");
             //Mbed reset command.
@@ -1161,7 +849,6 @@
             //999 to stop speed controller.
             if (vel==999) {
                 persistent_speed=-1;
-                pid_pitch.clear();
             }
             if ((vel>=841) and (vel<=847)) {
                 increment_persistent(vel10,(vel1-4));
@@ -1172,10 +859,45 @@
                 if (event_horizon_flag==0) persistent_speed=vel;
             }
         }
-        if (verified_zer==3) {
-            double zero=press_sensor.set_atm();
-            zero_set=1;
-            pc.printf("log: zeroized %f Pa\r\n",zero);
+        
+        //Heading offset commands.
+        if (verified_off==3) {
+            int off100=(input[4]-'0');
+            int off10=(input[5]-'0');
+            int off1=(input[6]-'0');
+            int off=(off100*100)+(off10*10)+(off1*1);
+            pc.printf("log: off rx: %i\r\n",off);
+            //999 to reset heading offset to 0.
+            if (off==999) {
+                persistent_offset=-1;
+            }
+            if ((off>=851) and (off<=857)) {
+                increment_persistent(off10,(off1-4));
+            }
+            if ((off>=0) and (off<=360)) {
+                if (event_horizon_flag==0) persistent_offset=off;
+            }
+        }
+
+        int reset_h=0;
+        if (verified_hkp==3) {
+            float kval=((input[4]-'0')*10)+input[5]-'0';
+            heading_Kp=k_commands(kval,input[6]-'0');
+            reset_h=1;
+        }
+        if (verified_hki==3) {
+            float kval=((input[4]-'0')*10)+input[5]-'0';
+            heading_Ki=k_commands(kval,input[6]-'0');
+            reset_h=1;
+        }
+        if (verified_hkd==3) {
+            float kval=((input[4]-'0')*10)+input[5]-'0';
+            heading_Kd=k_commands(kval,input[6]-'0');
+            reset_h=1;
+        }
+        if (reset_h) {
+            pid_heading.set_K(heading_Kp,heading_Ki,heading_Kd);
+            pc.printf("log: update Heading gains Kp %f, Ki %f, Kd %f\r\n",heading_Kp, heading_Ki, heading_Kd);
         }
     }
 }
@@ -1190,22 +912,9 @@
     tickerAzThrusters.attach(&az_thruster_logic, ticker_rate);   
 }
 
-//Initialize controllers associated with Elevation, depth and pitch.
-void init_ElController(void){
-    // Zero out PID values.
-    pid_depth.clear();
-    pid_pitch.clear();
-
-    // run El controller as set in globals, in ms
-    tickerElThrusters.attach(&el_thruster_logic, ticker_rate);
-}
-
 int main() {
     //engage plaidspeed
     pc.baud(baudrate);
-//  press_sensor.init(sensor_rate,0);   //02BA pressure sensor
-    press_sensor.init(sensor_rate,press_sensor_type);   //30BA pressure sensor
-    press_sensor.density(997);
 
     //Uncomment to derive timing using Oscope.
     //function_timer=0;
@@ -1228,10 +937,10 @@
     if (imu.chip_ready()) {
         ready_prefix=0xffff;
     }
-    switch_pos(1);  //BNO default position 1
+    switch_pos(0);  //BNO default position 1
+    switch_mode(0x08);      //0x08      MODE_IMU
 
     init_AzController();
-    init_ElController();
 
     //Look for serial input commands and send to 'commands' function.
     //If no serial input commands, stream data.

--- a/BNO055_fusion.lib	Mon Feb 03 18:37:11 2020 +0000
+++ b/BNO055_fusion.lib	Thu Nov 12 20:25:07 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/pmmccorkell/code/BNO055_fusion/#476e1789d63f
+https://os.mbed.com/users/pmmccorkell/code/BNO055_fusion_AUV/#909d9978003f

--- a/MS5837.lib	Mon Feb 03 18:37:11 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/pmmccorkell/code/MS5837/#28795d9a1d76

--- a/PIDusna/PID.cpp	Mon Feb 03 18:37:11 2020 +0000
+++ b/PIDusna/PID.cpp	Thu Nov 12 20:25:07 2020 +0000
@@ -14,11 +14,12 @@
 // K values as floats.
 // dt should be set to same as Ticker that uses this class.
 // deadzone represents the acceptable error, defaults to 0.
-PID::PID (float Kp, float Ki, float Kd, int dt, float deadzone)
+PID::PID (float Kp, float Ki, float Kd, float dt, float deadzone)
 {
     set_dt(dt);
     set_K(Kp, Ki, Kd);
     set_deadzone(deadzone);
+    clear();
 }
 
 /*
@@ -76,7 +77,7 @@
 // }
 
 // Change dt.
-void PID::set_dt(int dt) 
+void PID::set_dt(float dt) 
 {
     _dt=dt;
 }
@@ -158,26 +159,30 @@
 // Overloaded version to give function the error directly.
 float PID::process(float error)
 {
+    float k_term=0;
+    float i_term=0;
+    float d_term=0;
+    float out_PID=0;
     // If abs value of error is smaller than the deadzone,
     //  cause all the PID gains to zeroize.
-    if (abs(error)<_deadzone) {
+    if ((abs(error))<_deadzone) {
         clear_integral();
         clear_error_previous();
         error=0;
     }
 
     // Proportional = Kp * e
-    float k_term = (_Kp*error);
+    k_term = (_Kp*error);
 
     // Integral = Ki * e dt
     _integral+=(error*_dt);
-    float i_term = (_Ki*_integral);
+    i_term = (_Ki*_integral);
 
     // Derivative = Kd * (de/dt)
-    float d_term = (_Kd* ((error-_error_previous)/_dt) );
+    d_term = (_Kd* ((error-_error_previous)/_dt) );
 
     // PID = P + I + D
-    float out_PID = k_term+i_term+d_term;
+    out_PID = k_term+i_term+d_term;
 
     // Update last error for next Derivative calculation.
     _error_previous=error;

--- a/PIDusna/PID.h	Mon Feb 03 18:37:11 2020 +0000
+++ b/PIDusna/PID.h	Thu Nov 12 20:25:07 2020 +0000
@@ -19,9 +19,9 @@
 class PID
 {
 public:
-    PID (float Kp, float Ki, float Kd, int dt, float deadzone=0);
+    PID (float Kp, float Ki, float Kd, float dt, float deadzone=0);
     void calculate_K(float Tu);
-    void set_dt(int dt);
+    void set_dt(float dt);
     void clear_integral(); 
     void clear_error();
     void clear_error_previous();
@@ -38,7 +38,7 @@
     float _Kd;
     float _floor;
     float _deadzone;
-    int _dt;
+    float _dt;
     float _error_previous;
     float _integral;
 

--- a/mbed-os.lib	Mon Feb 03 18:37:11 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://github.com/ARMmbed/mbed-os/#430e64fce8098fd5e54f6de32f3f029f35dc705f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 12 20:25:07 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file