Airplane mode RTH

ardufriki · Post by **ardufriki** » Thu Mar 26, 2015 12:24 pm

OK, but the behaviour in a Multiwii FAILSAFE is to try to land, and in OLRS I can set RTH (I have GPS). It is not the same.
My question is if I can set both ON.

universam · Post by **universam** » Thu Mar 26, 2015 1:02 pm

are you mixing probably something? OLRS has no GPS RTH but MWII...

To your question: MWII failsafe ON, OLRS failsafe OFF (no PPM output enabled)

ardufriki · Post by **ardufriki** » Thu Mar 26, 2015 1:12 pm

Sorry, I dont explain myself well.

What I meant is that I set the OLRS FAISAFE to enable RTH in case of that (switches to RTH and throttle 50% in copter, I think in plane is the same). I other words, If I switch off the transmitter, the OLRS says to MultiWii code that it has to RTH. I have tested and used this in multicopters and works fine.

But in that case ¿should I set the FAILSAFE option in config.h?

PatrikE · Post by **PatrikE** » Thu Mar 26, 2015 3:13 pm

Hi.
Set Mwii failsafe on.
Olrs stop send ppm at failsafe.
Fixedwing Rth failsafe will take care of everything.
It is not the same as Mwii original FS.
You can check my blog where it's explained.
And a link to the absolute latest version.
Witch I have tested several flights now an it behaves good.
http://fotoflygarn.blogspot.com/2012/03 ... plane.html

rubadub · Post by **rubadub** » Fri Mar 27, 2015 12:09 am

@Patrik,
what would we need to do to get a heading lock / "cruise mode" setup?
Something similar to APM's, where it automatically navigates to a distant waypoint based on the current heading, and will automatically hold altitude as well.

PatrikE · Post by **PatrikE** » Fri Mar 27, 2015 9:01 am

I'm on it atm..
Just have to test if it works as good for real as in theory first..

Thinking to avoid more flightmodes.
If Mag is active when hold is activated move pos ex 1km ahead.
And simply use Poshold to maintain altitude ad navigate.
When it reaches the target it will start circling there.
1 km to target can be changed.

rubadub · Post by **rubadub** » Fri Mar 27, 2015 10:23 am

PatrikE wrote:I'm on it atm..
Just have to test if it works as good for real as in theory first..
Thinking to avoid more flightmodes.
If Mag is active when hold is activated move pos ex 1km ahead.
And simply use Poshold to maintain altitude ad navigate.
When it reaches the target it will start circling there.
1 km to target can be changed.

ok, cool, great...
using 'MAG+GPS HOLD' sounds like a good idea.

just for reference, I'm reposting the relevant APM functions:

Code: Select all

//Arduplane.pde
static void update_flight_mode(void)
{
...
    case CRUISE:
        /*
          in CRUISE mode we use the navigation code to control
          roll when heading is locked. Heading becomes unlocked on
          any aileron or rudder input
        */
        if ((channel_roll->control_in != 0 ||
             channel_rudder->control_in != 0)) {                
            cruise_state.locked_heading = false;
            cruise_state.lock_timer_ms = 0;
        }                 
        
        if (!cruise_state.locked_heading) {
            nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
            nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
            update_load_factor();
        } else {
            calc_nav_roll();
        }
        update_fbwb_speed_height();
        break;
        
//navigation.pde
static void update_cruise()
{
    if (!cruise_state.locked_heading &&
        channel_roll->control_in == 0 &&
        channel_rudder->control_in == 0 &&
        gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
        gps.ground_speed() >= 3 &&
        cruise_state.lock_timer_ms == 0) {
        // user wants to lock the heading - start the timer
        cruise_state.lock_timer_ms = hal.scheduler->millis();
    }
    if (cruise_state.lock_timer_ms != 0 &&
        (hal.scheduler->millis() - cruise_state.lock_timer_ms) > 500) {
        // lock the heading after 0.5 seconds of zero heading input
        // from user
        cruise_state.locked_heading = true;
        cruise_state.lock_timer_ms = 0;
        cruise_state.locked_heading_cd = gps.ground_course_cd();
        prev_WP_loc = current_loc;
    }
    if (cruise_state.locked_heading) {
        next_WP_loc = prev_WP_loc;
        // always look 1km ahead
        location_update(next_WP_loc,
                        cruise_state.locked_heading_cd*0.01f, 
                        get_distance(prev_WP_loc, current_loc) + 1000);
        nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
    }
}

So, it seems that they constantly update the 'next 1km' waypoint on each call, and only update the heading on AIL or RUD stick input. Does doing something like that sound like a reasonable approach? Or, is there a specific reason for opting to have a fixed loiter target vs. constantly updating it to always be 1km ahead?

PatrikE · Post by **PatrikE** » Fri Mar 27, 2015 11:25 am

I had a much simpler approach in mind.
Need to think about code size to support PRO mini FC's too.

Constant update would mean it you get some wind drift and model turns right next_WP_loc would be right of the original target.
And i don't like the Idé of letting a model flying away in infinity.(I have experienced that before...)
But i have planed non centered sticks to for resetting 1km ahead.(AP_MODE)

PatrikE · Post by **PatrikE** » Fri Mar 27, 2015 1:02 pm

A raw Idé how to implement it.
I think this inside void GPS_set_next_wp function should do it.
Just add the difference to the hold pos.

Code: Select all

if (f.GPS_mode == GPS_MODE_HOLD && f.MAG_MODE){ 
#define PRESET_DIST      1000  // Move WP forward xx meter
#define GEO_SKALEFACT    110.0f  // Scale to match meters

    float wp_lat_diff = cos(att.heading*0.0174532925f);
   float wp_lon_diff = sin(att.heading*0.0174532925f);   
   GPS_WP[LAT] += (float)(wp_lat_diff * PRESET_DIST * GEO_SKALEFACT);
    GPS_WP[LON] += (float)(wp_lon_diff * PRESET_DIST * GEO_SKALEFACT);
}

I havn't even testcompiled the code yet.
Any Idés about it?

msev · Post by **msev** » Fri Mar 27, 2015 6:58 pm

Can this latest approach be easily ported to naze32 -> baseflight? Since a lot of people are using that on that platform aparently

, cruise mode would be a really nice addition. How about rally points do you think they would be portable to baseflight?

PatrikE · Post by **PatrikE** » Fri Mar 27, 2015 9:14 pm

msev wrote:Can this latest approach be easily ported to naze32 -> baseflight? Since a lot of people are using that on that platform aparently , cruise mode would be a really nice addition. How about rally points do you think they would be portable to baseflight?

Cruise mode Yes but Maybe we can test it on 8bit first...
Rally points need a waypoint implemetation...
So with TC attitude to GPS functions i would say no.

PatrikE · Post by **PatrikE** » Sat Mar 28, 2015 7:40 pm

Does anyone have the formula room calculate longitude scaling available?
That seems.to be the last piece missing now.

rubadub · Post by **rubadub** » Sat Mar 28, 2015 11:45 pm

PatrikE wrote:Does anyone have the formula room calculate longitude scaling available?
That seems.to be the last piece missing now.

what do you mean by longitude scaling?
here's a link with a formula for calculating lat&lng given start coords, distance, and bearing:
http://www.movable-type.co.uk/scripts/latlong.html

(scroll down a bit for the correct distance/bearing formula)

rubadub · Post by **rubadub** » Sun Mar 29, 2015 12:32 am

more stuff from the APM code:

Code: Select all


/*
 *  extrapolate latitude/longitude given bearing and distance
 * Note that this function is accurate to about 1mm at a distance of 
 * 100m. This function has the advantage that it works in relative
 * positions, so it keeps the accuracy even when dealing with small
 * distances and floating point numbers
 */
void location_update(struct Location &loc, float bearing, float distance)
{
    float ofs_north = cosf(radians(bearing))*distance;
    float ofs_east  = sinf(radians(bearing))*distance;
    location_offset(loc, ofs_north, ofs_east);
}

/*
 *  extrapolate latitude/longitude given distances north and east
 *  This function costs about 80 usec on an AVR2560
 */
void location_offset(struct Location &loc, float ofs_north, float ofs_east)
{
    if (ofs_north != 0 || ofs_east != 0) {
        int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
        int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(loc);
        loc.lat += dlat;
        loc.lng += dlng;
    }
}

// radius of earth in meters
#define RADIUS_OF_EARTH 6378100

// scaling factor from 1e-7 degrees to meters at equater
// == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
#define LOCATION_SCALING_FACTOR 0.011131884502145034f
// inverse of LOCATION_SCALING_FACTOR
#define LOCATION_SCALING_FACTOR_INV 89.83204953368922f

float longitude_scale(const struct Location &loc)
{
    static int32_t last_lat;
    static float scale = 1.0;
    if (labs(last_lat - loc.lat) < 100000) {
        // we are within 0.01 degrees (about 1km) of the
        // same latitude. We can avoid the cos() and return
        // the same scale factor.
        return scale;
    }
    scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
    scale = constrain_float(scale, 0.01f, 1.0f);
    last_lat = loc.lat;
    return scale;
}

#ifndef cosf
# define cosf cos
#endif

#ifndef sinf
# define sinf sin
#endif

PatrikE · Post by **PatrikE** » Sun Mar 29, 2015 11:43 am

Well this is what i have come up with.
Reused most calculations from inside MWii code.
Hijacking safe_wp_distance as distance to make it settable from WinGui
You also need to take a look at Fence radius.
It will force the plane to RTH if it exceeds that distace.

Code: Select all

#ifdef FIXEDWING
  static int32_t hh;
  if (f.GPS_mode == GPS_MODE_HOLD && f.MAG_MODE){
    #define GEO_SKALEFACT    89.832f  // Scale to match meters
    
    if (att.heading >180)  // Unwrap +/-180  to  0 - 360
      {hh = 360 -att.heading ;} else {hh =att.heading;}
      
    float scaler=( GEO_SKALEFACT/GPS_scaleLonDown) * GPS_conf.safe_wp_distance;
    float wp_lat_diff = cos(hh*0.0174532925f) * GPS_scaleLonDown;
    float wp_lon_diff = sin(hh*0.0174532925f);
    GPS_WP[LAT] +=wp_lat_diff*scaler;
    GPS_WP[LON] +=wp_lon_diff*scaler;
  }
#endif

I now get same distance all over the planet.
Checked from Solomon island to Rejkavik.
1000 meters becomes ~995 - 1000 in the coordinat system.

Frank_W · Post by **Frank_W** » Sun Mar 29, 2015 4:32 pm

Was out testing my multiwii for the first time yesterday and now have some questions: 1. How should the baro mode work, i could change altitude with the elevator but was not expecting that. 2) I was switching between modes and suddenly did the plane start banking hard everytime i switched to acro mode. Landed and disconnected the battery ad it worked fine again after that, what may have caused that gyro (?) problem?. 3) The plane suddenly died but luckily landed ok and when i got to the plane did everything work again and i suspect my 3A bec is not enough to power 4 servos +2 for flaps (plugged in but unused) + Rx and Multiwii, causing the multiwii to restart, could that be the case?

PatrikE · Post by **PatrikE** » Sun Mar 29, 2015 5:40 pm

Actually Baromode is only for copters and handles only throttle and can stall the plane if you use Elevator.
Disable Barocode in config.h with this define or don't use it..
#define SUPPRESS_BARO_ALTHOLD
I will isolate the code later.

I guess you're on the correct path on the other issues!..
Bad calibrated Gyros and Brownout caused of the bec.

The MegaBased boards is sensitive to voltage drop and reboots.
I have tested and ended up in a tree.. (Two times)
The board can lock up in boot the sequence if ARM is AUX controlled.

Small MG-servos is thirsty for power.
At least a large Cap on FC is recommended.( ~3300 or larger. )
Best is separate bec's for FC and servos.

Frank_W · Post by **Frank_W** » Sun Mar 29, 2015 8:31 pm

PatrikE wrote:Actually Baromode is only for copters and handles only throttle and can stall the plane if you use Elevator.
Disable Barocode in config.h with this define or don't use it..
#define SUPPRESS_BARO_ALTHOLD
I will isolate the code later.

I guess you're on the correct path on the other issues!..
Bad calibrated Gyros and Brownout caused of the bec.

The MegaBased boards is sensitive to voltage drop and reboots.
I have tested and ended up in a tree.. (Two times)
The board can lock up in boot the sequence if ARM is AUX controlled.

Small MG-servos is thirsty for power.
At least a large Cap on FC is recommended.( ~3300 or larger. )
Best is separate bec's for FC and servos.

Thanks for the answers. Then I understand why Baro mode did not work, pity since I thought it would be a great mode to use for the kids.
I realised my HobbyWing ESC only has 2A BEC, not 3A, so probably not enough (lucky that it ended so well, the hard landing included a boucnce over a big stone so I was quite lucky to walk home with only a small crack in the landing gear fixture...).

I'd rather go for an ESC with a more powerful BEC, like the Hobbyking YEP 30 A which has 4A BEC. What would be the advantage with a separate UBEC over a better ESC? I like the idea of fewer compoents and wires, it is already quite full.

I did not understood what you ment with "a large CAP on FC is recommended"?

Thanks!

PatrikE · Post by **PatrikE** » Sun Mar 29, 2015 9:45 pm

The navigation for fixedwing is written for RTH as it is now.
It need to be modified to be able to run ex Althold separatly.

Cap is short for capacitator.(Kondensator)
And is connected on a free servo output.
It filters out some of the dips in power.

Plüschi · Post by **Plüschi** » Mon Mar 30, 2015 12:43 am

@PatrickE

As i understand you use / prefer GPS bearing instead of mag for RTH.
Do you use any sensor fusion with gyro to get better bearing measurement during turns than from gps alone?
What is the delay the GPS bearing measurement has? What delay do i have to expect from a 1hz GPS ?

PatrikE · Post by **PatrikE** » Mon Mar 30, 2015 8:06 am

As i understand you use / prefer GPS bearing instead of mag for RTH.

I do since planes usually don't fly where the nose points.
But often a bit sideways in wind.
GPS gives a more true course.

Today i just overwrite MAG course if it diff from GPS.
But it would be nice to use sensor fusion with gyro as you suggest.
Use the Heading from gyro and correct it with GPS data when needed.

I don't know how to do it but maybe someone else do?..
Im not sure of the delay on a 1HZ but i guess it don't really matter on a plane doing RTH.
In a mission Waypoint radius on the other hand can be a issue..
Since a moderate 36km/h = ~10m/s you can easily miss the waypoint between updates.

rubadub · Post by **rubadub** » Mon Mar 30, 2015 10:15 pm

Please correct me if I'm wrong, but I was under the impression that normally gyro data is fused with the 3-axis mag data in order to calculate heading (for more accuracy & quicker response?). Then, the heading is mixed/compared with the GPS heading. correct or incorrect? I believe that's how the APM does it anyway.

Maybe I'm missing something obvious, but I don't personally see how the gyro data could be effectively mixed with the GPS heading. It seems that the GPS heading would be pretty laggy, and as Patrik pointed out, the plane's nose might not be necessarily line up with the GPS heading.

However, in order to do it, I guess maybe the degree error about the yaw axis would need to be summed up / integrated within the sensor loop, and then compared & adjusted to the GPS heading as soon as new GPS heading info is available. Perhaps something like a complementary filter that adds more weight to the GPS heading? I'm not sure how much use this would be of or how accurate it would be though. thoughts?

on the topic of mag vs. GPS heading, I think Patrik is right that, in most cases (especially RTH or 'cruise' which are basically long-distance, straight-line flight paths), GPS heading is good enough and is actually preferable since it would automatically handle any cross-wind effects. However, for any type of flying that involves sharp turning and lots of quick heading changes (loiter/circle, waypoint nav, rallypoints, etc.), I would guess that it would be a better idea to rely on a mag-based heading since it's faster and would be more accurate in these cases. just my $0.02...

Plüschi · Post by **Plüschi** » Tue Mar 31, 2015 11:58 am

I think GPS heading is superior because MAG is affected by electrical current flowing nearby. Unless you have a sailplane or an external mag (like dji) mag readings are not very reliable.

Gyro angles are in the plane coordinate system. Gps heading is on a horizontal plane. A fusion of gps and gyro needs a 3D rotation of the beraing vector into the plane coordinate system. I have no idea how to do that. The "small angle rotation" used in "imu.cpp" doesent work since angles are not small. Mag calculation is simpler because mag comes in the plane coordinate system. Gps bearing does not.

If you use only yaw gyro and calculate in a 2D plane you will get wrong results if the plane is banking. Imagine a plane doing a steep 180deg turn with wings nearly vertical. Yaw gyro wont register anything at all, only pitch gyro will scream.

Sensor fusion of gps bearing and gyro would give a better bearing value during turns. This becomes important at very slow gps rates (i use 1 hz). Controlling yaw if you get a bearing only every second and delayed by another second is not easy.

rubadub · Post by **rubadub** » Tue Mar 31, 2015 9:39 pm

Plüschi wrote:I think GPS heading is superior because MAG is affected by electrical current flowing nearby. Unless you have a sailplane or an external mag (like dji) mag readings are not very reliable.

Gyro angles are in the plane coordinate system. Gps heading is on a horizontal plane. A fusion of gps and gyro needs a 3D rotation of the beraing vector into the plane coordinate system. I have no idea how to do that. The "small angle rotation" used in "imu.cpp" doesent work since angles are not small. Mag calculation is simpler because mag comes in the plane coordinate system. Gps bearing does not.

ok, now I understand.
yeah, I don't see how you could easily translate the 3D gyro values into the earth coordinate system, since there's nothing to tie the both together. I guess maybe you could use the accelerometer values to find out which way is "down"/Z-axis and then use the GPS heading to get a reference direction in the XY plane? Seems like the math will start to get really complicated and messy though, dunno. I'm no expert, but I'm guessing it will require using quaternions to do the rotation, not really sure though.

I realize that the mag can be prone to interference, but I've honestly never had that much of a problem with it on a plane w/ only a single motor that's usually at a distance from the FC. It just seems like it would make the math a whole lot easier to just fuse mag w/ gyro, and then fuse GPS bearing w/ mag+gyro. I guess we could always add a simple motor compensation kind of like how the APM does.

Plüschi wrote:If you use only yaw gyro and calculate in a 2D plane you will get wrong results if the plane is banking. Imagine a plane doing a steep 180deg turn with wings nearly vertical. Yaw gyro wont register anything at all, only pitch gyro will scream.

yeah, I realized this after I posted.
I wasn't taking into account the 3D components, but now I realize how only using the yaw gyro won't work.

Another thing that I wonder is if the gyros will be sensitive enough to pickup slow, constant rotations. It seems that gyros are great at picking up really quick angular rotations, but they're not so great at detecting something like the rotation caused from doing a long, slow banking turn (not even sure if they'd pick this up, would need to test I guess). thoughts?

Plüschi · Post by **Plüschi** » Tue Mar 31, 2015 10:34 pm

rubadub wrote:just fuse mag w/ gyro, and then fuse GPS bearing w/ mag+gyro

The mag - gyro fusion is already done in the current imu. Gyro is a relative value, mag an absolute. But mag and gps are both absolute values, how are you going to fuse them?

Plüschi wrote:gyros will be sensitive enough to pickup slow, constant rotations

In the experiment with the pockedquad "magless HH" code the mpu6050 gyros did show an astonishing performance. We got deviations of less than 5 deg per minute with full-res readings (pocketquad build or baseflight). Those gyros are good enough for very very slow turns.

I guess MWii still uses 256 resolution. This will somewhat destroy performance, but this is easy to modify to 4096.

rubadub · Post by **rubadub** » Tue Mar 31, 2015 11:31 pm

Plüschi wrote:The mag - gyro fusion is already done in the current imu. Gyro is a relative value, mag an absolute. But mag and gps are both absolute values, how are you going to fuse them?

oh, ok, cool. that's good to know.
I just looked at the code and saw the fusion.
So, MW does a vector rotation based on both the mag & gyro data.
In that case, I don't see how this would be done with GPS as the 3D heading components wouldn't be available to do such a rotation, and, like you stated, there's still a disconnect between the two coordinate systems (plane & earth).

I guess fusion wasn't the right term, maybe something more along the lines of comparing the mag&gps readings, inspecting the delta, and/or combining them, perhaps placing a larger weight on one or the other based on some parameter. Maybe something such as applying more weight to GPS heading since it's more stable & then mix in the mag data for the interim between successive GPS readings.

In any case, for whatever it's worth, I'd still vote for using the mag values, even if some motor/throttle compensation or GPS-weighting is required to stabilize the readings.

Plüschi wrote:In the experiment with the pockedquad "magless HH" code the mpu6050 gyros did show an astonishing performance. We got deviations of less than 5 deg per minute with full-res readings (pocketquad build or baseflight). Those gyros are good enough for very very slow turns.

I guess MWii still uses 256 resolution. This will somewhat destroy performance, but this is easy to modify to 4096.

cool. so why hasn't the resolution been increased then? Would this be affected by running a lower hardware LPF value (such as 20Hz)?

Plüschi · Post by **Plüschi** » Sun Apr 05, 2015 7:40 am

About gyro-gps bearing fusion i did experiment with this:

Code: Select all

  myheadingdelta = EstG.V.Y * deltaGyroAngle[0] - 
                   EstG.V.X * deltaGyroAngle[1] +
                   EstG.V.Z * deltaGyroAngle[2];
  myheading += myheadingdelta * 0.01391;// 57/4096

Result is in deg. This is experimental with no claims to be correct. Board is a 328 crius_se.
The factor 0.01391 is "rad-to-deg / gyro resolution", or "57 / gyro res". You have to adapt this for mwii.
This doesent like inverted flight, and i suppose it doesent like inclinations in both axes.

The complimentary filter has to be adapted for the -180 / +180 problem. Mine looks like:

Code: Select all

  float diff = gpsheading - myheading;
  if (diff >  180.0) diff -=360.0;
  if (diff < -180.0) diff +=360.0;
  myheading += GPS_CMPF_FACTOR * diff;
  if (myheading >  180.0) myheading -=360.0;
  if (myheading < -180.0) myheading +=360.0;
  heading = myheading;

where GPS_CMPF_FACTOR is 0.001.

PatrikE · Post by **PatrikE** » Sun Apr 05, 2015 10:45 am

Well im lost in the IMU stuff.
Just some other unsorted thoughts...(Rambling..)
The way it works today isn't really perfect.
If the plane is moving heading is overwritten By GPS. (that's OK.)
But when the plane is stationary It's not.
That means the heading jumps back to where Mag or Gyro say it's pointing.(Annoying on testbench at least.)
With a Mag in the system that's ok if it's calibrated and not disturbed.
For Gyro heading it's less perfect.
Since it assumes North is where the nose was pointing at init and calculates from that.(Who starts up FC heading north?)
It can be 180 degrees wrong from start and build up errors during the flight

It must be based on some I-term wind up or something.
Would it be possible to calibrate the Gyro heading when the model gets airborne to correct it using gps?

Plüschi · Post by **Plüschi** » Sun Apr 05, 2015 1:29 pm

Rant:
DONT use the actual 2.4 magless gyro code for planes.
It can go crazy with gyro drift. This was developped for pockedquad and short 3 min flights, not for planes.
I am surprised to see that code here without a warning message.

Try the patch above. It does a non-crazy version of the magless gyro code and it corrects the gyro heading according to GPS.
The 0.001 factor determines how fast gps corrects gyro heading. You may have to up that to 0.01 for 10hz gps.

Video: https://www.youtube.com/watch?v=n-4pDY1 ... e=youtu.be
Here gps is simulated and gives a constant 0 deg heading. This is the code above.

brm · Post by **brm** » Sun Apr 05, 2015 3:02 pm

yes, full ack for for plüschi.

rubadub · Post by **rubadub** » Mon Apr 06, 2015 12:55 am

Plüschi wrote:About gyro-gps bearing fusion i did experiment with this:
Code: Select all
  myheadingdelta = EstG.V.Y * deltaGyroAngle[0] - 
                   EstG.V.X * deltaGyroAngle[1] +
                   EstG.V.Z * deltaGyroAngle[2];
  myheading += myheadingdelta * 0.01391;// 57/4096
Result is in deg. This is experimental with no claims to be correct. Board is a 328 crius_se.
The factor 0.01391 is "rad-to-deg / gyro resolution", or "57 / gyro res". You have to adapt this for mwii.
This doesent like inverted flight, and i suppose it doesent like inclinations in both axes.

The complimentary filter has to be adapted for the -180 / +180 problem. Mine looks like:
Code: Select all
  float diff = gpsheading - myheading;
  if (diff >  180.0) diff -=360.0;
  if (diff < -180.0) diff +=360.0;
  myheading += GPS_CMPF_FACTOR * diff;
  if (myheading >  180.0) myheading -=360.0;
  if (myheading < -180.0) myheading +=360.0;
  heading = myheading;
where GPS_CMPF_FACTOR is 0.001.

although that looks pretty nifty, still seems like a sketchy solution. As you stated, it doesn't perform too well with inclination and would still be theoretically subject to some drift.

I'd still personally prefer to get heading readings from a mag sensor, and then run it through the filter, using the GPS heading as a reference point and simply using the heading deltas from the mag. The mag also gives the advantage of providing a more reliable heading on the ground or if there are any glitches/noise in either the gyro or GPS data.

However, for someone running a plane without a mag (or who simply doesn't trust the mag sensor and prefers to use gyros), this might be a viable option. I'd suggest that it be split off here into a user-configurable option to either fuse the GPS heading with the gyro data or the mag data (let the user decide).

Plüschi · Post by **Plüschi** » Mon Apr 06, 2015 1:56 pm

There is the full vector rotation code in baseflight imu. Not the "small angle" approximation, but the real thing.

Using that code we can construct a 3D vector out of GPS bearing and horizontal plane and rotate this into the plane frame (using roll and pitch angles). Then correct the the directional vector (reused EstM) just like acc is used to correct EstG (gravity) vector. This is not possible with MWii's approximation.

I guess this will totally blow mwii's cycle time since the rotation is 900usec on a atmega, but with baseflight and stm32 this should work.

rubadub · Post by **rubadub** » Thu Apr 09, 2015 10:52 pm

@Patrik,
I'm interested in adding an optional feature for FW RTH that relies on stick movement to disable RTH similar to the way the ET vector works, where RTH isn't disengaged unless the sticks are moved (whether it's been enabled via failsafe or an aux switch).

I'm looking at the MW code, and thinking about possibly setting up something in the 'annexCode()' function, perhaps a variable that saves the current stick state and checks to see if the value has changed. Perhaps a test to see if it has changed greater than some user-configurable threshold value & over some time frame (just shooting off ideas here).

So, if a stick 'wiggle' is detected & RTH is already enabled, then disable it. If not, then continue with RTH, regardless of whether FS has been disabled or what the state of the RTH aux switch is.

Anyway, I just wanted to run this by you and get your input as to what might be the easiest/best way to set this up. Any thoughts?

PatrikE · Post by **PatrikE** » Fri Apr 10, 2015 10:02 am

Hi Rub..

Like a Sticky mode..
When regaining control after Failsafe it would be good to not release the RTH until Pilot takes command...
As it is now it just releases the moment Failsafe conditions is OK and can leave the plane in Pasthru.

Fence function have kind of that.
You must toggle RTH before it releases.

But When switch operated the pilot knows it's not in RTH any more.(But why not)

It should be possible to reuse the AP_Mode code to detect input.

I have one more Sticky function i would like too.
Finish the mission even in Failsafe.

PatrikE · Post by **PatrikE** » Fri Apr 10, 2015 10:26 am

A small Update on the latest dev.

In MissionPlanner this Modes is supported but need to be verified.
MISSION_WAYPOINT - OK
MISSION_JUMP - ?
MISSION_RTH - ?

Modes not supported by 328p FC and will result in PosHold.
Should work on Mega FC's but needs to be verified.
MISSION_HOLD_TIME
MISSION_HOLD_UNLIM
MISSION_LAND

MISSION_SET_POI ( Totally crazy function on planes... )

I have also added a Cruise mode
The plane will set a Position to navigate to in the current Heading.(Discussed earlier in thread.)

From the test i have performed in windy conditions.
The plane Navigates well.
Even in 10m/s wind it can hit waypoint radius of 100cm.

It climbs to correct altitude good.
But descending is harder.(Especially in headwind)

I'll make next test with less ELEVATORCOMPENSATION
It was 100% But i reduced it to 60% now. (in gps.h)
Redone Acc calibration with zero angle of attack.
Hopefully it will make difference.

I will have a look at Plüschi's code if i can implement it.

I have added a Text doc in the latest dev with more info about the Navmodes.
MultiWii_FWnav_150407.zip

And last a nce site to check flying weather i found..
http://www.uavforecast.com/

rubadub · Post by **rubadub** » Sat Apr 11, 2015 12:07 am

PatrikE wrote:Hi Rub..

Like a Sticky mode..
When regaining control after Failsafe it would be good to not release the RTH until Pilot takes command...
As it is now it just releases the moment Failsafe conditions is OK and can leave the plane in Pasthru.

Fence function have kind of that.
You must toggle RTH before it releases.

But When switch operated the pilot knows it's not in RTH any more.(But why not)

It should be possible to reuse the AP_Mode code to detect input.

I have one more Sticky function i would like too.
Finish the mission even in Failsafe.

cool, good to know that you agree.

In my case, I don't use MW's failsafe, and instead use my receiver's programmable failsafe settings. So, I have RTH setup on an aux switch and can still make use of a sticky RTH feature.

Ideally, what I'd like to avoid is having RTH quickly go in & out when I'm at the edge of my signal range and have the plane severely jarred during the transitions. It would be much better to be able to just let RTH fully engage, allow the plane and RC signal to stabilize, and then take back control as soon as I feel I'm ready.

PatrikE · Post by **PatrikE** » Sun Apr 12, 2015 1:14 pm

rubadub wrote:In my case, I don't use MW's failsafe, and instead use my receiver's programmable failsafe settings. So, I have RTH setup on an aux switch and can still make use of a sticky RTH feature.

In that case MWii don't know it's in Failsafe.
In a real failsafe case your plane will be circling over home until you can get control and take over or Runs out of power!.

MWii Failsafe cuts power and lets the plane descend when it reaches home.
And it's really easy to make a sticky Failsafe to check stick inputs.

rubadub · Post by **rubadub** » Mon Apr 13, 2015 5:30 am

PatrikE wrote:
rubadub wrote:In my case, I don't use MW's failsafe, and instead use my receiver's programmable failsafe settings. So, I have RTH setup on an aux switch and can still make use of a sticky RTH feature.

In that case MWii don't know it's in Failsafe.
In a real failsafe case your plane will be circling over home until you can get control and take over or Runs out of power!.

MWii Failsafe cuts power and lets the plane descend when it reaches home.
And it's really easy to make a sticky Failsafe to check stick inputs.

ah yes, I see what you're saying. using built-in failsafe adds the 'auto-land/descend' feature.
I might use that in the future, but for now I'm ok with it only circling home when using AUX RTH.

I can try to put together some of the stick input code based on AP mode if it'll help speed things along. Just say the word...

rubadub · Post by **rubadub** » Tue Apr 14, 2015 6:41 am

rubadub wrote:ah yes, I see what you're saying. using built-in failsafe adds the 'auto-land/descend' feature.
I might use that in the future, but for now I'm ok with it only circling home when using AUX RTH.

just an FYI for anyone interested in running from an AUX switch while still having the auto-descend feature.
I added this to mine:

Code: Select all

//add a define to my config.h
#define RTH_DESCEND 1

//and, in GPS.cpp, make the following change to the FW_NAV() function
#if defined(FIXEDWING)
void FW_NAV(){
...
// Always DISARM when Home is within 10 meters if FC is in failsafe.
//rdub: add option to auto-descend in RTH even when not in FAILSAFE 
      if( f.FAILSAFE_RTH_ENABLE || (f.GPS_HOME_MODE && RTH_DESCEND)){
        if( GPS_distanceToHome <10 ){
          f.ARMED = 0;
          //NAV_Thro = MINTHROTTLE;
          f.CLIMBOUT_FW = 0 ;              // Abort Climbout
          GPS_hold[ALT]  =  GPS_home[ALT]+5; // Come down
        }
      }

simple fix. HTH...

bigcheese · Post by **bigcheese** » Mon May 25, 2015 10:16 pm

Hi,
have not used my hobbyking multiwii pro with gps (red one) long time because still have trouble to set it up...
nevermind, to get this thing running I have the following questions:

1) is it normal in the multiwiiconf gui, that if i roll or pitch the board, that it will automaticly levels again back (like drifting) ? or should it hold that position how I am holding the board?? what settings influence this?
2) what is the setting and the pins for flying wing?

(for this board)
3) is there anyone having exactly this board and can share the correct working sketch with me?

still trying with "FW_Nav_140402"

best regards,
bigcheese

PatrikE · Post by **PatrikE** » Wed May 27, 2015 5:19 pm

If the 3D model drifts back when the board is tilted something is wrong.
Probably sensor orientation.
If you have this board
MultiWii_PRO_Flight_Controller
You should select FFIMUv2 I think.

You can see the pinout in the chart here.
http://fotoflygarn.blogspot.fi/2012/03/ ... plane.html

FW_Nav_140402 is the latest "stable" relese for FixedWing Nav based on V2.3 code.

Plüschi · Post by **Plüschi** » Fri May 29, 2015 8:13 am

When my plane does RTH it turns in smoothly, but then oscillates a bit to the right and to the left. GPS is an MTK in nmea mode, 5hz refresh.

What do i do, lower P value, higner D value ? Are there other tricks ?

PatrikE · Post by **PatrikE** » Sun May 31, 2015 9:16 am

I have never got it to track straight either.
On the other hand i'm satisfied with getting the plane home with RTH.
Played around a bit with PID's but don't get anything clearly better.
Maybe the reaction time gets too long.

It's more noticeable when flying waypoints.
But there it seems I value may be too low.

Maybe someone with better knowledge about PID's could take a look at it.

brm · Post by **brm** » Sun May 31, 2015 8:10 pm

PatrikE wrote:I have never got it to track straight either.
On the other hand i'm satisfied with getting the plane home with RTH.
Played around a bit with PID's but don't get anything clearly better.
Maybe the reaction time gets too long.

It's more noticeable when flying waypoints.
But there it seems I value may be too low.

Maybe someone with better knowledge about PID's could take a look at it.

time you post the source code ;-)

i still hope to launch my multiplex panda next month using your sw ...

PatrikE · Post by **PatrikE** » Sun May 31, 2015 10:22 pm

Ok..
I just uploaded the code in my trunk.
https://code.google.com/p/multiwii/sour ... 150431.zip

Here is a testflight with the code.
https://youtu.be/TWfajvxNCFg
Switched to Mission at 0:10.
I only cut throttle to loose altitude for landing at 2:40
Otherwise the flight was in mission mode. No other stick inputs.

Weather here hasn't been the best for testing but maybe someone else have better climate.
Test it and check for improvements.
The PID's isn't the best.
I get the feeling it flies like a "S" between waypoints.

brm · Post by **brm** » Mon Jun 01, 2015 7:59 pm

PatrikE wrote:Ok..
I just uploaded the code in my trunk.
https://code.google.com/p/multiwii/sour ... 150431.zip

the video is nice - quite a few improvements.
is there somewhere an older naze32 port.
would be helpfull as i am not an atmel freak.
thanks!

PatrikE · Post by **PatrikE** » Mon Jun 01, 2015 8:33 pm

There's no Naze port of the GPS code from V2.4.
But the current Baseflight have RTH and PosHold integrated.
I hardly believe TC will let Waypoints be intgrared.

brm · Post by **brm** » Mon Jun 01, 2015 8:45 pm

PatrikE wrote:There's no Naze port of the GPS code from V2.4.
But the current Baseflight have RTH and PosHold integrated.
I hardly believe TC will let Waypoints be intgrared.

ok, i'll check the latest code base from tc.
didn't knew someone integrated the airplane stuff.
thanks.

Plüschi · Post by **Plüschi** » Tue Jun 02, 2015 11:40 am

PatrikE wrote:Here is a testflight with the code.
https://youtu.be/TWfajvxNCFg

Looks good. The althold is excellent !
I will copy the averaging for the d-term and the mixing of aile-elev-rud. Thats so much better than the rudder-only P-only thing i was using.

To better understand stuff i have graphed the path with D and without D term. This simulation spits out gps data over serial and expects back the FC's control term. Its a HIL (hardware in loop) thing.
Plane flies center to right side, then rth is engaged and it turns.
Seems high D does prevent overshoot, but also needs higher P since P and D counteract.

P=2, D=0

P=3, D=3

P=4 D=0 , significant overshoot

P=3 D=6 , too much of D

brm · Post by **brm** » Tue Jun 02, 2015 6:49 pm

plüschi, do you use marc's hil package for the simulation?