Posted by Pete Burnight on October 4, 2009 at 12:30pm
While flight testing my Magpie and ArduPilot, I was having a problem with over-shoot during Way Point Navigation. After hitting the way point, the aircraft would start turning towards the next way point, and then continue turning until way past the bearing to the next way point, requiring the aircraft to make a sweeping s-turn to get back on track towards the next way point.
I suspected a number of things... the directional bearing data the aircraft uses comes from the GPS "Ground Course" data. It has no idea which direction you're actually pointing, it can only tell you the bearing between where you are now, and where you were a little while ago. I'm using the EM406 GPS, and there's a little lag in what it tells you your course is, as it tries to integrate, and smooth the data it's receiving. In addition, while my aircraft is in a high banking turn, it's actually pointing "ahead" of where the direction vector says it is, and, my GPS only puts out new data once per second, so the current course data available to the Nav functions is old, and the aircraft easily gets pointed way ahead of where I want it to.So... I did a couple things. I added code to calculate my own CMG (course made good) that didn't filter or smooth the data so I'd have better data quicker, I added a Gyro to update my current bearing based on rate of rotation during a turn, and I added some code to compensate for high bank turns, that essentially add some offset to the current heading based on how much the aircraft is banking.Works Great. The Gyro is sampled every 20 ms, and updates the variable gyroHeading based on rate of rotation. Every 200 ms I set the update flag so the nav routines think there's new data, and re-evaluate the roll set point based on the updated heading provided by gyroHeading. Every time I get new GPS data (once per second) I re-calc my own version of CMG by calc'ing the bearing BACK to where we were 1 second ago, and setting gyroHeading to this new course. That way, gyro drift is minimized (it only has 1 second to get in trouble) and I really only need the gyro during high bank turns, where the aircraft's course is changing quickly, and I'm only getting GPS data once per second. This gives the nav function 5 updates per second on the "true" nature of the quickly changing course of the aircraft, and the PID functions then do a much better job of calculating the roll set point needed to "round the mark" smoothly.
// --------------------------------------------------------------------------------------------------// adjust the heading if we're banking hardint heading_roll_compensation(){int rollAmt = get_roll();if (abs(rollAmt) < 16) return 0; // don't adjust unless banking hardreturn (rollAmt/2);}// --------------------------------------------------------------------------------------------------// Calc our own Course Made Good - use as Gyro Referencevoid calc_our_CMG(){static float prevLat=0;static float prevLon=0;if (calc_dist(prevLat, prevLon, lat, lon) > 1){ourCMG = calc_bearing(prevLat, prevLon, lat, lon) + heading_roll_compensation();if (ourCMG > 360.0) ourCMG -= 360.0;if (ourCMG < 0) ourCMG += 360.0;prevLat = lat;prevLon = lon;gyroHeading = ourCMG;}}// --------------------------------------------------------------------------------------------------The code is then used within the function navigation():wp_bearing = calc_bearing(lat, lon, wp_current_lat, wp_current_lon);calc_our_CMG();roll_set_point = calc_roll(heading_error(wp_bearing, gyroHeading), dt_t);Note that calc_our_CMG() will NOT update gyroHeading every time thru, only when it gets new GPS data that puts us at least 1 meter away from our previous position (usually every second) In between, the gyro will have been updating gyroHeading 5 times per second so the nav routines can respond quick during the turn.The code below supports the Gyro. Init_Gyro_Bias() is called during init - NO MOTION.The main routine sample_gyro_data() is called each time thru the main loop.// ---------------------------------------------------------------------------------------------------------------------------// Called from Main Loop to sample gyro data & update gyroHeading.// Sample the analog data from the Gyro about 2,000 times per second.// call the Update Heading routine only every 20ms// smooth the over-sampled analog lines.void sample_gyro_data(){static unsigned long gyroTimer=0;static unsigned int gyroCounter = 0;unsigned long dt;analog4 = (analog4 + analogRead(4)) >> 1; // sample Analog inputs every time thruanalog5 = (analog5 + analogRead(5)) >> 1; // analog 4 & 5 are for the Gyrodt = millis() - gyroTimer;if (dt > 50) dt = 50;if (dt > 20) {update_gyro_heading(dt);gyroTimer = millis();gyroCounter++;if (gyroCounter > 10) {data_update_event |= 1; // update rudder info 5 times a secondgyroCounter = 0;}}}// -----------------------------------------------------------------------------------------------------------------------------// rate of gyro at full swing = 150 degrees per second// this should be called every 20ms -> 50 times per second// dt is in milli-secondsvoid update_gyro_heading(int dt){int gData;float gyroDelta;gData = (analog4 - analog5) - gyroBias; // range is now -512 ... 0 ... 512gyroDelta = (float)(gData) / 512.0; // range is now -1.0 ... 0 ... 1.0gyroDelta = gyroDelta * 150.0 * (float)(dt / 1000.0);gyroHeading += gyroDelta;if (gyroHeading > 360.0) gyroHeading -= 360.0;if (gyroHeading < 0.0) gyroHeading += 360.0;}// ---------------------------------------------------------------------------------------------------------------------------// gyroBias = reading of gyro at rest. So we know the zero point.// called at Init time gyroBias = init_gyro_bias();int init_gyro_bias(){int n;int theBias;analog4 = analogRead(4); // sample Analog input for gyro rateanalog5 = analogRead(5); // 2.5v voltage reference = 512theBias = analog4 - analog5;for (n=0; n<50; n++){analog4 = (analog4 + analogRead(4)) >> 1; // range 0..1023analog5 = (analog5 + analogRead(5)) >> 1;theBias = (theBias + (analog4-analog5)) / 2; // signed value -512..0..511}return theBias;}// ---------------------------------------------------------------------------------------------------------------------------
I'm using an ADXRS613 @ 150 degrees / sec. via a breakout board from Spark Fun. It outputs an analog signal 0 to 5 volts, so I sample the rotational rate voltage via analog(4) and the 2.5v vRef on analog(5).
Comments
What gyro are you using? How do you have it interfaced to Analog inputs 4 & 5?
Regards,
TCIII