The method of increasing sonar height by MWC flight control

The method of increasing sonar height by MWC flight control

Present situation

MWC Open source flight control has been a bit of a year, and now a lot of new cross-machine to cc3d, Aerial camera switch to APM, commercial flight control is also a lot. But MWC as an Arduino based open source flight control, can be said to be very mature and code easy to understand, cheap, the effect is good, so my four-axis platform is still using the MWC flight control. MWC2.4 relative to the previous generation of improvements, mainly joined the serial port of the support of GPS, fixed the problem of sonar crashes, as well as improved computing real-time. But the sonar is still not getting official support (including the I²c sonar). The newly added GPS support, due to high memory consumption, is actually not available on the Arduino outside of Mega. A non-GPS fixed-point and sonar high flight control, it is equivalent to a simple stability of flight control, without a certain technology, the survival ability is not too high. Personally feel that MWC is a little off.

So my idea is to further realize the sonar fixed height and GPS fixed point on the basis of MWC2.4, as for GPs RTB, WP and other functions such as the ability to improve after the consideration. Sonar fixed height and GPS fixed point should not cause too much pressure on memory. This time we first introduce the method of sonar height determination.

Traditional MWC in order to support GPs and sonar, is generally used i2c_gps_nav this project, an external piece of Arduino as the I²c GPS and sonar board. In addition, some experts on the Internet to the MWC to play a patch, you can use sonar data at low altitude to cover the barometer data, thereby using the pressure to set high code.

Ideas

The basic idea of my sonar is to use a high-pressure code.

But the altimeter's altitude readings have nothing to do with the plane's posture, and the ultrasound must take into account the tilt of the fuselage, projecting the measured distance to the z-axis, which is the true height.

The online description of the MWC Baro model is usually a high pressure, the specific use of the method did not speak very clearly. Read the code found that the specific behavior of the Baro mode is that the throttle is constant, MWC will maintain the height when entering the Baro, to enter the Baro mode as the benchmark, increase or decrease the throttle aircraft will rise and fall, the rate of the throttle every 100 points, the speed of the corresponding increase or decrease 30cm/s around The throttle back to the benchmark maintains the current height.

Using Baro mode to assist landing should be more useful, after the plane dropped to the effective range of sonar, open Baro mode, a little oil, the plane slowly landing.

Realize

The first is the need to transplant the code for the Pingpong sonar in the I2c_gps_nav into the Sensor.cpp of MWC2.4.

My MWC A1 and A2 are empty, so use these two signal pins as sonar.

// *********************************************************************************************************** *//pingpong Sonar// *********************************************************************************************************** *//2015.11.29 by xd:ported from I2c_gps_nav_v2_2#ifDefined (Pingpong_sonar)volatile Staticuint32_t sonar_startime =0;volatile Staticuint32_t sonar_echotime =0;volatile Staticuint16_t Sonar_waiting_echo =0;voidSonar_init () {//Pin Change Interrupt control register-enables Interrupt vectorsPCICR |= (1&LT;&LT;PCIE1);//Port C//Pin Change mask registers decide which pins is enabled as triggersPCMSK1 |= (1&LT;&LT;PCINT10);//Echo, Arduino pin A2, PC2DDRC|=0x02;//trigger, Arduino pin A1, triggerpin PC1 as outputsonar_update ();} ISR (pcint1_vect) {//uint8_t pin = pinc;    if(Pinc &1&LT;&LT;PCINT10) {//indicates if the bit 0 of the Arduino port [B0-b7] is at a high stateSonar_startime =Micros (); }    Else{sonar_echotime= Micros ()-sonar_startime;//Echo time in microseconds           if(Sonar_echotime <= the* -) {//Valid distanceSonaralt = sonar_echotime/ -; }      Else      {        //No Valid dataSonaralt =-1; } Sonar_waiting_echo=0; }}voidsonar_update () {if(Sonar_waiting_echo = =0)  {    //Send 2ms Low pulse to ensure we get a nice clean pulse//PORTC &= ~ (0x08);//PC3 LowPORTC &= ~ (0x02);//PC1 LowDelaymicroseconds (2); //send ten microsecond pulse//PORTC |= (0x08);//PC3 HighPORTC |= (0x02);//PC1 High//wait ten microseconds before turning offDelaymicroseconds (Ten); //stop sending the pulse//PORTC &= ~ (0x08);//PC3 LowPORTC &= ~ (0x02);//PC1 LowSonar_waiting_echo =1; }}#endif //#if defined (pingpong_sonar)

I use the sonar is X PO bought more than 10 pieces of the more quasi-ultrasonic module, unplug behind the jumper cap is Pingpong mode (or serial mode), this module can also output temperature value for distance compensation, but the reading temperature needs to occupy more than 10 us, and absolute distance in the flight control is not very meaningful, So for the time being not considered. This pingpong sonar consumes more than 10 us when reading distances, which is equivalent to more than 100 clock cycles, and software overhead is somewhat. But fortunately cheap, I²c's sonar to more than 100 soft sister coins, this as long as more than 10 pieces.

Official I²c Sonar code Finally, if all of the i²c sonar are not defined, then Sonar_init and sonar_update are defined as empty functions. Here (version 2.4 sensor.cpp:1576) requires a bit of modification to avoid repeating the definition of sonar_init and sonar_update:

#if // 2015.11.29 by Xd:check for Pingpong  void  Sonar_init () {}void  sonar_update () {}#endif

To open the Pingpong sonar in Config.h:

#define

Be aware of the addition of support for Pingpong sonar in def.h:

#if // 2015.11.29 by xd:ported from I2c_gps_nav_v2_2  #define Sonar 1#else  #define sonar 0#endif

By compiling and correctly connecting the sonar (trigger A1,echo A2), you can see that the sonar label on the MWC GUI turns green.

But now that sonar data is not being used, further code needs to be added to combine with the barometer data. Modify the Getestimatedaltitude () function in IMU.cpp to dissolve after 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)>- -&& Att.angle[roll] < -) && (Att.angle[pitch] >- -&& Att.angle[pitch] < -)))  {    //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 >> A; }  Elsealt. Estalt= Baroestalt;

In the above code, the measured value of sonar multiplied by the cosine of roll and pitch is the projection to the z axis, and the final height result is in cm. _COS10 () is the function used to calculate the cosine.

In fact, this algorithm personal feeling may be unsafe, when the height above the sonar threshold will suddenly switch to the height of the barometer, the middle of the error may cause the aircraft to shake violently. Consider setting up a transition zone, which weights the sonar/barometric data according to the height of the aircraft, and the weights are automatically adjusted according to the height.

The Cos function in the Arduino library is too expensive to test, using the original COS function, MWC is less likely to run a cycle within 2.8ms (the calculation cycle can be viewed at the GUI cycle time, and the 2.4 version is mostly 2800US, slightly beating).

So you need to write your own fast cosine function:

//2015.11.30 by XD, X was in 0.1 deg, returns COS * (1 <<)//When x approaches zero, cos (x) = 1-x ^ 2/2, X was in radians// https://en.wikipedia.org/wiki/Small-angle_approximation//rad = deg/180 * PIint32_t _cos10 (int16_t x) {//x within [ -1800, 1800]int32_t radtemp = (int32_t) x * the;//rad = x * ((pi/1800) << +), rad within [ -205200, 205200]int32_t rad = Radtemp >>6;//rad ^ 2 within [ -657922500, 657922500]int32_t COS20= ((uint32_t)1<< -)-((RAD * rad) >>1); int32_t result= Cos20 >>Ten; returnresult;}

The algorithm used is similar to the MWC Imu, and the first two items of the trigonometric function are computed under the condition of small angle. For cos: cos (x) = 1-x ^ 2/2,x is the radian unit.

Since Arduino does not have a floating-point unit and a hardware divider, the two operations need to be avoided as much as possible by manually calculating the pi/1800 and enlarging it by 65,536 times times (1 << 16).

In order to ensure that the calculation does not cross the bounds, it is necessary to advance the shift operation in the square.

When the calculation is completed, the result is shifted to the right, and the final 10-bit 2-digit valid number is approximately equivalent to 4-bit 10-binary valid digits. The end result is equal to cosine multiplied by 1024.

In the previous data fusion code, we can also see the shift operation, which is used to avoid the calculation of out-of-bounds, and the Yuxindo by 1024.

Finally, the actual test on the four-axis effect is good, Cycle time did not change significantly, the height of the calculation is very accurate, ready to go on the weekend to the field flight.

The method of increasing sonar height by MWC flight control