just got my HC-SR04 sonar module supported and implemented in MW2.4
first of all, I'd like to give credit to: kataventos for his codes and I just ported it to MultiWii 2.4. There are some changes too. This should work fairly in MultiWii 2.3 as well.
Here's the video
https://www.youtube.com/watch?v=QMQyNspc2Mg
the whole source code is uploaded here. No need to copy&paste the codes below
https://onedrive.live.com/redir?resid=a714effa21482b83!6680&authkey=!AMIFxrHS9w-pnek&ithint=file%2czip
Let's start! Please follow accordingly and let me know if you had some troubles making it work.
Let's first add a support to our sonar
add new parameter in config.h, put it anywere as long as it is inside the #ifndef CONFIG_H_ ..... #endif /* CONFIG_H_ */
I added that just before #endif /* CONFIG_H_ */Code: Select all
#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
look for this lineCode: Select all
#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR)
and add our new identifierCode: Select all
#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR) || defined(SONAR_GENERIC_ECHOPULSE)
still in def.h
just before
#endif /* DEF_H_ */
add thisCode: Select all
#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
moving on to Sensors.cpp
look for this lineCode: Select all
// ************************************************************************************************************
// 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
and in theCode: Select all
#else
void Sonar_init() {}
void Sonar_update() {}
#endif
change the two line inside the #else and #endif to thisCode: Select all
#if defined(SONAR_GENERIC_ECHOPULSE)
// ************************************************************************************************************
// Generic Sonar Support
// ************************************************************************************************************
volatile unsigned long SONAR_GEP_startTime = 0;
volatile unsigned long SONAR_GEP_echoTime = 0;
volatile static int32_t tempSonarAlt = 0;
void Sonar_init() {
SONAR_GEP_EchoPin_PCICR;
SONAR_GEP_EchoPin_PCMSK;
SONAR_GEP_EchoPin_PINMODE_IN;
SONAR_GEP_TriggerPin_PINMODE_OUT;
}
uint8_t Sonar_update() {
sonarAlt = 1 + tempSonarAlt;
SONAR_GEP_TriggerPin_PIN_LOW;
delayMicroseconds(2);
SONAR_GEP_TriggerPin_PIN_HIGH;
delayMicroseconds(10);
SONAR_GEP_TriggerPin_PIN_LOW;
return sonarAlt;
}
ISR(SONAR_GEP_EchoPin_PCINT_vect) {
if (SONAR_GEP_EchoPin_PIN & (1 << SONAR_GEP_EchoPin_PCINT)) {
SONAR_GEP_startTime = micros();
}
else {
SONAR_GEP_echoTime = micros() - SONAR_GEP_startTime;
if (SONAR_GEP_echoTime <= SONAR_GENERIC_MAX_RANGE*SONAR_GENERIC_SCALE)
tempSonarAlt = SONAR_GEP_echoTime / SONAR_GENERIC_SCALE;
else
tempSonarAlt = -1;
}
}
#else
void Sonar_init() {}
uint8_t Sonar_update() {}
#endif
ok so this time, our generic sonar modules should be working already. try to throw the sonar altitude on any debug array.
next thing we want to do is a way to activate/deactivate the sonar, much like the BARO mode.
Now we need a SONAR mode. So let's add a new box called "SONAR"
going back to MultiWii.cpp file. Look for this lineCode: Select all
#if GPS
"MISSION;"
"LAND;"
#endif
and add this line belowCode: Select all
#if SONAR
"SONAR;"
#endif
look for this line againCode: Select all
#if GPS
20, //"MISSION;"
21, //"LAND;"
#endif
and add this one belowCode: Select all
#if SONAR
23, //"SONAR;"
#endif
in Protocol.cpp
look for this lineCode: Select all
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
and add this one belowCode: Select all
#if SONAR
if (f.SONAR_MODE) tmp |= 1 << BOXSONAR;
#endif
now that we have a way to get the sonar status and a way to activate/deactivate our sonar.
what we need to do now is to have a way to integrate the sonar altitude once we activate it.
MultiWii.h
look for this lineCode: Select all
extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
and add this one belowCode: Select all
#if SONAR
extern uint16_t calibratingS;
#endif
MultiWii.cpp
in loop() method
look for this linesCode: Select all
#if ACC
if (rcOptions[BOXANGLE] || (failsafeCnt > 5 * FAILSAFE_DELAY)) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
}
else {
if (f.ANGLE_MODE) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
}
f.ANGLE_MODE = 0;
}
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
}
else {
if (f.HORIZON_MODE) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
}
f.HORIZON_MODE = 0;
}
#endif
and this one belowCode: Select all
#if SONAR
if (rcOptions[BOXSONAR]) {
if (f.SONAR_MODE == 0) {
f.SONAR_MODE = 1;
AltHold = alt.EstAlt;
#if defined(ALT_HOLD_THROTTLE_MIDPOINT)
initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
#else
initialThrottleHold = rcCommand[THROTTLE];
#endif
errorAltitudeI = 0;
BaroPID = 0;
f.THROTTLE_IGNORED = 0;
}
}
else {
f.SONAR_MODE = 0;
}
#endif
still at MultiWii.cpp
again, look for this lineCode: Select all
uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
uint16_t calibratingG;
and add this belowCode: Select all
#if SONAR
uint16_t calibratingS = 0; // sonar calibration = get new ground altitude
#endif
still at MultiWii.cpp file
in setup() method, look for this lineCode: Select all
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
and add this belowCode: Select all
#if SONAR
calibratingS = 10;
#endif
and in go_arm() method
look for this lineCode: Select all
#if BARO
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
and add this line belowCode: Select all
#if SONAR
calibratingS = 10;
#endif
and in loop() method
look for this lineCode: Select all
#if BARO
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
and add this line belowCode: Select all
#if SONAR
calibratingS = 10;
#endif
still at loop() method. Look for this lineCode: Select all
#if GPS
if (GPS_Compute() != 0) break; // performs computation on new frame only if present
#if defined(I2C_GPS)
if (GPS_NewData() != 0) break; // 160 us with no new data / much more with new data
#endif
#endif
and add/uncomment this line above!!Code: Select all
#if SONAR
Sonar_update();
#endif
it would be better though the task order of the sonar is next to BARO like mine hereCode: Select all
static uint8_t taskOrder = 0; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
taskOrder++;
#if MAG
if (Mag_getADC() != 0) break; // 320 µs
#endif
case 1:
taskOrder++;
#if BARO
if (Baro_update() != 0) break;
#endif
case 2:
taskOrder++;
#if SONAR
Sonar_update();
#endif
break;
case 3:
taskOrder++;
#if BARO || SONAR
if (getEstimatedAltitude() != 0) break; // 280 us
#endif
case 4:
taskOrder++;
#if GPS
if (GPS_Compute() != 0) break; // performs computation on new frame only if present
#if defined(I2C_GPS)
if (GPS_NewData() != 0) break; // 160 us with no new data / much more with new data
#endif
#endif
case 5:
taskOrder = 0;
//#if SONAR
// Sonar_update();
// //debug[0] = sonarAlt;
//#endif
#ifdef LANDING_LIGHTS_DDR
auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
still at loop() and in switch case for taskOrders. Look for this lineCode: Select all
#if BARO
if (getEstimatedAltitude() != 0) break; // 280 us
#endif
and update that line to make it look like thisCode: Select all
#if BARO || SONAR
if (getEstimatedAltitude() != 0) break; // 280 us
#endif
still at loop()! look for this lineCode: Select all
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
/* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
* altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
*/
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
and update it to look like thisCode: Select all
#if (BARO || SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))
/* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease
* altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms)
*/
if (f.BARO_MODE || f.SONAR_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
now let's move on to IMU.cpp and look for getEstimatedAltitude() method
look for this lineCode: Select all
// baroGroundPressureSum is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
uncomment this line or add "//" like thisCode: Select all
//alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
and add this linesCode: Select all
#if SONAR
if (f.SONAR_MODE) {
if (calibratingS > 0) {
if (!f.ARMED) { //init offset till motors not armed
//alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + sonarAlt * (1 - SONAR_BARO_LPF_LC); // additional LPF to reduce baro noise (faster by 30 µs)
BaroHome = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // play with optimal coef. here
}
calibratingS--;
}
}
#endif
#if BARO && !SONAR
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
#elif SONAR && !BARO
alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + sonarAlt * (1 - SONAR_BARO_LPF_LC);
#elif SONAR && BARO
// limit sonar altitude
//if (sonarAlt > SONAR_MAX_HOLD) {
// sonarAlt = SONAR_MAX_HOLD;
//}
if (sonarAlt < SONAR_BARO_FUSION_LC) {
alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_LC + (BaroHome + sonarAlt) * (1 - SONAR_BARO_LPF_LC); // additional LPF to reduce baro noise (faster by 30 µs)
}
else if (sonarAlt < SONAR_BARO_FUSION_HC) {
float fade = SONAR_BARO_FUSION_RATIO;
if (fade == 0.0) fade = ((float) (SONAR_BARO_FUSION_HC - sonarAlt)) / (SONAR_BARO_FUSION_HC - SONAR_BARO_FUSION_LC);
fade = constrain(fade, 0.0f, 1.0f);
// LOG: will LPF should be faded too ? sonar is less sloppy than baro and will be oversmoothed
// LOG: try same as baro alone 6/4 ratio (same as above about smoothing)
alt.EstAlt = alt.EstAlt * SONAR_BARO_LPF_HC + ((BaroHome + sonarAlt) * fade + (BaroAlt) * (1 - fade)) * (1 - SONAR_BARO_LPF_HC);
}
else {
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
}
#endif
debug[0] = sonarAlt; // raw sonar altitude
debug[1] = BaroAlt; // barometer altitude
debug[2] = alt.EstAlt;
then what follows those debug lines should be thisCode: Select all
#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
--------------- CODE UPDATE ---------------
this is update should be for our box name
in file types.h
look for this lineCode: Select all
#if GPS
uint8_t GPS_FIX :1 ;
uint8_t GPS_FIX_HOME :1 ;
uint8_t GPS_BARO_MODE : 1; // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
uint8_t GPS_head_set: 1; // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
uint8_t LAND_COMPLETED: 1;
uint8_t LAND_IN_PROGRESS: 1;
#endif
and below that, just addCode: Select all
#if SONAR
uint8_t SONAR_MODE : 1;
#endif
still in types.h
look for this lineCode: Select all
#if GPS
BOXGPSNAV,
BOXLAND,
#endif
and below that, juts addCode: Select all
#if SONAR
BOXSONAR,
#endif
go back to MultiWii.cpp and update the box ID. Reason why 23 was because I had other custom box names such as the inflight pid tuningCode: Select all
#if SONAR
23, //"SONAR;"
#endif
toCode: Select all
#if SONAR
22, //"SONAR;"
#endif
and other updates
again in types.h
look for this lineCode: Select all
#if BARO || GPS
uint8_t THROTTLE_IGNORED : 1; // If it is 1 then ignore throttle stick movements in baro mode;
#endif
and change it toCode: Select all
#if BARO || GPS || SONAR // SONAR update
uint8_t THROTTLE_IGNORED : 1; // If it is 1 then ignore throttle stick movements in baro mode;
#endif
go back to imu.cpp
look for getEstimatedAltitude() method and above that, change thisCode: Select all
#if BARO
toCode: Select all
#if BARO | SONAR
static int32_t BaroHome = 0;
(yes, that BaroHome is included)
--------------- CODE UPDATE TWO ---------------
go to Sensors.cpp and change the return type to void and do not return anything.
change this
Code: Select all
uint8_t Sonar_update() {
sonarAlt = 1 + tempSonarAlt;
SONAR_GEP_TriggerPin_PIN_LOW;
delayMicroseconds(2);
SONAR_GEP_TriggerPin_PIN_HIGH;
delayMicroseconds(10);
SONAR_GEP_TriggerPin_PIN_LOW;
return sonarAlt;
}
to this
Code: Select all
void Sonar_update() {
sonarAlt = 1 + tempSonarAlt;
SONAR_GEP_TriggerPin_PIN_LOW;
delayMicroseconds(2);
SONAR_GEP_TriggerPin_PIN_HIGH;
delayMicroseconds(10);
SONAR_GEP_TriggerPin_PIN_LOW;
}
and this
Code: Select all
#else
void Sonar_init() {}
uint8_t Sonar_update() {}
#endif
to this
Code: Select all
#else
void Sonar_init() {}
void Sonar_update() {}
#endif
to save you guys from trouble implementing the codes. I uploaded the the entire source code here
MultiWii_Generic_Sonar.zip
open MultiWii_Generic_Sonar.ino in Arduino IDE dont worry about the other files included in there. I write my codes in Visual Studio 2013 with Visual Micro to support Arduino. Make sure to setup your parameters at config.h before uploading the firmware