mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
Conflicts: ArduCopter/APM_Config.h
This commit is contained in:
Chris Anderson
commit
7b2bcaef20
@ -7,8 +7,6 @@
|
||||
|
||||
// GPS is auto-selected
|
||||
|
||||
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
|
||||
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
//#define HIL_MODE HIL_MODE_ATTITUDE
|
||||
|
||||
@ -66,8 +64,4 @@
|
||||
#define USERHOOK_VARIABLES "UserVariables.h"
|
||||
|
||||
|
||||
// enable this for the new 'APM2' hardware
|
||||
// #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// #define APM2_BETA_HARDWARE // for developers who received an early beta board with the older BMP085
|
||||
|
||||
//# define LOGGING_ENABLED DISABLED
|
||||
//# define LOGGING_ENABLED DISABLED
|
||||
@ -1,8 +1,8 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r10 alpha"
|
||||
#define THISFIRMWARE "ArduCopter V2.2"
|
||||
/*
|
||||
ArduCopter Version 2.0 Beta
|
||||
ArduCopter Version 2.2
|
||||
Authors: Jason Short
|
||||
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
|
||||
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
|
||||
@ -598,11 +598,12 @@ static boolean takeoff_complete;
|
||||
static int32_t takeoff_timer;
|
||||
// Used to see if we have landed and if we should shut our engines - not fully implemented
|
||||
static boolean land_complete = true;
|
||||
|
||||
// used to manually override throttle in interactive Alt hold modes
|
||||
static int16_t manual_boost;
|
||||
// An additional throttle added to keep the copter at the same altitude when banking
|
||||
static int16_t angle_boost;
|
||||
// Push copter down for clean landing
|
||||
static uint8_t landing_boost;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -900,6 +901,10 @@ static void fast_loop()
|
||||
// IMU DCM Algorithm
|
||||
read_AHRS();
|
||||
|
||||
if(GPS_enabled){
|
||||
update_GPS();
|
||||
}
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
update_yaw_mode();
|
||||
@ -930,10 +935,6 @@ static void medium_loop()
|
||||
case 0:
|
||||
medium_loopCounter++;
|
||||
|
||||
if(GPS_enabled){
|
||||
update_GPS();
|
||||
}
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
|
||||
if(g.compass_enabled){
|
||||
if (compass.read()) {
|
||||
@ -961,10 +962,6 @@ static void medium_loop()
|
||||
// clear nav flag
|
||||
nav_ok = false;
|
||||
|
||||
// invalidate GPS data
|
||||
// -------------------
|
||||
g_gps->new_data = false;
|
||||
|
||||
// calculate the copter's desired bearing and WP distance
|
||||
// ------------------------------------------------------
|
||||
if(navigate()){
|
||||
@ -1295,11 +1292,15 @@ static void update_GPS(void)
|
||||
}else{
|
||||
// after 12 reads we guess we may have lost GPS signal, stop navigating
|
||||
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
|
||||
nav_roll >>= 1;
|
||||
nav_pitch >>= 1;
|
||||
nav_roll = 0;
|
||||
nav_pitch = 0;
|
||||
}
|
||||
|
||||
if (g_gps->new_data && g_gps->fix) {
|
||||
|
||||
// clear new data flag
|
||||
g_gps->new_data = false;
|
||||
|
||||
gps_watchdog = 0;
|
||||
|
||||
// OK to run the nav routines
|
||||
@ -1418,16 +1419,9 @@ void update_roll_pitch_mode(void)
|
||||
update_simple_mode();
|
||||
}
|
||||
|
||||
#if WIND_COMP_STAB == 1
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
|
||||
#else
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
#endif
|
||||
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_AUTO:
|
||||
@ -1616,9 +1610,9 @@ void update_throttle_mode(void)
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping());
|
||||
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost);
|
||||
#else
|
||||
throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping();
|
||||
throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping() - landing_boost;
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -1702,6 +1696,7 @@ static void update_navigation()
|
||||
wp_control = LOITER_MODE;
|
||||
}
|
||||
|
||||
// Kick us out of loiter and begin landing if the auto_land_timer is set
|
||||
if(auto_land_timer != 0 && (millis() - auto_land_timer) > 20000){
|
||||
// just to make sure we clear the timer
|
||||
auto_land_timer = 0;
|
||||
@ -1713,10 +1708,6 @@ static void update_navigation()
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
// verify land will override WP_control if we are below
|
||||
// a certain altitude
|
||||
verify_land();
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
@ -1835,6 +1826,7 @@ static void update_altitude()
|
||||
#else
|
||||
// This is real life
|
||||
// calc the vertical accel rate
|
||||
// positive = going up.
|
||||
sonar_rate = (sonar_alt - old_sonar_alt) * 10;
|
||||
old_sonar_alt = sonar_alt;
|
||||
#endif
|
||||
@ -2006,6 +1998,7 @@ static void tuning(){
|
||||
}
|
||||
}
|
||||
|
||||
// Outputs Nav_Pitch and Nav_Roll
|
||||
static void update_nav_wp()
|
||||
{
|
||||
if(wp_control == LOITER_MODE){
|
||||
@ -2071,24 +2064,17 @@ static void update_nav_wp()
|
||||
// use error as the desired rate towards the target
|
||||
calc_nav_rate(speed);
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
//calc_nav_pitch_roll();
|
||||
calc_loiter_pitch_roll();
|
||||
|
||||
}else if(wp_control == NO_NAV_MODE){
|
||||
// calc the Iterms for Loiter based on velocity
|
||||
#if WIND_COMP_STAB == 1
|
||||
if (g_gps->ground_speed < 50)
|
||||
calc_wind_compensation();
|
||||
else
|
||||
reduce_wind_compensation();
|
||||
// clear out our nav so we can do things like land straight down
|
||||
|
||||
// rotate nav_lat, nav_lon to roll and pitch
|
||||
calc_loiter_pitch_roll();
|
||||
#else
|
||||
// clear out our nav so we can do things like land straight
|
||||
nav_pitch = 0;
|
||||
nav_roll = 0;
|
||||
#endif
|
||||
// We bring in our iterms for wind control, but we don't navigate
|
||||
nav_lon = g.pi_loiter_lon.get_integrator();
|
||||
nav_lat = g.pi_loiter_lat.get_integrator();
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_loiter_pitch_roll();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -166,6 +166,7 @@ static int16_t
|
||||
get_nav_throttle(int32_t z_error)
|
||||
{
|
||||
static int16_t old_output = 0;
|
||||
static int16_t rate_d = 0;
|
||||
|
||||
int16_t rate_error;
|
||||
int16_t output;
|
||||
@ -185,6 +186,12 @@ get_nav_throttle(int32_t z_error)
|
||||
// limit the rate - iterm is not used
|
||||
output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
|
||||
|
||||
// a positive climb rate means we're going up
|
||||
rate_d = ((rate_d + climb_rate)>>1) * 1; // replace with gain
|
||||
|
||||
// slight adjustment to alt hold output
|
||||
output -= constrain(rate_d, -25, 25);
|
||||
|
||||
// light filter of output
|
||||
output = (old_output * 3 + output) / 4;
|
||||
|
||||
@ -222,15 +229,6 @@ get_rate_yaw(int32_t target_rate)
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
}
|
||||
|
||||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
// Keeps outdated data out of our calculations
|
||||
//static void reset_hold_I(void)
|
||||
//{
|
||||
// g.pi_loiter_lat.reset_I();
|
||||
// g.pi_loiter_lon.reset_I();
|
||||
//}
|
||||
|
||||
// Keeps old data out of our calculation / logs
|
||||
static void reset_nav(void)
|
||||
{
|
||||
|
||||
@ -359,16 +359,20 @@ static bool verify_land()
|
||||
// remenber altitude for climb_rate
|
||||
old_alt = current_loc.alt;
|
||||
|
||||
if ((current_loc.alt - home.alt) < 250){
|
||||
if ((current_loc.alt - home.alt) < 200){
|
||||
// don't bank to hold position
|
||||
wp_control = NO_NAV_MODE;
|
||||
// try and come down faster
|
||||
// by setting next_WP really low.
|
||||
set_new_altitude(-1000);
|
||||
landing_boost++;
|
||||
landing_boost = min(landing_boost, 20);
|
||||
}else{
|
||||
landing_boost = 0;
|
||||
wp_control = LOITER_MODE;
|
||||
}
|
||||
|
||||
if((current_loc.alt - home.alt) < 100 && velocity_land <= 50){
|
||||
land_complete = true;
|
||||
landing_boost = 0;
|
||||
|
||||
// reset old_alt
|
||||
old_alt = 0;
|
||||
|
||||
@ -487,13 +487,6 @@
|
||||
#endif
|
||||
|
||||
|
||||
// experimental feature for
|
||||
#ifndef WIND_COMP_STAB
|
||||
# define WIND_COMP_STAB 0
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Attitude Control
|
||||
//
|
||||
@ -618,10 +611,10 @@
|
||||
// Loiter control gains
|
||||
//
|
||||
#ifndef LOITER_P
|
||||
# define LOITER_P 2.4 // was .25 in previous
|
||||
# define LOITER_P 2.0 // was .25 in previous
|
||||
#endif
|
||||
#ifndef LOITER_I
|
||||
# define LOITER_I 0.1 // Wind control
|
||||
# define LOITER_I 0.05 // Wind control
|
||||
#endif
|
||||
#ifndef LOITER_IMAX
|
||||
# define LOITER_IMAX 30 // degrees°
|
||||
|
||||
@ -46,38 +46,37 @@ static void calc_XY_velocity(){
|
||||
static int32_t last_longitude = 0;
|
||||
static int32_t last_latitude = 0;
|
||||
|
||||
static int16_t x_speed_old = 0;
|
||||
static int16_t y_speed_old = 0;
|
||||
|
||||
// y_GPS_speed positve = Up
|
||||
// x_GPS_speed positve = Right
|
||||
|
||||
// this speed is ~ in cm because we are using 10^7 numbers from GPS
|
||||
float tmp = 1.0/dTnav;
|
||||
//int8_t tmp = 5;
|
||||
//float tmp = 5;
|
||||
|
||||
int16_t x_diff = (g_gps->longitude - last_longitude) * tmp;
|
||||
int16_t y_diff = (g_gps->latitude - last_latitude) * tmp;
|
||||
// straightforward approach:
|
||||
/*
|
||||
int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp;
|
||||
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp;
|
||||
// slight averaging filter
|
||||
x_GPS_speed = (x_GPS_speed + x_estimate) >> 1;
|
||||
y_GPS_speed = (y_GPS_speed + y_estimate) >> 1;
|
||||
*/
|
||||
|
||||
// filter
|
||||
x_GPS_speed = (x_GPS_speed + x_diff) >> 1;
|
||||
y_GPS_speed = (y_GPS_speed + y_diff) >> 1;
|
||||
// Ryan Beall's forward estimator:
|
||||
int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
|
||||
int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * tmp;
|
||||
|
||||
//x_GPS_speed = x_diff;
|
||||
//y_GPS_speed = y_diff;
|
||||
x_GPS_speed = x_speed_new + (x_speed_new - x_speed_old);
|
||||
y_GPS_speed = y_speed_new + (y_speed_new - y_speed_old);
|
||||
|
||||
// Above simply works better than GPS groundspeed
|
||||
// which is proving to be problematic
|
||||
|
||||
/*if(g_gps->ground_speed > 120){
|
||||
// Derive X/Y speed from GPS
|
||||
// this is far more accurate when traveling about 1.5m/s
|
||||
float temp = g_gps->ground_course * RADX100;
|
||||
x_GPS_speed = sin(temp) * (float)g_gps->ground_speed;
|
||||
y_GPS_speed = cos(temp) * (float)g_gps->ground_speed;
|
||||
}*/
|
||||
x_speed_old = x_speed_new;
|
||||
y_speed_old = y_speed_new;
|
||||
|
||||
last_longitude = g_gps->longitude;
|
||||
last_latitude = g_gps->latitude;
|
||||
|
||||
//Serial.printf("GS: %d \tx:%d \ty:%d\n", g_gps->ground_speed, x_GPS_speed, y_GPS_speed);
|
||||
}
|
||||
|
||||
static void calc_location_error(struct Location *next_loc)
|
||||
@ -257,81 +256,6 @@ static void calc_loiter_pitch_roll()
|
||||
nav_pitch = -nav_pitch;
|
||||
}
|
||||
|
||||
#if WIND_COMP_STAB == 1
|
||||
static void calc_wind_compensation()
|
||||
{
|
||||
// this idea is a function that converts user input into I term for position hold
|
||||
// the concept is simple. The iterms always act upon flight no matter what mode were in.
|
||||
// when our velocity is 0, we call this function to update our iterms
|
||||
// otherwise we slowly reduce out iterms to 0
|
||||
|
||||
// take the pitch and roll of the copter and,
|
||||
float roll = dcm.roll_sensor;
|
||||
float pitch = -dcm.pitch_sensor; // flip pitch to make positive pitch forward
|
||||
|
||||
// rotate it to eliminate yaw of Copter
|
||||
int32_t roll_tmp = roll * sin_yaw_y - pitch * -cos_yaw_x;
|
||||
int32_t pitch_tmp = roll * -cos_yaw_x + pitch * sin_yaw_y;
|
||||
|
||||
roll_tmp = constrain(roll_tmp, -2000, 2000);
|
||||
pitch_tmp = constrain(pitch_tmp, -2000, 2000);
|
||||
|
||||
// filter the input and apply it to out integrator value
|
||||
// nav_lon and nav_lat will be applied to normal flight
|
||||
|
||||
// This filter is far too fast!!!
|
||||
//nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16;
|
||||
//nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16;
|
||||
|
||||
nav_lon = g.pi_loiter_lon.get_integrator();
|
||||
nav_lat = g.pi_loiter_lat.get_integrator();
|
||||
|
||||
// Maybe a slower filter would work?
|
||||
if(g.pi_loiter_lon.get_integrator() > roll_tmp){
|
||||
g.pi_loiter_lon.set_integrator(nav_lon - 5);
|
||||
}else if(g.pi_loiter_lon.get_integrator() < roll_tmp){
|
||||
g.pi_loiter_lon.set_integrator(nav_lon + 5);
|
||||
}
|
||||
if(g.pi_loiter_lat.get_integrator() > pitch_tmp){
|
||||
g.pi_loiter_lat.set_integrator(nav_lat - 5);
|
||||
}else if(g.pi_loiter_lat.get_integrator() < pitch_tmp){
|
||||
g.pi_loiter_lat.set_integrator(nav_lat + 5);
|
||||
}
|
||||
|
||||
// save smoothed input to integrator
|
||||
g.pi_loiter_lon.set_integrator(nav_lon); // X
|
||||
g.pi_loiter_lat.set_integrator(nav_lat); // Y
|
||||
|
||||
//Serial.printf("build wind iterm X:%d Y:%d, r:%d, p:%d\n",
|
||||
// nav_lon,
|
||||
// nav_lat,
|
||||
// nav_roll,
|
||||
// nav_pitch);
|
||||
}
|
||||
|
||||
static void reduce_wind_compensation()
|
||||
{
|
||||
//slow degradation of iterms
|
||||
float tmp;
|
||||
|
||||
tmp = g.pi_loiter_lon.get_integrator();
|
||||
tmp *= .98;
|
||||
g.pi_loiter_lon.set_integrator(tmp); // X
|
||||
|
||||
tmp = g.pi_loiter_lat.get_integrator();
|
||||
tmp *= .98;
|
||||
g.pi_loiter_lat.set_integrator(tmp); // Y
|
||||
|
||||
// debug
|
||||
//int16_t t1 = g.pi_loiter_lon.get_integrator();
|
||||
//int16_t t2 = g.pi_loiter_lon.get_integrator();
|
||||
|
||||
//Serial.printf("reduce wind iterm X:%d Y:%d \n",
|
||||
// t1,
|
||||
// t2);
|
||||
}
|
||||
#endif
|
||||
|
||||
static int16_t calc_desired_speed(int16_t max_speed)
|
||||
{
|
||||
/*
|
||||
@ -508,29 +432,22 @@ static int32_t get_new_altitude()
|
||||
}
|
||||
|
||||
int32_t diff = abs(next_WP.alt - target_altitude);
|
||||
int8_t _scale = 4;
|
||||
int8_t _scale = 3;
|
||||
|
||||
if (next_WP.alt < target_altitude){
|
||||
// we are below the target alt
|
||||
|
||||
if(diff < 200){
|
||||
// we are going up
|
||||
_scale = 5;
|
||||
} else {
|
||||
_scale = 4;
|
||||
}
|
||||
|
||||
}else {
|
||||
// we are above the target
|
||||
// stay at 16 for speed
|
||||
//_scale = 16;
|
||||
|
||||
if(diff < 400){
|
||||
// we are going down
|
||||
// we are above the target, going down
|
||||
if(diff < 600){
|
||||
_scale = 4;
|
||||
}
|
||||
if(diff < 300){
|
||||
_scale = 5;
|
||||
|
||||
}else if(diff < 100){
|
||||
_scale = 6;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -81,6 +81,7 @@ static void init_compass()
|
||||
dcm.set_compass(&compass);
|
||||
compass.init();
|
||||
compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
compass.null_offsets_enable();
|
||||
}
|
||||
|
||||
static void init_optflow()
|
||||
|
||||
@ -428,12 +428,6 @@ static void startup_ground(void)
|
||||
|
||||
static void set_mode(byte mode)
|
||||
{
|
||||
//if(control_mode == mode){
|
||||
// don't switch modes if we are already in the correct mode.
|
||||
// Serial.print("Double mode\n");
|
||||
//return;
|
||||
//}
|
||||
|
||||
// if we don't have GPS lock
|
||||
if(home_is_set == false){
|
||||
// our max mode should be
|
||||
@ -458,6 +452,9 @@ static void set_mode(byte mode)
|
||||
// clearing value used in interactive alt hold
|
||||
manual_boost = 0;
|
||||
|
||||
// clearing value used to force the copter down in landing mode
|
||||
landing_boost = 0;
|
||||
|
||||
// do not auto_land if we are leaving RTL
|
||||
auto_land_timer = 0;
|
||||
|
||||
@ -574,13 +571,6 @@ static void set_mode(byte mode)
|
||||
// We are under manual attitude control
|
||||
// removes the navigation from roll and pitch commands, but leaves the wind compensation
|
||||
reset_nav();
|
||||
|
||||
#if WIND_COMP_STAB == 1
|
||||
if(GPS_enabled){
|
||||
wp_control = NO_NAV_MODE;
|
||||
update_nav_wp();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
Log_Write_Mode(control_mode);
|
||||
|
||||
@ -22,7 +22,7 @@ static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
||||
//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_tuning(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
||||
//static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
|
||||
@ -70,7 +70,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"tune", test_tuning},
|
||||
//{"tri", test_tri},
|
||||
{"current", test_current},
|
||||
// {"relay", test_relay},
|
||||
{"relay", test_relay},
|
||||
{"wp", test_wp},
|
||||
//{"nav", test_nav},
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
@ -777,9 +777,8 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
||||
return (0);
|
||||
}
|
||||
|
||||
/*
|
||||
//static int8_t
|
||||
//test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
@ -800,7 +799,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
static int8_t
|
||||
test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
||||
@ -202,6 +202,7 @@ static void init_ardupilot()
|
||||
} else {
|
||||
dcm.set_compass(&compass);
|
||||
compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
compass.null_offsets_enable();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -586,6 +586,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
Serial.println_P(PSTR("Compass initialisation failed!"));
|
||||
return 0;
|
||||
}
|
||||
compass.null_offsets_enable();
|
||||
dcm.set_compass(&compass);
|
||||
report_compass();
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Quad</name>
|
||||
<name>ArduCopter V2.2 Quad</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG QUAD_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
@ -67,7 +67,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Tri</name>
|
||||
<name>ArduCopter V2.2 Tri</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG TRI_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
@ -79,7 +79,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Hexa</name>
|
||||
<name>ArduCopter V2.2 Hexa</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG HEXA_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
@ -91,7 +91,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Y6</name>
|
||||
<name>ArduCopter V2.2 Y6</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG Y6_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
@ -103,7 +103,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Octav</name>
|
||||
<name>ArduCopter V2.2 Octav</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG OCTA_FRAME
|
||||
#define FRAME_ORIENTATION V_FRAME
|
||||
@ -115,7 +115,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Octa</name>
|
||||
<name>ArduCopter V2.2 Octa</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG OCTA_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
@ -175,7 +175,7 @@
|
||||
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex</url>
|
||||
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex</url2560>
|
||||
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex</url2560-2>
|
||||
<name>ArduCopter V2.1.0 Alpha Quad Hil</name>
|
||||
<name>ArduCopter V2.2 Quad Hil</name>
|
||||
<desc>
|
||||
#define FRAME_CONFIG QUAD_FRAME
|
||||
#define FRAME_ORIENTATION PLUS_FRAME
|
||||
|
||||
@ -11,6 +11,7 @@ Compass::Compass(AP_Var::Key key) :
|
||||
_offset (&_group, 1),
|
||||
_declination (&_group, 2, 0.0, PSTR("DEC")),
|
||||
_null_init_done(false),
|
||||
_null_enable(false),
|
||||
product_id(AP_COMPASS_TYPE_UNKNOWN)
|
||||
{
|
||||
// Default the orientation matrix to none - will be overridden at group load time
|
||||
@ -146,6 +147,8 @@ Compass::null_offsets(const Matrix3f &dcm_matrix)
|
||||
float weight;
|
||||
|
||||
Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
|
||||
|
||||
if(_null_enable == false) return;
|
||||
|
||||
if(_null_init_done) {
|
||||
dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is.
|
||||
@ -166,3 +169,18 @@ Compass::null_offsets(const Matrix3f &dcm_matrix)
|
||||
_last_dcm_matrix = dcm_matrix;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Compass::null_offsets_enable(void)
|
||||
{
|
||||
_null_init_done = false;
|
||||
_null_enable = true;
|
||||
}
|
||||
|
||||
void
|
||||
Compass::null_offsets_disable(void)
|
||||
{
|
||||
_null_init_done = false;
|
||||
_null_enable = false;
|
||||
}
|
||||
@ -41,7 +41,7 @@ public:
|
||||
float heading_x; ///< compass vector X magnitude
|
||||
float heading_y; ///< compass vector Y magnitude
|
||||
unsigned long last_update; ///< millis() time of last update
|
||||
bool healthy; ///< true if last read OK
|
||||
bool healthy; ///< true if last read OK
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
@ -117,6 +117,16 @@ public:
|
||||
void null_offsets(const Matrix3f &dcm_matrix);
|
||||
|
||||
|
||||
/// Enable/Start automatic offset updates
|
||||
///
|
||||
void null_offsets_enable(void);
|
||||
|
||||
|
||||
/// Disable/Stop automatic offset updates
|
||||
///
|
||||
void null_offsets_disable(void);
|
||||
|
||||
|
||||
/// Sets the local magnetic field declination.
|
||||
///
|
||||
/// @param radians Local field declination.
|
||||
@ -130,6 +140,7 @@ protected:
|
||||
AP_VarS<Vector3f> _offset;
|
||||
AP_Float _declination;
|
||||
|
||||
bool _null_enable; ///< enabled flag for offset nulling
|
||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
|
||||
Vector3f _mag_body_last; ///< ?? used by offset nulling
|
||||
|
||||
@ -195,6 +195,9 @@ AP_DCM::accel_adjust(void)
|
||||
void
|
||||
AP_DCM::matrix_reset(void)
|
||||
{
|
||||
if (_compass != NULL) {
|
||||
_compass->null_offsets_disable();
|
||||
}
|
||||
_dcm_matrix.a.x = 1.0f;
|
||||
_dcm_matrix.a.y = 0.0f;
|
||||
_dcm_matrix.a.z = 0.0f;
|
||||
@ -207,6 +210,11 @@ AP_DCM::matrix_reset(void)
|
||||
_omega_I.x = 0.0f;
|
||||
_omega_I.y = 0.0f;
|
||||
_omega_I.z = 0.0f;
|
||||
if (_compass != NULL) {
|
||||
_compass->null_offsets_enable(); // This call is needed to restart the nulling
|
||||
// Otherwise the reset in the DCM matrix can mess up
|
||||
// the nulling
|
||||
}
|
||||
}
|
||||
|
||||
/*************************************************
|
||||
|
||||
Loading…
Reference in New Issue
Block a user