Atsumi Toda
UAV_Logger1から改造。サーボ機構のプログラムを追加
Dependencies: MPU6050_alter HMC5883L
Diff: main.cpp
- Revision:
- 2:e6496a794bde
- Parent:
- 1:9a5f06e7969e
- Child:
- 3:3fa7882a5fd0
--- a/main.cpp Thu Feb 14 10:37:28 2019 +0000
+++ b/main.cpp Fri May 24 05:57:12 2019 +0000
@@ -1,21 +1,12 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2013 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
#include "mbed.h"
#include <stdio.h>
#include <errno.h>
+#include "MPU6050.h"
+#include "HMC5883L.h"
+
+#include "Vector.h"
+#include "Matrix.h"
+#include "Vector_Matrix_operator.h"
// Block devices
//#include "SPIFBlockDevice.h"
@@ -28,192 +19,482 @@
//#include "LittleFileSystem.h"
#include "FATFileSystem.h"
-// Physical block device, can be any device that supports the BlockDevice API
-SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
+#define M_PI 3.141592
+#define PI2 (2*M_PI)
+
+/*地磁気センサの補正値（足し合わせる）*/
+#define MAG_FIX_X 338
+#define MAG_FIX_Y 20
+#define MAG_FIX_Z -50
+
+/*ジャイロセンサの補正値（引く）*/
+#define GYRO_FIX_X 290.5498
+#define GYRO_FIX_Y 99.29363
+#define GYRO_FIX_Z 61.41011
// File system declaration
FATFileSystem fileSystem("fs");
+/*-----割り込み--------*/
+Ticker flipper;
+/*--------------------*/
+/*-----タイマー---------*/
+Timer passed_time;
+/*---------------------*/
+/*-------ピン指定------------*/
+SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
+MPU6050 mpu(p28, p27);//sda scl
+HMC5883L compass(p28, p27);//sda scl
+
+RawSerial gps(p9,p10); //serial port for gps_module
+RawSerial pc(USBTX, USBRX);
+
+/*--------------------------*/
+
+DigitalIn switch_off(p30);
+DigitalOut led2(LED2);
+
+/*-----------グローバル変数-----------*/
+float g_hokui,g_tokei;
+int fp_count=0;
+int gps_flag=0;
+int gps_flag_conversion=0;
+
+char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
+char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している
+
+unsigned int tokei_int_receive;
+unsigned int hokui_int_receive;
+
+float tokei_float_receive;
+float hokui_float_receive;
+
+float a[3];//加速度の値
+float g[3];//ジャイロの値[rad/s]
+float dpsX,dpsY,dpsZ;
+float AccX,AccY,AccZ;
+
+int sensor_array[6];
+int16_t mag[3];
+
+double Roll_Acc,Pitch_Acc;
+double Yaw;
+double Azi_mag;//地磁気からの方位
+double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion
+
+/*----------------------------------*/
+/*--------------------------行列、ベクトル-----------------------------*/
+
+Vector Quaternion_atitude_from_omega(4);
+Matrix Omega_matrix(4,4),Half_matrix(4,4);
+Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);
+
+/*-------------------------------------------------------------------*/
+
+/*プロトタイプ宣言*/
+void toString_V(Vector& v); // ベクトルデバッグ用
+void gps_rx();
+void gps_convertion();
+/*--------------*/
+
+void toString_V(Vector& v)
+{
+
+ for(int i=0; i<v.GetDim(); i++) {
+ pc.printf("%.6f\t", v.GetComp(i+1));
+ }
+ pc.printf("\r\n");
+
+}
+
+void scan_update(int box_sensor[6],int16_t m[3]){
+
+ int a[3];//加速度の値
+ int g[3];//角速度の値
+
+ int16_t raw_compass[3];//地磁気センサの値
+
+ mpu.setAcceleroRange(0);//acceleration range is +-4G
+ mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
+
+ mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
+ mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s]
+ compass.getXYZ(raw_compass);//地磁気の値を格納する配列
+
+ box_sensor[0]=-g[0];
+ box_sensor[1]=g[1];
+ box_sensor[2]=-g[2];
+
+ box_sensor[3]=a[0];
+ box_sensor[4]=-a[1];
+ box_sensor[5]=a[2];
+
+ m[0]=(raw_compass[0]);//x
+ m[1]=(raw_compass[2]);//y
+ m[2]=(raw_compass[1]);//z
+
+}
+
+void gps_rx(){
+
+ if((gps.getc()=='$')&&(gps_flag==0)){
+ for(int i=0;i<9;i++){
+ gps_data[i]=gps.getc();
+ //pc.putc(gps_data[i]);
+ }
+
+ gps_data[9]='\0';
+
+ for(int i=0;i<9;i++){
+ gps_data_2[i]=gps.getc();
+ //pc.putc(gps_data[i]);
+ }
+
+ gps_data_2[9]='\0';
+
+ }//if(twe.getc()==':')
+
+ //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
+
+ gps_flag=1;
+
+ }//gps_rx ends
+
+void gps_convertion(){
+ while(1){
+ if(gps_flag==1){
+ tokei_int_receive=atoi(gps_data);
+ hokui_int_receive=atoi(gps_data_2);
+
+ //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
+
+ tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
+ hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;
+
+ gps_flag_conversion=1;
+
+ }else{gps_flag_conversion=0;}
+ }//while ends
+
+ }//ends
+
+void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
+
+ double Roll_before = double(y_acc)/double(z_acc);
+ double Roll = atan(Roll_before);
+
+ double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
+ double Pitch = -atan(Pitch_before);
+
+ //Yaw=0.0;
+
+ /*Quaternionの要素へオイラー角を変換する*/
+ qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
+ qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
+ qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0);
+ qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0);
+
+
+ }
+
+void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
+
+ double Roll_before = double(y_acc)/double(z_acc);
+ double Roll = atan(Roll_before);
+
+ double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
+ double Pitch = -atan(Pitch_before);
+
+
+ qua[0]=Pitch;
+ qua[1]=Roll;
+
+ }
+
+void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
+
+ //y_acc*=-1.0;
+
+ n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
+ n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
+ n[2]=0.0;
+
+ n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
+
+ }
+
+double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
+
+ Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系
+ Matrix Rotation(3, 3); //検算の行列
+
+ double Correct_vector_n[4];//回転行列の成分
+ Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
+
+ double n_x=Correct_vector_n[0];
+ double n_y=Correct_vector_n[1];
+ double n_z=Correct_vector_n[2];
+ double theta=-Correct_vector_n[3];
+
+ float Rotate_element[9] = {
+ n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta),
+ n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta),
+ n_y*sin(theta) , n_x*sin(theta) , cos(theta)
+ };
+
+ Rotation.SetComps(Rotate_element);
+ //x y z
+ float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X)
+ , float(magnet[0]+MAG_FIX_Y)
+ , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
+
+ Mag_from_sensor.SetComps(Geomagnetism);
+
+ Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
+
+ float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
+ float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
+
+ double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
+
+ /*
+ if(Azi < 0.0) // fix sign
+ Azi += PI2;
+
+ if(Azi > PI2) // fix overflow
+ Azi -= PI2;
+ */
+
+ return Azi;
+
+ }
+
+//void led2_thread(void const *argument) {
+void imu_thread() {
+ while (true) {
+ }//while ends
+}
+
// Entry point for the example
int main()
{
- printf("--- Mbed OS filesystem example ---\n");
+ gps.baud(115200);//GT-720Fボーレート
+ pc.baud(115200);
+ //imu_thread
+
+ compass.init();//hmc5883の起動
+
+ Thread thread1 (gps_convertion);
+ gps.attach(gps_rx, Serial::RxIrq);
+
+ float Time_old=0.0;//時間初期化
+ passed_time.start();//タイマー開始
+
+ pc.printf("--- Mbed OS filesystem example ---\n");
// Try to mount the filesystem
- printf("Mounting the filesystem... ");
+ pc.printf("Mounting the filesystem... ");
fflush(stdout);
int err = fileSystem.mount(&blockDevice);
- printf("%s\n", (err ? "Fail :(" : "OK"));
+ pc.printf("%s\n", (err ? "Fail :(" : "OK"));
if (err) {
// Reformat if we can't mount the filesystem
// this should only happen on the first boot
- printf("No filesystem found, formatting... ");
+ pc.printf("No filesystem found, formatting... ");
fflush(stdout);
err = fileSystem.reformat(&blockDevice);
- printf("%s\n", (err ? "Fail :(" : "OK"));
+ pc.printf("%s\n", (err ? "Fail :(" : "OK"));
if (err) {
error("error: %s (%d)\n", strerror(-err), err);
}
}
// Open the numbers file
- printf("Opening \"/fs/numbers.txt\"... ");
+ pc.printf("Opening \"/fs/numbers.txt\"... ");
fflush(stdout);
FILE* f = fopen("/fs/numbers.txt", "r+");
- printf("%s\n", (!f ? "Fail :(" : "OK"));
+ pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
+
if (!f) {
// Create the numbers file if it doesn't exist
- printf("No file found, creating a new file... ");
+ pc.printf("No file found, creating a new file... ");
fflush(stdout);
f = fopen("/fs/numbers.txt", "w+");
- printf("%s\n", (!f ? "Fail :(" : "OK"));
- if (!f) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
-
- for (int i = 0; i < 10; i++) {
- printf("\rWriting numbers (%d/%d)... ", i, 10);
- fflush(stdout);
- err = fprintf(f, " %d\n", i);
- if (err < 0) {
- printf("Fail :(\n");
- error("error: %s (%d)\n", strerror(errno), -errno);
+ pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
}
- }
-
- printf("\rWriting numbers (%d/%d)... OK\n", 10, 10);
-
- printf("Seeking file... ");
- fflush(stdout);
- err = fseek(f, 0, SEEK_SET);
- printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
- if (err < 0) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
- }
-
- // Go through and increment the numbers
- for (int i = 0; i < 10; i++) {
- printf("\rIncrementing numbers (%d/%d)... ", i, 10);
- fflush(stdout);
-
- // Get current stream position
- long pos = ftell(f);
-
- // Parse out the number and increment
- int32_t number;
- fscanf(f, "%ld", &number);
- number += 1;
-
- // Seek to beginning of number
- fseek(f, pos, SEEK_SET);
-
- // Store number
- fprintf(f, " %ld\n", number);
-
- // Flush between write and read on same file
- fflush(f);
- }
-
- printf("\rIncrementing numbers (%d/%d)... OK\n", 10, 10);
-
- // Close the file which also flushes any cached writes
- printf("Closing \"/fs/numbers.txt\"... ");
- fflush(stdout);
- err = fclose(f);
- printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
- if (err < 0) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
-
- // Display the root directory
- printf("Opening the root directory... ");
- fflush(stdout);
-
- DIR* d = opendir("/fs/");
- printf("%s\n", (!d ? "Fail :(" : "OK"));
- if (!d) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
+
+ /*初期姿勢のQuaternionの設定*/
+ scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
+ Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
+
+ Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
+
+ float Initialize_atitude[4];
+ Initialize_atitude[0]=Quaternion_from_acc[0];
+ Initialize_atitude[1]=Quaternion_from_acc[1];
+ Initialize_atitude[2]=Quaternion_from_acc[2];
+ Initialize_atitude[3]=Quaternion_from_acc[3];
+
+ Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
+
+
+ while(1){
+
+ /*gpsから来た文字列の処理
+ if((gps_flag==1)&&(gps_flag_conversion==1)){
+
+ err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
+
+ gps_flag=0;
+ gps_flag_conversion=0;
+
+ }else{}
+ */
+
+ scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
+ float Time_new=float(passed_time.read());//時間の取得
+
+ //地磁気の補正用に使う。
+ //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
+
+ //ジャイロの補正用に使う
+ //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
+
+ /*----------------ジャイロセンサからQuaternionを求める----------------*/
+ float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7);
+ float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7);
+ float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7);
+
+ float omega[16] = {
+
+ 0.0 , -omega_x , -omega_y, -omega_z,
+ omega_x ,0.0 ,omega_z ,-omega_y ,
+ omega_y , -omega_z , 0.0 , omega_x ,
+ omega_z , omega_y ,-omega_x , 0.0
+
+ };
- printf("root directory:\n");
- while (true) {
- struct dirent* e = readdir(d);
- if (!e) {
- break;
- }
-
- printf(" %s\n", e->d_name);
- }
-
- printf("Closing the root directory... ");
- fflush(stdout);
- err = closedir(d);
- printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
- if (err < 0) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
-
- // Display the numbers file
- printf("Opening \"/fs/numbers.txt\"... ");
- fflush(stdout);
- f = fopen("/fs/numbers.txt", "r");
- printf("%s\n", (!f ? "Fail :(" : "OK"));
- if (!f) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
-
- printf("numbers:\n");
- while (!feof(f)) {
- int c = fgetc(f);
- printf("%c", c);
- }
+ Omega_matrix.SetComps(omega);
+
+ float half[16] ={
+ (Time_new-Time_old)*0.5,0.0,0.0,0.0,
+ 0.0,(Time_new-Time_old)*0.5,0.0,0.0,
+ 0.0,0.0,(Time_new-Time_old)*0.5,0.0,
+ 0.0,0.0,0.0,(Time_new-Time_old)*0.5
+ };
+
+
+ Half_matrix.SetComps(half);
+ //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
+ Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
+
+ Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
+
+
+ Time_old=Time_new;//時間の更新
+
+ /*
+ pc.printf("%f,%f,%f,%f\r\n"
+ ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
+ ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
+ */
+
+ /*----------------------------------------------------------------*/
+
+ /*
+ err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
+ ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
+ ,sensor_array[6],sensor_array[7],sensor_array[8]);
+ */
+
+ /*--------------------加速センサで地磁気の値を補正する --------------------*/
+
+ if(9.8065*1.1 >
+ sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
+ / 16384.0 * 9.81)
+ {
+
+ Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
+
+ pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);
+
+ double pitch_and_roll[2];
+ Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
+
+ //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG);
+
+ Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
+
+ /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
+ float Quaternion_from_acc_fixed[4];
+ Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
+ Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
+ Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
+ Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
+
+ Vector Qua_acc(4);
+
+ Qua_acc.SetComps(Quaternion_from_acc_fixed);
+ /*---------------------------------------------*/
+
+ /*---------------------相補フィルタのゲインに用いる---------------------*/
+ float comp_1[16]={0.95,0.0,0.0,0.0,
+ 0.0,0.95,0.0,0.0,
+ 0.0,0.0,0.95,0.0,
+ 0.0,0.0,0.0,0.95
+ };
+ /*--------------------------------------------------------------------*/
+ /*---------------------quaternionの時間微分に用いる---------------------*/
+ float comp_2[16]={0.05,0.0,0.0,0.0,
+ 0.0,0.05,0.0,0.0,
+ 0.0,0.0,0.05,0.0,
+ 0.0,0.0,0.0,0.05
+ };
+
+ Complement_matrix_1.SetComps(comp_1);
+ Complement_matrix_2.SetComps(comp_2);
+
+ /*--------------------------------------------------------------------*/
+
+
+ /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
+ /*
+ Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
+ +Complement_matrix_2*Qua_acc;
+ */
+ /*----------------------------------------------------------------------------*/
+
+
+ /*
+ pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
+ ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
+ ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
+ ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
+ ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
+ */
+
+ }else{}
+
+ /*
+ pc.printf("%f,%f,%f,%f,%f\r\n"
+ ,Time_new
+ ,Quaternion_atitude_from_omega.GetComp(1)
+ ,Quaternion_atitude_from_omega.GetComp(2)
+ ,Quaternion_atitude_from_omega.GetComp(3)
+ ,Quaternion_atitude_from_omega.GetComp(4));
+ */
+ wait(0.001);
+
+ /*----------------------------------------------------------------*/
- printf("\rClosing \"/fs/numbers.txt\"... ");
- fflush(stdout);
- err = fclose(f);
- printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
- if (err < 0) {
- error("error: %s (%d)\n", strerror(errno), -errno);
- }
-
- // Tidy up
- printf("Unmounting... ");
- fflush(stdout);
- err = fileSystem.unmount();
- printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
- if (err < 0) {
- error("error: %s (%d)\n", strerror(-err), err);
- }
-
- printf("Initializing the block device... ");
- fflush(stdout);
-
- err = blockDevice.init();
- printf("%s\n", (err ? "Fail :(" : "OK"));
- if (err) {
- error("error: %s (%d)\n", strerror(-err), err);
- }
-
- printf("Erasing the block device... ");
- fflush(stdout);
- err = blockDevice.erase(0, blockDevice.size());
- printf("%s\n", (err ? "Fail :(" : "OK"));
- if (err) {
- error("error: %s (%d)\n", strerror(-err), err);
- }
-
- printf("Deinitializing the block device... ");
- fflush(stdout);
- err = blockDevice.deinit();
- printf("%s\n", (err ? "Fail :(" : "OK"));
- if (err) {
- error("error: %s (%d)\n", strerror(-err), err);
- }
-
- printf("\r\n");
-
- printf("Mbed OS filesystem example done!\n");
-}
+ if(switch_off==1){
+ // Close the file which also flushes any cached writes
+ pc.printf("Closing \"/fs/numbers.txt\"... ");
+ err = fclose(f);
+ break;
+ }
+
+ }//while(1) ends
+}//main ends