MWC飛控增長聲納定高的方法html
2015.12.17 更新:通過2個週末的上機測試,該算法效果很好,在低空超聲鎖高以後離地高度很是穩定,如今已經成功應用在低空航拍上了。算法
現狀安全
MWC開源飛控已經頗有點年頭了,如今好多新的穿越機改用CC3D、航拍機改用APM,商業化的飛控也不少了。可是MWC做爲基於Arduino的開源飛控,能夠說很是成熟並且代碼簡單易懂,便宜,效果也不錯,因此個人四軸平臺依然採用了MWC飛控。MWC2.4相對前代的改進,主要是加入了對串口GPS的支持,修復了聲納崩潰問題,以及改進了計算實時性。可是聲納定高依然沒有獲得官方支持(包括I2C聲納)。新加入的GPS支持,因爲佔用內存太高,實際上在Mega以外的Arduino上沒法使用。一個沒有GPS定點和聲納定高的飛控,就至關於一個簡單的增穩飛控而已,沒有必定的技術的話生存能力不是過高。我的以爲MWC有點走偏了。app
因此個人思路是在MWC2.4的基礎上進一步實現聲納定高和GPS定點,至於GPS RTB、WP等功能等生存能力提升以後再考慮。聲納定高和GPS定點應該都不會對內存形成過大的壓力。此次首先介紹聲納定高的方法。less
傳統的MWC爲了支持GPS和聲納,通常是使用I2C_GPS_Nav這個項目,外置一片Arduino做爲I2C GPS和聲納板。另外網上有些高手自行給MWC打了補丁,能夠在低空用聲納數據覆蓋氣壓計數據,從而利用氣壓定高代碼。ide
思路函數
個人聲納定高基本思路也是利用氣壓定高代碼。測試
可是氣壓計的高度讀數與飛機自己姿態無關,而超聲波必須考慮機身傾斜程度,將測得的距離向Z軸投影,纔是真實的高度。ui
網上對於MWC Baro模式的描述每每就是一句氣壓定高,具體的使用方法並無講得很清楚。閱讀代碼發現,Baro模式的具體行爲是,油門不變,MWC會維持在進入Baro時的高度上;以進入Baro模式時爲基準,增減油門飛機會升降,其速率爲油門每增減100點,速度相應的增減30cm/s左右;油門回到基準則維持當前高度。this
用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沒有明顯變化,算出來的高度仍是很準的,準備週末去實地飛行了。