Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed MS5837 LSM9DS1project SDFileSystemproject SCI_SENSORproject
main.cpp@0:47a98d724c30, 2020-12-01 (annotated)
- Committer:
- alekboving
- Date:
- Tue Dec 01 15:02:30 2020 +0000
- Revision:
- 0:47a98d724c30
- Child:
- 1:90d5de35324c
initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
alekboving | 0:47a98d724c30 | 1 | /* |
alekboving | 0:47a98d724c30 | 2 | Author Mingxi Zhou |
alekboving | 0:47a98d724c30 | 3 | OCE360 underwater float template program |
alekboving | 0:47a98d724c30 | 4 | */ |
alekboving | 0:47a98d724c30 | 5 | #include "mbed.h" |
alekboving | 0:47a98d724c30 | 6 | #include "LSM9DS1.h" //IMU library |
alekboving | 0:47a98d724c30 | 7 | #include "MS5837.h" //pressure sensor library |
alekboving | 0:47a98d724c30 | 8 | #include "SCI_SENSOR.h" //science sensor |
alekboving | 0:47a98d724c30 | 9 | #include "SDFileSystem.h" // SD card |
alekboving | 0:47a98d724c30 | 10 | |
alekboving | 0:47a98d724c30 | 11 | DigitalOut myled(LED1); |
alekboving | 0:47a98d724c30 | 12 | Serial pc(USBTX, USBRX); //initial serial |
alekboving | 0:47a98d724c30 | 13 | Serial BLE(p13,p14); //Bluetooth |
alekboving | 0:47a98d724c30 | 14 | LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); //initial IMU |
alekboving | 0:47a98d724c30 | 15 | LM19 temp(p19); |
alekboving | 0:47a98d724c30 | 16 | PhotoCell light(p20); |
alekboving | 0:47a98d724c30 | 17 | MS5837 p_sensor(p9, p10, ms5837_addr_no_CS); //pressure sensor |
alekboving | 0:47a98d724c30 | 18 | PwmOut thruster(p21); //set PWM pin //max 1.3ms min 1.1ms |
alekboving | 0:47a98d724c30 | 19 | PwmOut thruster2(p22); //set PWM pin |
alekboving | 0:47a98d724c30 | 20 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
alekboving | 0:47a98d724c30 | 21 | |
alekboving | 0:47a98d724c30 | 22 | //global ticker |
alekboving | 0:47a98d724c30 | 23 | Ticker log_ticker; |
alekboving | 0:47a98d724c30 | 24 | Ticker imu_ticker; |
alekboving | 0:47a98d724c30 | 25 | // global timer |
alekboving | 0:47a98d724c30 | 26 | Timer t; |
alekboving | 0:47a98d724c30 | 27 | //MS5837 p_sensor(p9, p10, ms5837_addr_no_CS); |
alekboving | 0:47a98d724c30 | 28 | ///File |
alekboving | 0:47a98d724c30 | 29 | FILE *fp; |
alekboving | 0:47a98d724c30 | 30 | char fname[100]; |
alekboving | 0:47a98d724c30 | 31 | float PI = 3.14159265358979323846f; |
alekboving | 0:47a98d724c30 | 32 | |
alekboving | 0:47a98d724c30 | 33 | //float operation parameters |
alekboving | 0:47a98d724c30 | 34 | float target_depth=0; //global target depth default 0 |
alekboving | 0:47a98d724c30 | 35 | int yo_num=0; //global yo_num default 0 |
alekboving | 0:47a98d724c30 | 36 | float thrust_on_time=0; //global thrust_on time default 0 |
alekboving | 0:47a98d724c30 | 37 | float accel[3], mag[3], gyro[3], euler[3]; //global IMU data |
alekboving | 0:47a98d724c30 | 38 | |
alekboving | 0:47a98d724c30 | 39 | //functions |
alekboving | 0:47a98d724c30 | 40 | void welcome(); |
alekboving | 0:47a98d724c30 | 41 | void log_data(); |
alekboving | 0:47a98d724c30 | 42 | //IMU related |
alekboving | 0:47a98d724c30 | 43 | void IMU_update(); //update IMU related varibles. we use imu_ticker to call this function |
alekboving | 0:47a98d724c30 | 44 | void mag_correction(float mx, float my, float mz, float mag_c[3]); //raw mag -> mag[3], mag_c[3] calibrated |
alekboving | 0:47a98d724c30 | 45 | void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]); //pose estimation function |
alekboving | 0:47a98d724c30 | 46 | //Control related functions |
alekboving | 0:47a98d724c30 | 47 | void thrust_on(float pw, float on_time); //input is pulse width |
alekboving | 0:47a98d724c30 | 48 | |
alekboving | 0:47a98d724c30 | 49 | //-------------Main functions----------------------------------------------------------------------------------------- |
alekboving | 0:47a98d724c30 | 50 | int main() |
alekboving | 0:47a98d724c30 | 51 | { |
alekboving | 0:47a98d724c30 | 52 | //-----Initialization realted code-------// |
alekboving | 0:47a98d724c30 | 53 | //inital set the thruster esc to 1ms duty cycle |
alekboving | 0:47a98d724c30 | 54 | thruster.period(0.002); // 2 ms period |
alekboving | 0:47a98d724c30 | 55 | thruster.pulsewidth(1.0/1000.000); /////IMU initial and begin |
alekboving | 0:47a98d724c30 | 56 | thruster2.period(0.002); // 2 ms period |
alekboving | 0:47a98d724c30 | 57 | thruster2.pulsewidth(1.0/1000.000); /////IMU initial and begin |
alekboving | 0:47a98d724c30 | 58 | IMU.begin(); |
alekboving | 0:47a98d724c30 | 59 | IMU.calibrate(true); |
alekboving | 0:47a98d724c30 | 60 | myled=1; |
alekboving | 0:47a98d724c30 | 61 | //initialize pressure sensor |
alekboving | 0:47a98d724c30 | 62 | pc.printf("setting the pressure sensor\r\n"); |
alekboving | 0:47a98d724c30 | 63 | p_sensor.MS5837Reset(); |
alekboving | 0:47a98d724c30 | 64 | p_sensor.MS5837Init(); |
alekboving | 0:47a98d724c30 | 65 | pc.printf("settting the tickers\r\n"); |
alekboving | 0:47a98d724c30 | 66 | t.start(); |
alekboving | 0:47a98d724c30 | 67 | myled=0; |
alekboving | 0:47a98d724c30 | 68 | welcome(); |
alekboving | 0:47a98d724c30 | 69 | //-----setup ticker-------// |
alekboving | 0:47a98d724c30 | 70 | //setup ticker to separate log and IMU data update. |
alekboving | 0:47a98d724c30 | 71 | //so we could have all our control code in the while loop |
alekboving | 0:47a98d724c30 | 72 | // //log at 2 Hz |
alekboving | 0:47a98d724c30 | 73 | imu_ticker.attach(&IMU_update,0.1); //10Hz |
alekboving | 0:47a98d724c30 | 74 | log_ticker.attach(&log_data,0.5); |
alekboving | 0:47a98d724c30 | 75 | wait(1); |
alekboving | 0:47a98d724c30 | 76 | while(1) { |
alekboving | 0:47a98d724c30 | 77 | // put your main control code here |
alekboving | 0:47a98d724c30 | 78 | |
alekboving | 0:47a98d724c30 | 79 | |
alekboving | 0:47a98d724c30 | 80 | |
alekboving | 0:47a98d724c30 | 81 | |
alekboving | 0:47a98d724c30 | 82 | } |
alekboving | 0:47a98d724c30 | 83 | |
alekboving | 0:47a98d724c30 | 84 | } |
alekboving | 0:47a98d724c30 | 85 | |
alekboving | 0:47a98d724c30 | 86 | //-------------Customized functions---------------------------------------------//---------------------------------------- |
alekboving | 0:47a98d724c30 | 87 | ///-----------Welcome menu---------------------/// |
alekboving | 0:47a98d724c30 | 88 | void welcome() |
alekboving | 0:47a98d724c30 | 89 | { |
alekboving | 0:47a98d724c30 | 90 | char buffer[100]= {0}; |
alekboving | 0:47a98d724c30 | 91 | int flag=1; |
alekboving | 0:47a98d724c30 | 92 | //Flush the port |
alekboving | 0:47a98d724c30 | 93 | while(BLE.readable()) { |
alekboving | 0:47a98d724c30 | 94 | BLE.getc(); |
alekboving | 0:47a98d724c30 | 95 | } |
alekboving | 0:47a98d724c30 | 96 | while(flag) { |
alekboving | 0:47a98d724c30 | 97 | BLE.printf("### I am alive\r\n"); |
alekboving | 0:47a98d724c30 | 98 | BLE.printf("### Please enter the log file name you want\r\n"); |
alekboving | 0:47a98d724c30 | 99 | if(BLE.readable()) { |
alekboving | 0:47a98d724c30 | 100 | BLE.scanf("%s",buffer); |
alekboving | 0:47a98d724c30 | 101 | sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name |
alekboving | 0:47a98d724c30 | 102 | |
alekboving | 0:47a98d724c30 | 103 | flag = 0; //set the flag to 0 to break the while |
alekboving | 0:47a98d724c30 | 104 | } |
alekboving | 0:47a98d724c30 | 105 | myled=!myled; |
alekboving | 0:47a98d724c30 | 106 | wait(1); |
alekboving | 0:47a98d724c30 | 107 | } |
alekboving | 0:47a98d724c30 | 108 | //print name |
alekboving | 0:47a98d724c30 | 109 | BLE.printf("### name received\r\n"); |
alekboving | 0:47a98d724c30 | 110 | BLE.printf("### file name and directory is: \r\n %s\r\n",fname); //file name and location |
alekboving | 0:47a98d724c30 | 111 | //open file test |
alekboving | 0:47a98d724c30 | 112 | mkdir("/sd/mydir",0777); //keep 0777, this is magic # |
alekboving | 0:47a98d724c30 | 113 | fp = fopen(fname, "a"); |
alekboving | 0:47a98d724c30 | 114 | if(fp == NULL) { |
alekboving | 0:47a98d724c30 | 115 | BLE.printf("Could not open file for write\n"); |
alekboving | 0:47a98d724c30 | 116 | } else { |
alekboving | 0:47a98d724c30 | 117 | BLE.printf("##file open good \n"); //open file and tell if open |
alekboving | 0:47a98d724c30 | 118 | fprintf(fp, "Hello\r\n"); |
alekboving | 0:47a98d724c30 | 119 | fclose(fp); |
alekboving | 0:47a98d724c30 | 120 | } |
alekboving | 0:47a98d724c30 | 121 | |
alekboving | 0:47a98d724c30 | 122 | BLE.printf("### The main program will start in 10 seconds\r\n"); |
alekboving | 0:47a98d724c30 | 123 | wait(5); |
alekboving | 0:47a98d724c30 | 124 | } |
alekboving | 0:47a98d724c30 | 125 | |
alekboving | 0:47a98d724c30 | 126 | ///-----------log functions---------------------/// |
alekboving | 0:47a98d724c30 | 127 | void log_data() |
alekboving | 0:47a98d724c30 | 128 | { |
alekboving | 0:47a98d724c30 | 129 | //log system time t.read() |
alekboving | 0:47a98d724c30 | 130 | // log imu data, log sciene data |
alekboving | 0:47a98d724c30 | 131 | // log pulse width |
alekboving | 0:47a98d724c30 | 132 | // log pressure sensor data. |
alekboving | 0:47a98d724c30 | 133 | //science sensor: temp.temp(), light.light() |
alekboving | 0:47a98d724c30 | 134 | //IMU sensor |
alekboving | 0:47a98d724c30 | 135 | |
alekboving | 0:47a98d724c30 | 136 | } |
alekboving | 0:47a98d724c30 | 137 | |
alekboving | 0:47a98d724c30 | 138 | ///-----------IMU related functions---------------------/// |
alekboving | 0:47a98d724c30 | 139 | void IMU_update() |
alekboving | 0:47a98d724c30 | 140 | { |
alekboving | 0:47a98d724c30 | 141 | IMU.readMag(); |
alekboving | 0:47a98d724c30 | 142 | IMU.readGyro(); |
alekboving | 0:47a98d724c30 | 143 | IMU.readAccel(); |
alekboving | 0:47a98d724c30 | 144 | accel[0] = IMU.calcAccel(IMU.ax); |
alekboving | 0:47a98d724c30 | 145 | accel[1] = IMU.calcAccel(IMU.ay); |
alekboving | 0:47a98d724c30 | 146 | accel[2] = -IMU.calcAccel(IMU.az); |
alekboving | 0:47a98d724c30 | 147 | gyro[0] = IMU.calcGyro(IMU.gx); |
alekboving | 0:47a98d724c30 | 148 | gyro[1] = IMU.calcGyro(IMU.gy); |
alekboving | 0:47a98d724c30 | 149 | gyro[2] = -IMU.calcGyro(IMU.gz); |
alekboving | 0:47a98d724c30 | 150 | mag_correction(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz), mag); //mag correction |
alekboving | 0:47a98d724c30 | 151 | mag[2] = - mag[2]; |
alekboving | 0:47a98d724c30 | 152 | pose_estimate(euler, accel, gyro, mag); //pose update |
alekboving | 0:47a98d724c30 | 153 | } |
alekboving | 0:47a98d724c30 | 154 | |
alekboving | 0:47a98d724c30 | 155 | void mag_correction(float mx, float my, float mz, float mag_c[3]) |
alekboving | 0:47a98d724c30 | 156 | { |
alekboving | 0:47a98d724c30 | 157 | float bias[3] = {0.0793,0.0357,0.2333}; |
alekboving | 0:47a98d724c30 | 158 | float scale[3][3] = {{1.0070, 0.0705, 0.0368}, |
alekboving | 0:47a98d724c30 | 159 | {0.0705, 1.0807, 0.0265}, |
alekboving | 0:47a98d724c30 | 160 | {0.0368, 0.0265, 0.9250} |
alekboving | 0:47a98d724c30 | 161 | }; |
alekboving | 0:47a98d724c30 | 162 | //mag_c = (mag-bias)*scale |
alekboving | 0:47a98d724c30 | 163 | |
alekboving | 0:47a98d724c30 | 164 | mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0]; |
alekboving | 0:47a98d724c30 | 165 | mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1]; |
alekboving | 0:47a98d724c30 | 166 | mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2]; |
alekboving | 0:47a98d724c30 | 167 | } |
alekboving | 0:47a98d724c30 | 168 | |
alekboving | 0:47a98d724c30 | 169 | void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3]) //pose estimation function |
alekboving | 0:47a98d724c30 | 170 | { |
alekboving | 0:47a98d724c30 | 171 | euler[0] = atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2])))); |
alekboving | 0:47a98d724c30 | 172 | euler[1] = - atan2( -accel[0],( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])))); |
alekboving | 0:47a98d724c30 | 173 | float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0])); |
alekboving | 0:47a98d724c30 | 174 | float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1])) |
alekboving | 0:47a98d724c30 | 175 | + (mag[2] * cos(euler[0]) * sin(euler[1])); |
alekboving | 0:47a98d724c30 | 176 | euler[2] = atan2(Yh, Xh); |
alekboving | 0:47a98d724c30 | 177 | //convert into degrees |
alekboving | 0:47a98d724c30 | 178 | euler[0] *= 180.0f / PI; |
alekboving | 0:47a98d724c30 | 179 | euler[1] *= 180.0f / PI; |
alekboving | 0:47a98d724c30 | 180 | euler[2] *= 180.0f / PI; |
alekboving | 0:47a98d724c30 | 181 | //wrap the values to be within 0 to 360. |
alekboving | 0:47a98d724c30 | 182 | for (int i=0; i<3; i++) { |
alekboving | 0:47a98d724c30 | 183 | if(euler[i]<=0) { |
alekboving | 0:47a98d724c30 | 184 | euler[i]=euler[i]+360; |
alekboving | 0:47a98d724c30 | 185 | } |
alekboving | 0:47a98d724c30 | 186 | if(euler[i]>360) { |
alekboving | 0:47a98d724c30 | 187 | euler[i]=euler[i]-360; |
alekboving | 0:47a98d724c30 | 188 | } |
alekboving | 0:47a98d724c30 | 189 | } |
alekboving | 0:47a98d724c30 | 190 | |
alekboving | 0:47a98d724c30 | 191 | } |
alekboving | 0:47a98d724c30 | 192 | |
alekboving | 0:47a98d724c30 | 193 | ///-----------Control related functions---------------------/// |
alekboving | 0:47a98d724c30 | 194 | ////Thruster on control, pw->pulse width in milli-second// |
alekboving | 0:47a98d724c30 | 195 | //// pw range between 1 to 1.5// |
alekboving | 0:47a98d724c30 | 196 | //// on_time-> thruster on time. |
alekboving | 0:47a98d724c30 | 197 | void thrust_on(float pw, float on_time) //input is pulse width |
alekboving | 0:47a98d724c30 | 198 | { |
alekboving | 0:47a98d724c30 | 199 | float pw_max=2.0; |
alekboving | 0:47a98d724c30 | 200 | if(pw>pw_max) { |
alekboving | 0:47a98d724c30 | 201 | pw=pw_max; //hard limitation |
alekboving | 0:47a98d724c30 | 202 | } |
alekboving | 0:47a98d724c30 | 203 | Timer tt; |
alekboving | 0:47a98d724c30 | 204 | tt.reset(); |
alekboving | 0:47a98d724c30 | 205 | tt.start(); |
alekboving | 0:47a98d724c30 | 206 | // lets set the pulse width |
alekboving | 0:47a98d724c30 | 207 | //thruster.period(20.0/1000.00); // 20 ms period |
alekboving | 0:47a98d724c30 | 208 | thruster.pulsewidth(pw/1000.00); |
alekboving | 0:47a98d724c30 | 209 | thruster2.pulsewidth(pw/1000.00); |
alekboving | 0:47a98d724c30 | 210 | //PWM will be kept until time out |
alekboving | 0:47a98d724c30 | 211 | while(tt.read()<=on_time) { |
alekboving | 0:47a98d724c30 | 212 | } |
alekboving | 0:47a98d724c30 | 213 | //stop the timer |
alekboving | 0:47a98d724c30 | 214 | tt.stop(); |
alekboving | 0:47a98d724c30 | 215 | //turn off the thruster |
alekboving | 0:47a98d724c30 | 216 | thruster.pulsewidth(1.0/1000.00); |
alekboving | 0:47a98d724c30 | 217 | thruster2.pulsewidth(1.0/1000.00); |
alekboving | 0:47a98d724c30 | 218 | |
alekboving | 0:47a98d724c30 | 219 | } |