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.
//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);
}
}
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);
}
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?
PatrikE wrote:Does anyone have the formula room calculate longitude scaling available?
That seems.to be the last piece missing now.
/*
* 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
#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
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.
As i understand you use / prefer GPS bearing instead of mag for RTH.
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.
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.
rubadub wrote:just fuse mag w/ gyro, and then fuse GPS bearing w/ mag+gyro
Plüschi wrote:gyros will be sensitive enough to pickup slow, constant rotations
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?
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.
myheadingdelta = EstG.V.Y * deltaGyroAngle[0] -
EstG.V.X * deltaGyroAngle[1] +
EstG.V.Z * deltaGyroAngle[2];
myheading += myheadingdelta * 0.01391;// 57/4096
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;
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.
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.
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.
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.
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.
//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
}
}
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.
PatrikE wrote:Ok..
I just uploaded the code in my trunk.
https://code.google.com/p/multiwii/sour ... 150431.zip
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.
Users browsing this forum: No registered users and 4 guests