给MWC飞控添加HC-SR04声呐支持[译]

原文地址:http://www.multiwii.com/forum/viewtopic.php?f=8&t=6282

原作者:jaysonragasa

我在mwc2.4中实现了HC-SR04声呐模块

 

让我们开始,请一步一步跟着做.并且让我知道你遇到了什么问题

在config.h中加入新的参数

在#ifndef CONFIG_H_ ..... #endif /* CONFIG_H_ */之间加入:

#pragma region GENERIC SONAR
/* Generic sonar: hc-sr04, srf04, dyp-me007, all generic sonar with echo/pulse pin
default pulse is PH6/12, echo is PB4/11
*/
#define SONAR_GENERIC_ECHOPULSE 
#define SONAR_GENERIC_SCALE 58            //scale for ranging conversion (hcsr04 is 58)
#define SONAR_GENERIC_MAX_RANGE 500         //cm (could be more)
#define SONAR_GENERIC_TRIGGER_PIN 12      // motor 12
#define SONAR_GENERIC_ECHO_PIN 11         // motor 11

/************************* Sonar alt hold / precision / ground collision keeper *******/
#define SONAR_MAX_HOLD 400               //cm, kind of error delimiter, for now to avoid rocket climbing, only usefull if no baro

//if using baro + sonar       
#define SONAR_BARO_FUSION_LC 100         //cm, baro/sonar readings fusion, low cut, below = full sonar
#define SONAR_BARO_FUSION_HC SONAR_MAX_HOLD   //cm, baro/sonar readings fusion, high cut, above = full baro
#define SONAR_BARO_FUSION_RATIO 0.0         //0.0-1.0,  baro/sonar readings fusion, amount of each sensor value, 0 = proportionnel between LC and HC
#define SONAR_BARO_LPF_LC 0.9f 
#define SONAR_BARO_LPF_HC 0.9f
#pragma endregion

def.h文件

找到这一行

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR)

加入一个新的参数

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR) || defined(SONAR_GENERIC_ECHOPULSE)

 

仍然在def.h中

在#endif /* DEF_H_ */之前加入

#pragma region GENERIC SONAR SUPPORT
#if defined(SONAR_GENERIC_ECHOPULSE)
#define SONAR_GEP_TriggerPin             SONAR_GENERIC_TRIGGER_PIN
#define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
#define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
#define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
#define SONAR_GEP_EchoPin                SONAR_GENERIC_ECHO_PIN
#define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
#define SONAR_GEP_EchoPin_PCINT          PCINT5
#define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
#define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
#define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
#define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif
#pragma endregion

到Sensors.cpp文件

找到这些

// ************************************************************************************************************
// I2C Sonar SRF08
// ************************************************************************************************************
// first contribution from guru_florida (02-25-2012)
//
#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235)
   .
   .
   .
   .
   .
#else
   void Sonar_init() {}
   void Sonar_update() {}
#endif

在这两行中

#else
   void Sonar_init() {}
   void Sonar_update() {}
#endif