Revision 0:b021d725d528, committed 2017-04-05

Comitter:

makakensanba

Date:

Wed Apr 05 10:45:35 2017 +0000

Commit message:

?????????

Changed in this revision

--- /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

--- /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

--- /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

--- /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));
+}

--- /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