Just managed to get the HCSR04 working using the TinyGPS route in altitude hold mode.
I'm using the modified BARO PID code derived from Olaf.J 's solution posted earlier in this tread....
I initially wanted to use my Mega, and directly wire the sonar to the Flyduino, but I could not get Olaf's code compiled, as it seems to interfere with the recently added TinyGPS code in dev20120606. And I'm only starting to figure out how to program the Arduino, so not good enough to figure it out myself (yet)...
I assume Olaf used the bare 2.0 code (or even earlier) for the HCSR04 to Mega connection, and I'm using dev20120606.
With the TinyGPS route, the Sonar is working,, and I used the basis of Olaf Baro code (the getEstimatedAltitude part that goes in IMU) to get altitude hold....
Code: Select all
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
static int32_t BaroHigh,BaroLow;
if (currentTime < deadLine) return;
deadLine = currentTime + UPDATE_INTERVAL;
//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
last = BaroHistTab[BaroHistIdx]; // letzter Wert aus BaroHistTab
BaroHistTab[BaroHistIdx] = BaroAlt/10; // durch 10 teilen
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;
if (SonarAlt < 0) return; //wilco.... -1 = not a correct reading, Exit this routine and leave BaroPID at last value, so copter does not jump.
BaroPID = 0;
temp32 = conf.D8[PIDALT]*(SonarAlt-LastSonarAlt) / 40; // example climbed from 150 to 170 cm, D=7 -> temp32=7*(170-150)/40=3,5 -> BaroPID = -3,5
temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
// Use sonar to hold at 180 cm instead of last height
if (SonarAlt < 500)
temp32 = 100 - SonarAlt;
temp32 = 0; // if no valid result from SONAR -> no P-Value!
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0; //remove small differences to reduce noise near zero position
BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100; // temp32 auf should be for example in range +- 32 for P=16 (= 1.6 in GUI)
// example: climbed from 150 cm to 170 cm, target height: 180 cm, P=40
// BaroPID += 40*(180-170)/100 = increase BaroPID for 4,0
BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150
errorAltitudeI += temp32*conf.I8[PIDALT]/50; //example: height is 170 cm, target heigth 180 cm, I = 15 (0.015 in GUI) -> 10*15/50 = increase for 3
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
temp32 = errorAltitudeI / 500; //I in range +/-60 // this is a long term PID - will for example compensate decreasing battery voltage
I'm running a normal 328P on the multiwii, and a Attiny4313 to connect the HCSR04 (No GPS on this copter). I've temporarily connected all to my quad, and it is holding altitude pretty good.
Weather is very windy at the moment, and I can only fly around the house, where there is a lot of turbulence, but still, it manages quite OK...
made a small modification that if the sonar data is faulty/out of range (-1), the BaroPID is not reset to zero, (or even worse, run through the PID algorithm), but simply left at the last state.... That way, the copter does not jump if momentarily loosing the sonar distance (which happens quite often).
I need to activate Baro, even though my quad does not have a baro (just sonar now), otherwise the baroPID routine will not run.... but time permitting, I'm going to have a shot at improving that.... Right now I get lots of I2C errors, due to the missing Baro... (zero errors if no baro defined, so TinyGPS is working perfect).
And if I'm feeling very lucky, I might even attempt to integrate Baro + Sonar in the same routine.....
These are my first attempts to actually change something more that the config.h.... so please bare with me....
But I still would like to go the route with directly connected HCSR04 to the Flyduino.... Any progress on that with the latest dev as a base ?
I think we're getting close to a working Sonar hold.