ムササビチームの電装です

Dependencies:   MPU6050 MS5607 SDFileSystem mbed

Files at this revision

API Documentation at this revision

Comitter:
makakensanba
Date:
Wed Apr 05 10:45:35 2017 +0000
Commit message:
?????????

Changed in this revision

MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
MS5607.lib Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b021d725d528 MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Wed Apr 05 10:45:35 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
diff -r 000000000000 -r b021d725d528 MS5607.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MS5607.lib	Wed Apr 05 10:45:35 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/yamaguch/code/MS5607/#5760862143d1
diff -r 000000000000 -r b021d725d528 SDFileSystem.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Wed Apr 05 10:45:35 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/mbed/code/SDFileSystem/#8db0d3b02cec
diff -r 000000000000 -r b021d725d528 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 05 10:45:35 2017 +0000
@@ -0,0 +1,653 @@
+/*
+2017年3月 伊豆大島共同打上実験
+団体名:CORE
+チーム名:ムササビ
+該当電装:ロケット搭載用
+
+使用部品
+・LPC1768(マイコン)
+・MPU6050(加速度・ジャイロセンサ)
+・MS5607(気圧・気温センサ)
+・MicroSDスロット
+・MG995(サーボモータ)×4
+
+使用ライブラリ
+・MPU6050.h
+   https://developer.mbed.org/teams/mbed/code/SDFileSystem/
+・MS5607.h
+   https://developer.mbed.org/users/yamaguch/code/MS5607/
+・SDFileSystem.h
+   https://developer.mbed.org/teams/mbed/code/SDFileSystem/
+*/
+#include "mbed.h"
+#include "math.h"
+#include "MS5607I2C.h"
+#include "MPU6050.h"
+#include "SDFileSystem.h"
+
+#define ACC_LAUNCH      4.0f//FIXME:本番は4g
+#define TOP_DROP_AMOUNT 1.5f
+#define TIME_REACH_TOP  12.5f
+
+#define RATE_LOG  1
+#define RATE_OPEN 10
+/*サーボ動作*/
+#define LOCK      0
+#define UNLOCK    1
+/*モード定義*/
+#define STANDBY   0
+#define TEST      1
+#define FLIGHT    2
+/*開放フェーズ定義*/
+#define SETUP     0
+#define LAUNCH    1
+#define RISE      2
+#define DROP      3
+/**/
+/*DCモータ動作*/
+#define FORW      0
+#define STOP      1
+#define BACK      2
+#define P0        1013.25f//海面気圧[hPa]
+#define ACC       4096.0f//加速度オフセット値
+
+/*目標地点の座標*/
+#define L_HOKUI  34.73416 //ムササビチーム目標地点
+#define L_TOUKEI 139.4227
+#define ECC2     0.00669437999019758
+#define A_RADIUS 6378137.000          //長半径(赤道半径)[m]
+#define B_RADIUS 6356752.314245     //短半径(極半径)(WGS84)->http://yamadarake.jp/trdi/report000001.html
+#define M_PI     3.1415926535897932384626433832795
+#define M_PI_2   1.57079632679489661923
+#define epsilon 1.0E-6
+
+/*ピン指定*/
+MS5607I2C     ms5607(p9, p10, false);
+MPU6050       mpu(p9,p10);
+BusOut        myled(LED1,LED2,LED3,LED4);
+SDFileSystem  sd(p5, p6, p7, p8, "sd");
+Serial        device(USBTX, USBRX);
+DigitalIn     flightpin(p19);
+DigitalOut    RED(p30);
+Serial        twe(p13,p14);
+Serial        gps(p28,p27);       // tx, rx
+PwmOut        Door_UP(p21);
+PwmOut        Door_BOTTOM(p22);
+PwmOut        DC(p23);
+
+
+/*タイマー類*/
+Timer  timer;
+Ticker loop_log;
+Ticker loop_open;
+/*ログカウンタ*/
+bool   row = 0;
+int8_t col = 0;
+/*ログ格納用*/
+float alt_drop;
+FILE *fp;
+/*フェーズ変数*/
+int8_t Phase = SETUP;
+int8_t Mode  = STANDBY;
+char flight[5] = {};
+int i_f =0;
+/*判定用*/
+float  alt_buff[RATE_OPEN];
+static float  alt_max,alt_launch;
+float  t_drop,t_top,t_launch;
+int8_t cnt_judge = 0;
+int8_t col_open  = 0;
+/*入力用*/
+int8_t input_buff[2] = {};
+int8_t input_cnt = 0;
+int u;
+
+/*GPS変数*/
+const float dt =0.06;
+int i,mode,Step;
+int j = 0;
+char gps_data[256];
+char st,ns,ew;
+float w_time,hokui,tokei;
+float vel_norm,vel_head;
+
+float g_hokui,g_tokei;
+float d_hokui,m_hokui,d_tokei,m_tokei;
+float def;
+unsigned char c;
+float dy, dx;
+float yAve,n,m,ecc,w;
+float xDist, yDist, dist;
+float x_tgt[3]= {166.7,-199.7,0.0};
+
+/*関数*/
+void  _Open();
+//void  _Log();
+void  _GPS();
+void  _Motor(int8_t door_num, int8_t motion);
+void  _Input();
+float _Measure_Alt(float press, float temp);
+float _Median(float data[], int num);
+int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max);
+int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt);
+void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]);
+void periodize(float &x, float max, float min);
+float hdg_conv(float hdgN0deg);
+void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]);
+void segmentBisector(float a[], float v1[], float v2[]);
+int intersection(float r[], float l[], float m[]);
+float dot2(float a[], float b[]);
+float norm2(float v[]);
+
+/*---------------------------------------------------------------------*/
+int main() {
+   device.baud(115200);
+   gps.baud(115200);
+   twe.baud(115200);
+   mpu.setAcceleroRange(2);
+   mpu.setGyroRange(2);
+   timer.start();
+   Door_UP.period_ms(20);
+   Door_BOTTOM.period_ms(20);
+   DC.period_ms(20);
+       ms5607.printCoefficients();
+
+   // _Motor(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定?
+   // _Motor(2,UNLOCK);//
+   fp = fopen("/sd/log.txt", "a");
+   device.attach(&_Input,Serial::RxIrq);
+   loop_open.attach(&_Open,1.0/RATE_OPEN);
+   while(1);
+}
+/*開放用関数 RATE_OPEN[Hz]で判定を行う*/
+void _Open(){
+   myled = 1 << (Phase-1);
+   switch (Phase) {
+      case SETUP://セットアップモード(発射判定不可)
+                  break;
+      case LAUNCH://点火モード(発射判定可)
+            device.printf("Phase:LAUNCH\r\n");
+            
+            float acc_buffx2 = (float)mpu.getAcceleroRawX()/(ACC*0.981);
+            float acc_buffy2 = (float)mpu.getAcceleroRawY()/(ACC*0.981);
+            float acc_buffz2 = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
+            
+            float x_acc=acc_buffx2*acc_buffx2;
+            float y_acc=acc_buffy2*acc_buffy2;
+            float z_acc=acc_buffz2*acc_buffz2;
+
+            float acc_sum = (x_acc+y_acc+z_acc)*1.0;
+                  alt_buff[col_open] = ms5607.getAltitude();
+                  if(acc_sum>ACC_LAUNCH||flightpin==1){
+                     if(cnt_judge++==3){
+                        Phase = RISE;
+                        alt_launch = _Median(alt_buff, RATE_OPEN);
+                        cnt_judge = 0;
+                     }
+                     device.printf("launch:%f",alt_launch);
+                     t_launch = timer.read();
+                     alt_max = alt_launch;
+                  }else{
+                     if(timer.read()>t_launch+1.0) cnt_judge = 0;
+                  }
+                  break;
+      case RISE://上昇中(パラシュート開放判定)
+                  device.printf("Phase:RISE\r\n");
+                  float alt_rising = ms5607.getAltitude();
+                  if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising;
+                  if(alt_rising<alt_max-TOP_DROP_AMOUNT){
+                     if(cnt_judge++==3){
+                        twe.printf("Phase:RISE.ALT Open.\r\n");
+                           gps.attach(&_GPS,Serial::RxIrq);
+
+                        _Motor(1,UNLOCK);
+                        Phase = DROP;
+                        cnt_judge = 0;
+                     }
+                     t_top = timer.read();
+                  }else{
+                     if(timer.read()>t_top+1.0) cnt_judge = 0;
+                  }
+                  if(timer.read()-t_launch>TIME_REACH_TOP){
+                     _Motor(1,UNLOCK);
+   gps.attach(&_GPS,Serial::RxIrq);
+
+                     Phase = DROP;
+                     cnt_judge = 0;
+                  }
+                  break;
+      case DROP://降下中(缶サット開放判定)
+
+            //device.printf("%d %c %f %f %f %f \n\r",u,st,vel_norm, vel_head, alt_drop, alt_max);
+            //device.printf("Phase:DROP. Input:%d /r/n",u);
+            
+            //_Measure alt 現在の高度 + dt->60ms
+
+                  break;
+   }
+   if(col_open++==RATE_OPEN) col_open = 0;
+}
+/*
+void _Log(){
+   if(t[row][col] = timer.read()>=30.0*60.0){
+      timer.reset();
+      t[row][col] = timer.read();
+   }
+   pressure[row][col]    = ms5607.getPressure()/100;
+   temperature[row][col] = ms5607.getTemperature();
+   fprintf(fp,"%d,%f,%f\r\n",t[row][col],pressure[row][col],temperature[row][col]);
+   if(col++==RATE_LOG){
+      col = 0;
+      row =! row;
+      fclose(fp);
+      fp  =  fopen("/sd/log.txt", "a");
+   }
+}
+*/
+    void _Motor(int8_t num, int8_t motion) {
+        if(num==1) { //扉
+            if(motion==UNLOCK) {
+                Door_UP.pulsewidth(0.0015);
+                Door_BOTTOM.pulsewidth(0.0023);
+                wait(1.0);
+                Door_UP.pulsewidth(0);
+                Door_BOTTOM.pulsewidth(0);
+            } else if(motion==LOCK) {
+                Door_UP.pulsewidth(0.0023);
+                Door_BOTTOM.pulsewidth(0.0015);
+                wait(1.0);
+                Door_UP.pulsewidth(0);
+                Door_BOTTOM.pulsewidth(0);
+            } else {
+                device.printf("error%f\r\n",motion);
+            }
+        } else if(num==2) { //DC
+            if(motion==FORW) {
+                DC.pulsewidth(0.001);
+                wait(0.2);
+            } else if(motion==STOP) {
+                DC.pulsewidth(0.0015);
+                wait(0.2);
+            } else if(motion==BACK) {
+                DC.pulsewidth(0.002);
+                wait(0.2);
+            } else {
+                device.printf("error%f\r\n",motion);
+            }
+        } else {
+            device.printf("Motor error:%d\r\n",num);
+        }
+    }
+void _GPS() {
+  static int cnt=0;
+  cnt++;
+  RED =1;
+  c = gps.getc();
+  if( c=='$' || i == 256){
+    mode = 0;
+    i = 0;
+    for(int j=0; j<256; j++){
+        gps_data[j]=NULL;
+    }
+  }
+  if(mode==0){
+    if((gps_data[i]=c) != '\r'){
+      i++;
+    }else{
+      gps_data[i]='\0';
+      
+      if( sscanf(gps_data, "$GNRMC,%f,%c,%f,%c,%f,%c,%f,%f",&w_time,&st,&hokui,&ns,&tokei,&ew,&vel_norm,&vel_head) >= 1){
+          //logitude
+
+          d_tokei= int(tokei/100);
+          m_tokei= (tokei-d_tokei*100)/60;
+          g_tokei= d_tokei+m_tokei;
+          //Latitude
+          d_hokui=int(hokui/100);
+          m_hokui=(hokui-d_hokui*100)/60;
+          g_hokui=d_hokui+m_hokui;
+          
+          dy = (L_HOKUI-g_hokui)/180*M_PI;
+          dx = (L_TOUKEI-g_tokei)/180*M_PI;
+          yAve = (g_hokui+L_HOKUI)/2/180*M_PI;
+          w = sqrt(1-ECC2*sin(yAve)*sin(yAve));
+          m = A_RADIUS*(1-ECC2)/(w*w*w);
+          n = A_RADIUS/w;
+          dist = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))/1);
+          xDist = dx*n*cos(yAve);
+          yDist = dy*m;
+          
+                float alt = ms5607.getAltitude();
+                //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head);
+                device.printf("Radio \r\n");
+
+/*
+          static int k=0;
+          if (k++ % 6 == 0){
+                float alt = ms5607.getAltitude();
+                //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head);
+                device.printf("Radio \r\n");
+
+                twe.printf("%c,%f %f %f %f %f %d \r\n",Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u);
+                fprintf(fp,"%c,%f %f %f %f %f %d \r\n" ,Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u);
+                
+                if(k % 30 == 0){
+                    k = 0;
+                    row =! row;
+                    fclose(fp);
+                    fp  =  fopen("/sd/log.txt", "a");
+                }
+          }                  
+*/          static float alt_drop2=ms5607.getAltitude();
+            alt_drop2=ms5607.getAltitude();
+          alt_drop=alt_drop2-alt_launch;
+            
+           float x_s[] = { xDist, yDist };
+           device.printf("x_s %f %f drp:%f max:%f \r\n",x_s[0],x_s[1],alt_drop,alt_max - alt_launch);
+        u = set_input(st == 'A', x_s, vel_norm, vel_head, alt_drop, alt_max - alt_launch);
+            device.printf("%d",u);
+            twe.printf("%d,%f %f %f %f %f %d \r\n",cnt,hokui,tokei,vel_norm,vel_head,alt_drop,alt_max - alt_launch,u);
+
+            static int ui = 0;
+            const int ui_max = 30, ui_min = 10;   // ui_min = 0 ?
+            if (ui < ui_min) u = 1;
+            else if (ui > ui_max) u = -1;
+            ui += u;
+
+            if(u == -1) _Motor(2,FORW);//右旋回
+            else if(u == 0) _Motor(2,STOP);
+            else if(u == 1) _Motor(2,BACK);//左旋回
+           
+   
+          sprintf(gps_data, "");
+      }//if
+    }
+  }
+}
+
+void _Input(){
+   input_buff[input_cnt] = device.getc();
+   device.printf("\r\n");
+   switch (Mode) {
+      case STANDBY:
+                  if(input_cnt==0){
+                     if(input_buff[0]=='S'){
+                        device.printf("U >> UNLOCK\r\n");
+                        device.printf("L >> LOCK\r\n");
+                     }else if(input_buff[0]=='M'){
+                        device.printf("S >> STANDBY\r\n");
+                        device.printf("F >> FLIGHT\r\n");
+                     }else{
+                        device.printf("This command is not found >> %c\r\n",input_buff[0]);
+                        device.printf(">>MAINMENU<<\r\n");
+                        device.printf("S >> Servo Operation\r\n");
+                        device.printf("M >> Mode Change\r\n");
+                        device.printf("-->>");
+                        return;
+                     }
+                  }else if(input_cnt==1){
+                     if(input_buff[0]=='S'){
+                          if(input_buff[1]=='U')_Motor(1,UNLOCK);
+                          else if(input_buff[1]=='L')_Motor(1,LOCK);
+                          else{
+                            device.printf("This command is not found >> %c\r\n",input_buff[1]);
+                            device.printf("U >> UNLOCK\r\n");
+                            device.printf("L >> LOCK\r\n");
+                            device.printf("-->>");
+                            return;
+                          }
+                     }else if(input_buff[0]=='M'){
+                        if(input_buff[1]=='S'){
+                           Mode = STANDBY;
+                        }else if(input_buff[1]=='F'){
+                           Mode = FLIGHT;
+                           Phase = LAUNCH;
+
+                           device.printf("FLIGHT-Mode ON!\r\n");
+                           device.printf("***alert***\r\n");
+                           device.printf("You will be able to reset only!\r\n");
+                           return;
+                        }else{
+                           device.printf("This command is not found >> %c\r\n",input_buff[1]);
+                           device.printf("S >> STANDBY\r\n");
+                           device.printf("F >> FLIGHT\r\n");
+                           device.printf("-->>");
+                           return;
+                        }
+                     }
+                     input_cnt = 0;
+                     device.printf(">>MAINMENU<<\r\n");
+                     device.printf("S >> Servo Operation\r\n");
+                     device.printf("M >> Mode Change\r\n");
+                     device.printf("-->>");
+                     return;
+                  }
+                  device.printf("-->>");
+                  input_cnt++;
+                  break;
+      case FLIGHT://reset only
+                  break;
+   }
+}
+
+/*その他雑関数*/
+float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){
+   return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+}
+float _Median(float data[], int num){
+   float median;
+   float *sort = (float *)malloc(sizeof(float)*num);
+   for(int i=0; i<num; i++) sort[i] = data[i];
+   for(int i=0; i<num; i++){
+      for(int j=0; j<num-i-1; j++){
+         if(sort[j]>sort[j+1]){
+            float buff = sort[j+1];
+            sort[j+1] = sort[j];
+            sort[j] = buff;
+         }
+      }
+   }
+   if(num%2!=0)median = sort[num/2];
+   else median = (sort[num/2-1]+sort[num/2])/2.0;
+   free(sort);
+   return median;
+}
+
+int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max)
+{
+    // グローバルにするかも?
+    const int dn = 40;                          // windstarのベクトルを取得するステップ間隔
+    const float vz_est = 4.62;                  // パラフォイルのsinkrate[m/s]
+    const float dist_tgt = 30.0;                // 目標旋回半径[m]
+    const float alt_unctrl = 50.0;              // 無制御で降下する高度[m]
+    
+    static int gps_step = 1;
+    float u = 0.0;
+    if (gps_good)
+    {
+        enum Phase
+        {
+            WIND_EST,
+            CTRL
+        };
+        static Phase phase = WIND_EST;
+
+        // sensor data
+        float dt_ = dt*gps_step;
+        gps_step = 1;
+
+        // state variable estimation
+        float hdg_s = hdg_conv(hdg_gps);
+        static float hdg0 = hdg_s;
+        float omg_calc = hdg_s - hdg0;
+        if (omg_calc >= M_PI) omg_calc -= 2 * M_PI;
+        else if (omg_calc <= -M_PI) omg_calc += 2 * M_PI;
+        omg_calc /= dt_;
+        hdg0 = hdg_s;
+
+        // static float alt0 = alt_s;
+        // float vz_calc = (alt_s - alt0) / dt_;
+        // alt0 = alt_s;
+
+        float vg_s[] = { vg_knot*0.514444444*cos(hdg_s), vg_knot*0.514444444*sin(hdg_s) };
+        // if want to use estimated position, uncomment below
+        // float x_est[2] = {};
+        // filter_x(dt_, 0.1, x_est, x_s, vg_s);
+        
+        // wind estimation sequence
+        static float vw_est[2] = {};
+        switch (phase)
+        {
+            case WIND_EST:
+            {
+                         const int dn2 = 2 * dn;
+                         static int n = 0;
+                         float vw_t[2] = {};
+                         static int cnt = 0;
+                         if (n >= dn2)
+                         {
+                             static int j = 0;
+                             static float vg_buf[dn2][2] = {};
+                             windstar(vw_t, vg_buf[n % dn2], vg_buf[(n + dn) % dn2], vg_s);
+                             vw_est[0] = (vw_est[0] * j + vw_t[0]) / (j + 1);
+                             vw_est[1] = (vw_est[1] * j + vw_t[1]) / (j + 1);
+                             j++;
+                             vg_buf[n % dn2][0] = vg_s[0];
+                             vg_buf[n % dn2][1] = vg_s[1];
+                         }
+                         n++;
+
+                         if (alt_s < alt_max - alt_unctrl) // 
+                            if (cnt++ >= 3) phase = CTRL;
+            }
+            break;
+            case CTRL:
+            {
+                     float t_flt_est = (alt_s - x_tgt[2]) / vz_est;
+                     if (abs(vz_est) <= epsilon) t_flt_est = 0.0;
+                     float windoffset[2] = { -vw_est[0] * t_flt_est, -vw_est[1] * t_flt_est };
+                     if (windoffset[1] > 200.0) windoffset[1] = 200.0;
+                     float x_virt[2] = { x_s[0] + windoffset[0], x_s[1] + windoffset[1] };
+                     // if want to use estimated position, comment out above and uncomment below
+                     // float x_virt[2] = { x_est[0] + windoffset[0], x_est[1] + windoffset[1] }; 
+                     float x_virt_rel[2] = { x_virt[0] - x_tgt[0], x_virt[1] - x_tgt[1] };
+                     u = control(x_virt_rel, vw_est, vg_s, hdg_s, omg_calc, dist_tgt);
+            }
+            break;
+        }
+
+    }
+    else
+        gps_step++;
+
+    return u;
+}
+
+int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt)
+{
+    const float threshold = 0.20;
+
+    float theta = atan2(r_rel[1], r_rel[0]);
+
+    float dist = norm2(r_rel);
+    float Vr = dot2(r_rel, v) / dist;
+    float Vtheta = (r_rel[0] * v[1] - r_rel[1] * v[0]) / dist;
+
+    float hdg_tgt = 0.0;
+    float alpha = atan(0.5*(dist - dist_tgt));
+    float limitalpha = M_PI_2 - asin(dist_tgt / dist);
+    if (alpha > limitalpha) alpha = limitalpha;
+    if (Vtheta > 0)
+        hdg_tgt = theta + M_PI_2 + alpha;
+    else
+        hdg_tgt = theta - M_PI_2 - alpha;
+    periodize(hdg_tgt, M_PI, -M_PI);
+
+    float dhdg = hdg - hdg_tgt;
+    periodize(dhdg, M_PI, -M_PI);
+
+    float domg = omg - Vtheta / dist;
+    float Kp = 0.5, Kd = 3.0;
+    float u = -Kp*dhdg - Kd*domg;
+
+    if (dist < 200.0)
+    {
+        if (Vtheta > 0) u += 0.05*Vr;
+        else u -= 0.05*Vr;
+    }
+    
+    if (u > threshold) return 1;
+    else if (u < -threshold) return -1;
+    else return 0;
+}
+
+void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[])
+{
+    static float x0[] = { x_gps[0], x_gps[1], x_gps[2] };
+    static int called = 0;
+    if (!called)
+    {
+        for (int i = 0; i < 2; i++)
+            x_est[i] = x0[i];
+        called++;
+        return;
+    }
+    float xt[2];
+    for (int i = 0; i < 2; i++)
+    {
+        xt[i] = x0[i] + vg_gps[i] * dt;
+        x_est[i] = C*x_gps[i] + (1 - C)*xt[i];
+        x0[i] = x_est[i];
+    }
+}
+
+float hdg_conv(float hdgN0deg)
+{
+    hdgN0deg *= -M_PI / 180.0;
+    hdgN0deg += M_PI_2;
+    periodize(hdgN0deg, M_PI, -M_PI);
+    return hdgN0deg;
+}
+void periodize(float &x, float max, float min)
+{
+    float range = max - min;
+    while (x > max) x -= range;
+    while (x < min) x += range;
+}
+
+void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[])
+{
+    float l[3][3] = {};  // line coeffs l[0]*x + l[1]*y + l[2] = 0
+    segmentBisector(l[0], Vg0, Vg1);
+    segmentBisector(l[1], Vg1, Vg2);
+    segmentBisector(l[2], Vg2, Vg0);
+    float r[3][3] = {};  // intersection point
+    intersection(r[0], l[0], l[1]);
+    intersection(r[1], l[1], l[2]);
+    intersection(r[2], l[2], l[0]);
+    Vw[0] = (r[0][0] + r[1][0] + r[2][0]) / 3;
+    Vw[1] = (r[0][1] + r[1][1] + r[2][1]) / 3;
+}
+void segmentBisector(float a[], float v1[], float v2[])
+{
+    a[0] = v2[0] - v1[0];
+    a[1] = v2[1] - v1[1];
+    a[2] = -0.5*(a[0] * (v1[0] + v2[0]) + a[1] * (v1[1] + v2[1]));
+}
+int intersection(float r[], float l[], float m[])
+{
+    float det = l[0] * m[1] - l[1] * m[0];
+    if (abs(det) < epsilon) return -1;
+    r[0] = (-m[1] * l[2] + l[1] * m[2]) / det;
+    r[1] = (m[0] * l[2] - l[0] * m[2]) / det;
+    return 0;
+}
+
+float dot2(float a[], float b[])
+{
+    return a[0] * b[0] + a[1] * b[1];
+}
+float norm2(float v[])
+{
+    return sqrt(dot2(v, v));
+}
diff -r 000000000000 -r b021d725d528 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 05 10:45:35 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb
\ No newline at end of file