HEPTA-Sat Training 2019 / Mbed 2 deprecated Lab4-04_Detect_Angular_velocity_x-axis

Lab4

Dependencies:   mbed HEPTA_EPS

main.cpp@2:1c5cdb2c3e0f, 2017-08-23 (annotated)

Committer:
umeume
Date:
Wed Aug 23 06:19:35 2017 +0000
Revision:
2:1c5cdb2c3e0f
Parent:
0:bdbd3d6fc5d5
Child:
5:c5ccb1b07e8f
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:bdbd3d6fc5d5 1 #include "mbed.h"
mbed_official 0:bdbd3d6fc5d5 2 #include "SDFileSystem.h"
umeume 2:1c5cdb2c3e0f 3 #include "HeptaXbee.h"
umeume 2:1c5cdb2c3e0f 4 #include "HeptaCamera_GPS.h"
umeume 2:1c5cdb2c3e0f 5 #include "Hepta9axis.h"
umeume 2:1c5cdb2c3e0f 6 #include "HeptaTemp.h"
umeume 2:1c5cdb2c3e0f 7 #include "HeptaBattery.h"
umeume 2:1c5cdb2c3e0f 8
umeume 2:1c5cdb2c3e0f 9 Serial pc(USBTX,USBRX);
umeume 2:1c5cdb2c3e0f 10 SDFileSystem sd(p5, p6, p7, p8, "fs");
umeume 2:1c5cdb2c3e0f 11 HeptaXbee xbee(p9,p10);
umeume 2:1c5cdb2c3e0f 12 HeptaCamera_GPS cam_gps(p13, p14,p25,p24);
umeume 2:1c5cdb2c3e0f 13 Hepta9axis _9axis(p28,p27,0xD0,0x18);//sda,scl,acc&gyro_address,mag_gyro
umeume 2:1c5cdb2c3e0f 14 HeptaTemp heptatemp(p17);
umeume 2:1c5cdb2c3e0f 15 HeptaBattery bat(p16,p26);
umeume 2:1c5cdb2c3e0f 16
umeume 2:1c5cdb2c3e0f 17 int main()
umeume 2:1c5cdb2c3e0f 18 {
umeume 2:1c5cdb2c3e0f 19 char mode;
umeume 2:1c5cdb2c3e0f 20 pc.baud(9600);
umeume 2:1c5cdb2c3e0f 21 pc.printf("Hello world.\r\n");
umeume 2:1c5cdb2c3e0f 22 while(1) {
umeume 2:1c5cdb2c3e0f 23 pc.printf("\r\n");
umeume 2:1c5cdb2c3e0f 24 pc.printf("*********************************\r\n");
umeume 2:1c5cdb2c3e0f 25 pc.printf("Please select mode.\r\n");
umeume 2:1c5cdb2c3e0f 26 pc.printf("a:SD test Mode\r\n");
umeume 2:1c5cdb2c3e0f 27 pc.printf("b:Check Battery Level\r\n");
umeume 2:1c5cdb2c3e0f 28 pc.printf("c:Gyro Sening Mode\r\n");
umeume 2:1c5cdb2c3e0f 29 pc.printf("d:Accel Sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 30 pc.printf("e:Magnet Sensig Mode\r\n");
umeume 2:1c5cdb2c3e0f 31 pc.printf("f:GPS Test Sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 32 pc.printf("g:GPS data_GPGGA Mode\r\n");
umeume 2:1c5cdb2c3e0f 33 pc.printf("h:Camera Synchro Mode\r\n");
umeume 2:1c5cdb2c3e0f 34 pc.printf("i:CAM SnapShot Mode\r\n");
umeume 2:1c5cdb2c3e0f 35 pc.printf("j:Saving Camera Data Mode\r\n");
umeume 2:1c5cdb2c3e0f 36 pc.printf("k:Camera Data Transmitting Mode\r\n");
umeume 2:1c5cdb2c3e0f 37 pc.printf("l:Temperature Sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 38 pc.printf("m:Xbee Mode\r\n");
umeume 2:1c5cdb2c3e0f 39 pc.printf("n:All Transmitting Mode\r\n");;
umeume 2:1c5cdb2c3e0f 40 pc.printf("*********************************");
umeume 2:1c5cdb2c3e0f 41 pc.printf("\r\n");
umeume 2:1c5cdb2c3e0f 42
umeume 2:1c5cdb2c3e0f 43 mode = pc.getc();
umeume 2:1c5cdb2c3e0f 44 pc.printf("Your select Mode = %c\r\n",mode);
umeume 2:1c5cdb2c3e0f 45
umeume 2:1c5cdb2c3e0f 46 switch(mode) {
umeume 2:1c5cdb2c3e0f 47 case'a': {
umeume 2:1c5cdb2c3e0f 48 pc.printf("SD test Mode\r\n");
umeume 2:1c5cdb2c3e0f 49 FILE *fp = fopen("/fs/myfile.txt", "w");
umeume 2:1c5cdb2c3e0f 50 if(fp == NULL) {
umeume 2:1c5cdb2c3e0f 51 pc.printf("Could not open file for write\r\n");
umeume 2:1c5cdb2c3e0f 52 } else {
umeume 2:1c5cdb2c3e0f 53 fprintf(fp, "\n\rHello World!\n\r");
umeume 2:1c5cdb2c3e0f 54 pc.printf("SD Check Complete!!\r\n");
umeume 2:1c5cdb2c3e0f 55 fclose(fp);
umeume 2:1c5cdb2c3e0f 56 }
umeume 2:1c5cdb2c3e0f 57 break;
umeume 2:1c5cdb2c3e0f 58 }//case'a'
umeume 2:1c5cdb2c3e0f 59
umeume 2:1c5cdb2c3e0f 60 case'b': {
umeume 2:1c5cdb2c3e0f 61 pc.printf("Check Battery Level\r\n");
umeume 2:1c5cdb2c3e0f 62 float bt;
umeume 2:1c5cdb2c3e0f 63 for(int i = 0; i<30; i++) {
umeume 2:1c5cdb2c3e0f 64 bat.vol(&bt);
umeume 2:1c5cdb2c3e0f 65 pc.printf("V = %f\r\n",bt);
umeume 2:1c5cdb2c3e0f 66 wait(0.5);
umeume 2:1c5cdb2c3e0f 67 }
umeume 2:1c5cdb2c3e0f 68 break;
umeume 2:1c5cdb2c3e0f 69 }//case'b'
umeume 2:1c5cdb2c3e0f 70
umeume 2:1c5cdb2c3e0f 71 case'c': {
umeume 2:1c5cdb2c3e0f 72 pc.printf("Gyro sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 73 float gx,gy,gz;
umeume 2:1c5cdb2c3e0f 74 for(int i = 0; i < 30; i++) {
umeume 2:1c5cdb2c3e0f 75 _9axis.sen_gyro(&gx,&gy,&gz);
umeume 2:1c5cdb2c3e0f 76 pc.printf("GX = %f,GY = %f,GZ = %f\r\n",gx,gy,gz);
umeume 2:1c5cdb2c3e0f 77 wait(0.5);
umeume 2:1c5cdb2c3e0f 78 }
umeume 2:1c5cdb2c3e0f 79 break;
umeume 2:1c5cdb2c3e0f 80 }//case'c'
umeume 2:1c5cdb2c3e0f 81
umeume 2:1c5cdb2c3e0f 82 case'd': {
umeume 2:1c5cdb2c3e0f 83 pc.printf("Accel sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 84 float ax,ay,az;
umeume 2:1c5cdb2c3e0f 85 for(int i = 0; i < 30; i++) {
umeume 2:1c5cdb2c3e0f 86 _9axis.sen_acc(&ax,&ay,&az);
umeume 2:1c5cdb2c3e0f 87 pc.printf("AX = %f,AY = %f,AZ = %f\r\n",ax,ay,az);
umeume 2:1c5cdb2c3e0f 88 wait(0.5);
umeume 2:1c5cdb2c3e0f 89 }
umeume 2:1c5cdb2c3e0f 90 break;
umeume 2:1c5cdb2c3e0f 91 }//case'd'
umeume 2:1c5cdb2c3e0f 92
umeume 2:1c5cdb2c3e0f 93 case'e': {
umeume 2:1c5cdb2c3e0f 94 float mx,my,mz;
umeume 2:1c5cdb2c3e0f 95 pc.printf("Magnet sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 96 for(int i = 0; i < 30; i++) {
umeume 2:1c5cdb2c3e0f 97 _9axis.sen_mag(&mx,&my,&mz);
umeume 2:1c5cdb2c3e0f 98 pc.printf("MX = %f,MY = %f,MZ = %f\r\n",mx,my,mz);
umeume 2:1c5cdb2c3e0f 99 wait(0.5);
umeume 2:1c5cdb2c3e0f 100 }
umeume 2:1c5cdb2c3e0f 101 break;
umeume 2:1c5cdb2c3e0f 102 }//case'e'
umeume 2:1c5cdb2c3e0f 103
umeume 2:1c5cdb2c3e0f 104 case'f': {
umeume 2:1c5cdb2c3e0f 105 pc.printf("GPS sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 106 cam_gps.gps_setting();
umeume 2:1c5cdb2c3e0f 107 cam_gps.flushSerialBuffer();
umeume 2:1c5cdb2c3e0f 108 while(1) pc.putc(cam_gps.getc());
umeume 2:1c5cdb2c3e0f 109 break;
umeume 2:1c5cdb2c3e0f 110 }//case'f'
umeume 2:1c5cdb2c3e0f 111
umeume 2:1c5cdb2c3e0f 112
umeume 2:1c5cdb2c3e0f 113 case'g': {
umeume 2:1c5cdb2c3e0f 114 cam_gps.gps_setting();
umeume 2:1c5cdb2c3e0f 115 cam_gps.flushSerialBuffer();
umeume 2:1c5cdb2c3e0f 116 pc.printf("GPS GPGGA Mode\r\n");
umeume 2:1c5cdb2c3e0f 117 int quality=0,stnum=0,gps_check=0;
umeume 2:1c5cdb2c3e0f 118 char ns='A',ew='B',aunit='m';
umeume 2:1c5cdb2c3e0f 119 float time=0.0,latitude=0.0,longitude=0.0,hacu=0.0,altitude=0.0;
umeume 2:1c5cdb2c3e0f 120 for(int i=1; i<10; i++) {
umeume 2:1c5cdb2c3e0f 121 cam_gps.gga_sensing(&time, &latitude, &ns, &longitude, &ew, &quality, &stnum, &hacu, &altitude, &aunit, &gps_check);
umeume 2:1c5cdb2c3e0f 122 if((gps_check==0)|(gps_check==1)) {
umeume 2:1c5cdb2c3e0f 123 pc.printf("GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c\r\n",time,latitude,ns,longitude,ew,quality,stnum,hacu,altitude,aunit);
umeume 2:1c5cdb2c3e0f 124 }
umeume 2:1c5cdb2c3e0f 125 }
umeume 2:1c5cdb2c3e0f 126 break;
umeume 2:1c5cdb2c3e0f 127 }//case'g'
umeume 2:1c5cdb2c3e0f 128
umeume 2:1c5cdb2c3e0f 129
umeume 2:1c5cdb2c3e0f 130 case'h': {
umeume 2:1c5cdb2c3e0f 131 wait(0.5);
umeume 2:1c5cdb2c3e0f 132 pc.printf("Camera Synchro\r\n");
umeume 2:1c5cdb2c3e0f 133 cam_gps.Sync();
umeume 2:1c5cdb2c3e0f 134 break;
umeume 2:1c5cdb2c3e0f 135 }//case'h'
umeume 2:1c5cdb2c3e0f 136
umeume 2:1c5cdb2c3e0f 137 case'i': {
umeume 2:1c5cdb2c3e0f 138 pc.printf("CAM snapshot Mode\r\n");
umeume 2:1c5cdb2c3e0f 139 cam_gps.initialize(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
umeume 2:1c5cdb2c3e0f 140 cam_gps.test_jpeg_snapshot_picture(1);
umeume 2:1c5cdb2c3e0f 141
umeume 2:1c5cdb2c3e0f 142 break;
umeume 2:1c5cdb2c3e0f 143 }//case'i'
umeume 2:1c5cdb2c3e0f 144
umeume 2:1c5cdb2c3e0f 145 case'j': {
umeume 2:1c5cdb2c3e0f 146 pc.printf("Saving Camera Data Mode\r\n");
umeume 2:1c5cdb2c3e0f 147 cam_gps.initialize(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
umeume 2:1c5cdb2c3e0f 148 cam_gps.test_jpeg_snapshot_data(1);
umeume 2:1c5cdb2c3e0f 149
umeume 2:1c5cdb2c3e0f 150 break;
umeume 2:1c5cdb2c3e0f 151 }//case'j'
umeume 2:1c5cdb2c3e0f 152
umeume 2:1c5cdb2c3e0f 153 case'k': {
umeume 2:1c5cdb2c3e0f 154 pc.printf("Camera Data Transmitting Mode\r\n");
umeume 2:1c5cdb2c3e0f 155 cam_gps.initialize(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
umeume 2:1c5cdb2c3e0f 156 cam_gps.test_jpeg_snapshot_picture(1);
umeume 2:1c5cdb2c3e0f 157
umeume 2:1c5cdb2c3e0f 158 FILE *fp = fopen("/fs/test.txt", "r");
umeume 2:1c5cdb2c3e0f 159 if(fp == NULL) {
umeume 2:1c5cdb2c3e0f 160 pc.printf("Could not open file for write\r\n");
umeume 2:1c5cdb2c3e0f 161 } else {
umeume 2:1c5cdb2c3e0f 162 char str[1024];
umeume 2:1c5cdb2c3e0f 163 while((fgets(str,256,fp))!=NULL) {
umeume 2:1c5cdb2c3e0f 164 //pc.printf("%s",str);
umeume 2:1c5cdb2c3e0f 165 pc.printf("%s",str);
umeume 2:1c5cdb2c3e0f 166 wait(0.001);
umeume 2:1c5cdb2c3e0f 167 }
umeume 2:1c5cdb2c3e0f 168 fclose(fp);
umeume 2:1c5cdb2c3e0f 169 }
umeume 2:1c5cdb2c3e0f 170
umeume 2:1c5cdb2c3e0f 171 break;
umeume 2:1c5cdb2c3e0f 172 }//case'k'
umeume 2:1c5cdb2c3e0f 173
umeume 2:1c5cdb2c3e0f 174 case'l': {
umeume 2:1c5cdb2c3e0f 175 pc.printf("Temp sensing Mode\r\n");
umeume 2:1c5cdb2c3e0f 176 float temp;
umeume 2:1c5cdb2c3e0f 177 for(int i = 0; i<100; i++) {
umeume 2:1c5cdb2c3e0f 178 heptatemp.temp_sense(&temp);
umeume 2:1c5cdb2c3e0f 179 pc.printf("temper = %f\r\n",temp);
umeume 2:1c5cdb2c3e0f 180 //xbee.printf("%f\r\n",temp);
umeume 2:1c5cdb2c3e0f 181 }
umeume 2:1c5cdb2c3e0f 182 break;
umeume 2:1c5cdb2c3e0f 183 }//case'l'
umeume 2:1c5cdb2c3e0f 184
umeume 2:1c5cdb2c3e0f 185 case'm': {
umeume 2:1c5cdb2c3e0f 186
umeume 2:1c5cdb2c3e0f 187 int i=0,rcmd=0,cmdflag=0;
umeume 2:1c5cdb2c3e0f 188 xbee.printf("Count Up!\r");
umeume 2:1c5cdb2c3e0f 189 while(1) {
umeume 2:1c5cdb2c3e0f 190 xbee.printf("num = %d\r",i);
umeume 2:1c5cdb2c3e0f 191 i++;
umeume 2:1c5cdb2c3e0f 192 wait(1.0);
umeume 2:1c5cdb2c3e0f 193 xbee.xbee_recieve(&rcmd,&cmdflag);
umeume 2:1c5cdb2c3e0f 194 pc.printf("rcmd=%d, cmdflag=%d\r\n",rcmd, cmdflag);
umeume 2:1c5cdb2c3e0f 195 if (cmdflag == 1) {
umeume 2:1c5cdb2c3e0f 196 if (rcmd == 'a') {
umeume 2:1c5cdb2c3e0f 197 xbee.printf("Command Get %d\r\n",rcmd);
umeume 2:1c5cdb2c3e0f 198 xbee.printf("HEPTA Uplink OK\r");
umeume 2:1c5cdb2c3e0f 199 }
umeume 2:1c5cdb2c3e0f 200 xbee.initialize();
umeume 2:1c5cdb2c3e0f 201 }
umeume 2:1c5cdb2c3e0f 202 }
umeume 2:1c5cdb2c3e0f 203
umeume 2:1c5cdb2c3e0f 204 break;
umeume 2:1c5cdb2c3e0f 205 }//case'm'
umeume 2:1c5cdb2c3e0f 206
umeume 2:1c5cdb2c3e0f 207 case'n': {
umeume 2:1c5cdb2c3e0f 208 cam_gps.gps_setting();
umeume 2:1c5cdb2c3e0f 209 cam_gps.flushSerialBuffer();
umeume 2:1c5cdb2c3e0f 210 pc.printf("All Transmitting Mode\r\n");
umeume 2:1c5cdb2c3e0f 211 //char iii = pc.getc();
umeume 2:1c5cdb2c3e0f 212 char gx[4],gy[4],gz[4],ax[4],ay[4],az[4],mx[4],my[4],mz[4],lad[8],log[8],height[4],bt[4],temp[4];
umeume 2:1c5cdb2c3e0f 213 char ddata[64];
umeume 2:1c5cdb2c3e0f 214 int dsize[7];
umeume 2:1c5cdb2c3e0f 215 int Count = 0;
umeume 2:1c5cdb2c3e0f 216 char fname[64];
umeume 2:1c5cdb2c3e0f 217 while(1) {
umeume 2:1c5cdb2c3e0f 218 snprintf(fname, sizeof(fname), "/fs/telemetry.txt");
umeume 2:1c5cdb2c3e0f 219 FILE*fpx = fopen(fname, "a");
umeume 2:1c5cdb2c3e0f 220 _9axis.sen_gyro_u16(gx,gy,gz,&dsize[0]);
umeume 2:1c5cdb2c3e0f 221 _9axis.sen_acc_u16(ax,ay,az,&dsize[1]);
umeume 2:1c5cdb2c3e0f 222 _9axis.sen_mag_u16(mx,my,mz,&dsize[2]);
umeume 2:1c5cdb2c3e0f 223 bat.vol_u16(bt,&dsize[5]);
umeume 2:1c5cdb2c3e0f 224 heptatemp.temp_sense_u16(temp,&dsize[6]);
umeume 2:1c5cdb2c3e0f 225 cam_gps.lat_log_sensing_u16(lad,log,height,&dsize[3],&dsize[4]);
umeume 2:1c5cdb2c3e0f 226 //xbee.printf("DN:%d",Count);
umeume 2:1c5cdb2c3e0f 227 xbee.xbee_transmit(ddata,64,gx,gy,gz,ax,ay,az,mx,my,mz,lad,log,height,bt,temp,dsize[0],dsize[0],dsize[0],dsize[1],dsize[1],dsize[1],dsize[2],dsize[2],dsize[2],dsize[3],dsize[3],dsize[4],dsize[5],dsize[6],14);
umeume 2:1c5cdb2c3e0f 228 fprintf(fpx,"DN%d:",Count);
umeume 2:1c5cdb2c3e0f 229 for(int ii = 0; ii<64; ii++) {
umeume 2:1c5cdb2c3e0f 230 fprintf(fpx,"%c",ddata[ii]);
umeume 2:1c5cdb2c3e0f 231 }
umeume 2:1c5cdb2c3e0f 232 fprintf(fpx,"\r\n");
umeume 2:1c5cdb2c3e0f 233 fclose(fpx);
umeume 2:1c5cdb2c3e0f 234 Count++;
umeume 2:1c5cdb2c3e0f 235 }
umeume 2:1c5cdb2c3e0f 236 break;
umeume 2:1c5cdb2c3e0f 237 }//case'9'
umeume 2:1c5cdb2c3e0f 238
umeume 2:1c5cdb2c3e0f 239 default:
umeume 2:1c5cdb2c3e0f 240 break;
umeume 2:1c5cdb2c3e0f 241
umeume 2:1c5cdb2c3e0f 242
umeume 2:1c5cdb2c3e0f 243 }
mbed_official 0:bdbd3d6fc5d5 244 }
umeume 2:1c5cdb2c3e0f 245 }