MWC飛控增加聲納定高的方法

MWC飛控增加聲納定高的方法

2015.12.17 更新：經過2個周末的上機測試，該算法效果很好，在低空超聲鎖高之后離地高度非常穩定，現在已經成功應用在低空航拍上了。

現狀

MWC開源飛控已經很有點年頭了，現在好多新的穿越機改用CC3D、航拍機改用APM，商業化的飛控也很多了。但是MWC作為基於Arduino的開源飛控，可以說非常成熟而且代碼簡單易懂，便宜，效果也不錯，所以我的四軸平台依然采用了MWC飛控。MWC2.4相對前代的改進，主要是加入了對串口GPS的支持，修復了聲納崩潰問題，以及改進了計算實時性。但是聲納定高依然沒有得到官方支持（包括I2C聲納）。新加入的GPS支持，由於占用內存過高，實際上在Mega之外的Arduino上無法使用。一個沒有GPS定點和聲納定高的飛控，就相當於一個簡單的增穩飛控而已，沒有一定的技術的話生存能力不是太高。個人覺得MWC有點走偏了。

所以我的思路是在MWC2.4的基礎上進一步實現聲納定高和GPS定點，至於GPS RTB、WP等功能等生存能力提高之后再考慮。聲納定高和GPS定點應該都不會對內存造成過大的壓力。這次首先介紹聲納定高的方法。

傳統的MWC為了支持GPS和聲納，一般是使用I2C_GPS_Nav這個項目，外置一片Arduino作為I2C GPS和聲納板。另外網上有些高手自行給MWC打了補丁，可以在低空用聲納數據覆蓋氣壓計數據，從而利用氣壓定高代碼。

思路

我的聲納定高基本思路也是利用氣壓定高代碼。

但是氣壓計的高度讀數與飛機本身姿態無關，而超聲波必須考慮機身傾斜程度，將測得的距離向Z軸投影，才是真實的高度。

網上對於MWC Baro模式的描述往往就是一句氣壓定高，具體的使用方法並沒有講得很清楚。閱讀代碼發現，Baro模式的具體行為是，油門不變，MWC會維持在進入Baro時的高度上；以進入Baro模式時為基准，增減油門飛機會升降，其速率為油門每增減100點，速度相應的增減30cm/s左右；油門回到基准則維持當前高度。

用Baro模式輔助降落應該是比較有用的，飛機降到聲納有效范圍之后，開啟Baro模式，稍微收油，飛機緩緩降落。

實現

首先是需要把I2C_GPS_Nav中對於Pingpong型聲納的代碼移植到MWC2.4的Sensor.cpp中。

我的MWC上A1和A2是空出來的，所以就用這兩個作為聲納的信號引腳。

// ************************************************************************************************************
// Pingpong Sonar
// ************************************************************************************************************
// 2015.11.29 by XD : ported from I2C_GPS_NAV_v2_2
#if defined(PINGPONG_SONAR)
volatile static uint32_t Sonar_starTime = 0;
volatile static uint32_t Sonar_echoTime = 0;
volatile static uint16_t Sonar_waiting_echo = 0;

void Sonar_init()
{
  // Pin change interrupt control register - enables interrupt vectors
  PCICR  |= (1<<PCIE1); // Port C
 
  // Pin change mask registers decide which pins are enabled as triggers
  PCMSK1 |= (1<<PCINT10); // echo, arduino pin A2, PC2
  
  DDRC |= 0x02; // trigger, arduino pin A1, triggerpin PC1 as output
  Sonar_update();
}

ISR(PCINT1_vect) {
    //uint8_t pin = PINC;
    if (PINC & 1<<PCINT10) {     //indicates if the bit 0 of the arduino port [B0-B7] is at a high state
      Sonar_starTime = micros();
    }
    else {
      Sonar_echoTime = micros() - Sonar_starTime; // Echo time in microseconds
     
      if (Sonar_echoTime <= 700*58) {     // valid distance
        sonarAlt = Sonar_echoTime / 58;
      }
      else
      {
        // No valid data
        sonarAlt = -1;
      }
      Sonar_waiting_echo = 0;
    }
}

void Sonar_update()
{
  if (Sonar_waiting_echo == 0)
  {
    // Send 2ms LOW pulse to ensure we get a nice clean pulse
    // PORTC &= ~(0x08);//PC3 low    
    PORTC &= ~(0x02);//PC1 low    
    delayMicroseconds(2);
   
    // send 10 microsecond pulse
    // PORTC |= (0x08);//PC3 high 
    PORTC |= (0x02);//PC1 high 
    // wait 10 microseconds before turning off
    delayMicroseconds(10);
    // stop sending the pulse
    // PORTC &= ~(0x08);//PC3 low
    PORTC &= ~(0x02);//PC1 low
    Sonar_waiting_echo = 1;
  }
}
#endif // #if defined(PINGPONG_SONAR)

我用的聲納就是X寶上買的10多塊號稱比較准的超聲波模塊，拔掉背后的跳線帽就是Pingpong模式（不然是串口模式），這種模塊還可以輸出溫度值用於距離補償，但是讀取溫度需要額外占用10多us，而且絕對距離在飛控上意義不大，所以暫時不考慮。這種Pingpong聲納在讀取距離的時候會占用10多us，相當於100多個時鍾周期，軟件開銷還是有點的。不過好在便宜，I2C的聲納要100多軟妹幣，這個只要10多塊。

官方I2C聲納的代碼最后，如果所有的I2C聲納都沒有定義，則會把Sonar_init和Sonar_update定義為空函數。這里（2.4版本Sensor.cpp:1576）需要進行一點修改，避免重復定義Sonar_init和Sonar_update：

#if !defined(PINGPONG_SONAR) // 2015.11.29 by XD : check for pingpong
inline void Sonar_init() {}
void Sonar_update() {}
#endif

在Config.h中開啟Pingpong聲納：

#define PINGPONG_SONAR 

要注意在def.h中添加對Pingpong聲納的支持：

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR) || defined(PINGPONG_SONAR) // 2015.11.29 by XD : ported from I2C_GPS_NAV_v2_2
  #define SONAR 1
#else
  #define SONAR 0
#endif

編譯通過並且正確連接聲納（Trigger接A1，Echo接A2）的話，可以看到MWC GUI上Sonar的標簽變綠了。

但是現在聲納數據還沒有被使用，需要進一步添加代碼使之與氣壓計數據結合。修改IMU.cpp中getEstimatedAltitude()函數，在BaroEstAlt LPF之后進行融合：

  BaroEstAlt = (BaroEstAlt * 6 + BaroAlt ) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

  // 2015.11.29 by XD, if sonar reads less than 4.2m (sensor limit 4.5m - safety margin 0.3m), use sonar
  // this maybe unsafe...
  if ((sonarAlt > 0 && sonarAlt < 420) || 
    ((att.angle[ROLL] > -90 && att.angle[ROLL] < 90) && (att.angle[PITCH] > -90 && att.angle[PITCH] < 90)))
  {
    // actual alt = sonarAlt * cos(att.angle[ROLL]) * cos(att.angle[PITCH])
    int32_t actualAlt = abs((int32_t)sonarAlt * (_cos10(att.angle[ROLL]) * _cos10(att.angle[PITCH]) >> 8));
    alt.EstAlt = actualAlt >> 12;
  }
  else
    alt.EstAlt = BaroEstAlt;

上面這段代碼中，聲納的測量值乘以Roll和Pitch的余弦就是向Z軸的投影，最終高度結果是以cm為單位。_cos10()是用於計算余弦的函數。

其實這個算法個人感覺有可能不安全，當高度超過聲納閾值的時候會突然切換到氣壓計高度，中間的誤差可能導致飛機猛烈晃動。可以考慮設置一個過渡區，根據飛機的高度對聲納/氣壓數據進行權重加和，權重根據高度自動調節。

Arduino庫中的cos函數開銷太大，經過測試，使用原裝cos函數，MWC較難在2.8ms內跑完一個周期（計算周期可以在GUI的Cycle Time查看，2.4版本大部分時候都在2800us，略有跳動）。

所以需要自己編寫快速余弦函數：

// 2015.11.30 by XD, x is in 0.1 deg, returns cos * (1 << 10)
// when x approaches zero, cos(x) = 1 - x ^ 2 / 2, x is in radians
// https://en.wikipedia.org/wiki/Small-angle_approximation
// rad = deg / 180 * PI
int32_t _cos10(int16_t x)
{
  // x within [-1800, 1800]
  int32_t radTemp = (int32_t)x * 114; // rad = x * ((PI / 1800) << 16), rad within [-205200, 205200]
  int32_t rad = radTemp >> 6; // rad ^ 2 within [-657922500, 657922500]
  
  int32_t cos20 = ((uint32_t)1 << 20) - ((rad * rad) >> 1);
  int32_t result = cos20 >> 10;
  
  return result;
}

使用的算法和MWC IMU差不多，在小角度的條件下計算三角函數的級數展開式前兩項。對於cos來說：cos(x) = 1 - x ^ 2 / 2，x是弧度單位。

由於Arduino沒有浮點單元和硬件除法器，需要盡量避免這兩種運算，這里直接手工計算PI / 1800並擴大65536倍（1 << 16）。

在求平方的時候，為了保證計算不越界，需要預先進行移位操作。

計算完成后將結果右移，最終保持10位2進制有效數字，差不多相當於4位10進制有效數字。最終結果相當於余弦乘以1024。

在前面的數據融合代碼中，我們也可以看到移位操作，就是用來避免計算越界，以及把余弦多乘的1024除回來。

最后在四軸上實際測試的效果不錯，Cycle Time沒有明顯變化，算出來的高度還是很准的，准備周末去實地飛行了。