aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: incorporate new version of inertial navigation

Browse Source 
Moved several navigation functions from ArduCopter.pde to navigation.pde
This commit is contained in:
rmackay9 2012-11-07 19:03:30 +09:00
parent 1c7118a2d1
commit 1f801714e8
13 changed files with 502 additions and 637 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

296
ArduCopter/ArduCopter.pde
View File

@ -105,6 +105,8 @@
#include <AP_Camera.h> // Photo or video camera #include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera/Antenna mount #include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed.h> // needed for AHRS build #include <AP_Airspeed.h> // needed for AHRS build
#include <AP_InertialNav3D.h> // ArduPilot Mega inertial navigation library
#include <ThirdOrderCompFilter3D.h> // Complementary filter for combining barometer altitude with accelerometers
#include <memcheck.h> #include <memcheck.h>
// Configuration // Configuration
@ -371,10 +373,10 @@ AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
//Documentation of GLobals: //Documentation of GLobals:
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// The GPS based velocity calculated by offsetting the Latitude and Longitude // velocity in lon and lat directions calculated from GPS position and accelerometer data
// updated after GPS read - 5-10hz // updated after GPS read - 5-10hz
static int16_t x_actual_speed; static int16_t lon_speed; // expressed in cm/s. positive numbers mean moving east
static int16_t y_actual_speed; static int16_t lat_speed; // expressed in cm/s. positive numbers when moving north
// The difference between the desired rate of travel and the actual rate of travel // The difference between the desired rate of travel and the actual rate of travel
@ -505,7 +507,7 @@ static float scaleLongDown = 1;
// Used by Mavlink for unknow reasons // Used by Mavlink for unknow reasons
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
// Used by Mavlink for unknow reasons // Used by Mavlink for unknow reasons
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.80665; // meters/ sec^2
// Unions for getting byte values // Unions for getting byte values
union float_int { union float_int {
@ -517,9 +519,6 @@ union float_int {
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Location & Navigation // Location & Navigation
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Status flag indicating we have data that can be used to navigate
// Set by a GPS read with 3D fix, or an optical flow read
static bool nav_ok;
// This is the angle from the copter to the "next_WP" location in degrees * 100 // This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing; static int32_t target_bearing;
// Status of the Waypoint tracking mode. Options include: // Status of the Waypoint tracking mode. Options include:
@ -730,8 +729,6 @@ static struct Location home;
static boolean home_is_set; static boolean home_is_set;
// Current location of the copter // Current location of the copter
static struct Location current_loc; static struct Location current_loc;
// lead filtered loc
static struct Location filtered_loc;
// Next WP is the desired location of the copter - the next waypoint or loiter location // Next WP is the desired location of the copter - the next waypoint or loiter location
static struct Location next_WP; static struct Location next_WP;
// Prev WP is used to get the optimum path from one WP to the next // Prev WP is used to get the optimum path from one WP to the next
@ -860,21 +857,7 @@ static float G_Dt = 0.02;
// Inertial Navigation // Inertial Navigation
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
// The rotated accelerometer values AP_InertialNav3D inertial_nav(&ahrs, &ins, &barometer, &g_gps);
static Vector3f accels_velocity;
static Vector3f accels_position;
// accels rotated to world frame
static Vector3f accels_rotated;
//static Vector3f position_error;
// error correction
static Vector3f speed_error;
// Manage accel drift
//static float z_offset;
//static Vector3f accels_scale;
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -884,9 +867,6 @@ static Vector3f speed_error;
static int16_t perf_mon_counter; static int16_t perf_mon_counter;
// The number of GPS fixes we have had // The number of GPS fixes we have had
static int16_t gps_fix_count; static int16_t gps_fix_count;
// gps_watchdog checks for bad reads and if we miss 12 in a row, we stop navigating
// by lowering nav_lat and navlon to 0 gradually
static byte gps_watchdog;
// System Timers // System Timers
// -------------- // --------------
@ -902,8 +882,6 @@ static byte slow_loopCounter;
static byte counter_one_herz; static byte counter_one_herz;
// Counter of main loop executions. Used for performance monitoring and failsafe processing // Counter of main loop executions. Used for performance monitoring and failsafe processing
static uint16_t mainLoop_count; static uint16_t mainLoop_count;
// used to track the elapsed time between GPS reads
static uint32_t nav_loopTimer;
// Delta Time in milliseconds for navigation computations, updated with every good GPS read // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static float dTnav; static float dTnav;
// Counters for branching from 4 minute control loop used to save Compass offsets // Counters for branching from 4 minute control loop used to save Compass offsets
@ -998,7 +976,7 @@ void loop()
// Execute the fast loop // Execute the fast loop
// --------------------- // ---------------------
fast_loop();//// fast_loop();
// run the 50hz loop 1/2 the time // run the 50hz loop 1/2 the time
run_50hz_loop = !run_50hz_loop; run_50hz_loop = !run_50hz_loop;
@ -1015,18 +993,13 @@ void loop()
// port manipulation for external timing of main loops // port manipulation for external timing of main loops
//PORTK |= B01000000; //PORTK |= B01000000;
// reads all of the necessary trig functions for cameras, throttle, etc.
// --------------------------------------------------------------------
update_trig();
// Rotate the Nav_lon and nav_lat vectors based on Yaw
// ---------------------------------------------------
calc_loiter_pitch_roll();
// check for new GPS messages // check for new GPS messages
// -------------------------- // --------------------------
update_GPS(); update_GPS();
// run navigation routines
update_navigation();
// perform 10hz tasks // perform 10hz tasks
// ------------------ // ------------------
medium_loop(); medium_loop();
@ -1098,11 +1071,13 @@ static void fast_loop()
// -------------------- // --------------------
read_AHRS(); read_AHRS();
// reads all of the necessary trig functions for cameras, throttle, etc.
// --------------------------------------------------------------------
update_trig();
// Inertial Nav // Inertial Nav
// -------------------- // --------------------
#if INERTIAL_NAV == ENABLED read_inertia();
calc_inertia();
#endif
// optical flow // optical flow
// -------------------- // --------------------
@ -1159,23 +1134,6 @@ static void medium_loop()
case 1: case 1:
medium_loopCounter++; medium_loopCounter++;
// calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
if(nav_ok) {
// clear nav flag
nav_ok = false;
// calculate distance, angles to target
navigate();
// update flight control system
update_navigation();
// update log
if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()) {
Log_Write_Nav_Tuning();
}
}
break; break;
// command processing // command processing
@ -1492,15 +1450,6 @@ static void update_GPS(void)
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
if (gps_watchdog < 30) {
gps_watchdog++;
}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
auto_roll >>= 1;
auto_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) { if (g_gps->new_data && g_gps->fix) {
// clear new data flag // clear new data flag
g_gps->new_data = false; g_gps->new_data = false;
@ -1508,27 +1457,10 @@ static void update_GPS(void)
// check for duiplicate GPS messages // check for duiplicate GPS messages
if(last_gps_time != g_gps->time) { if(last_gps_time != g_gps->time) {
// look for broken GPS
// ---------------
gps_watchdog = 0;
// OK to run the nav routines
// ---------------
nav_ok = true;
// for performance monitoring // for performance monitoring
// -------------------------- // --------------------------
gps_fix_count++; gps_fix_count++;
// used to calculate speed in X and Y, iterms
// ------------------------------------------
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
// --------------------------
dTnav = min(dTnav, 1.0);
if(ground_start_count > 1) { if(ground_start_count > 1) {
ground_start_count--; ground_start_count--;
@ -1551,11 +1483,6 @@ static void update_GPS(void)
} }
} }
current_loc.lng = g_gps->longitude; // Lon * 10^7
current_loc.lat = g_gps->latitude; // Lat * 10^7
calc_XY_velocity();
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) { if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
Log_Write_GPS(); Log_Write_GPS();
} }
@ -1880,181 +1807,6 @@ void update_throttle_mode(void)
} }
} }
// called after a GPS read
static void update_navigation()
{
// wp_distance is in CM
// --------------------
switch(control_mode) {
case AUTO:
// note: wp_control is handled by commands_logic
verify_commands();
// calculates desired Yaw
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case GUIDED:
wp_control = WP_MODE;
// check if we are close to point > loiter
wp_verify_byte = 0;
verify_nav_wp();
if (wp_control == WP_MODE) {
update_auto_yaw();
} else {
set_mode(LOITER);
}
update_nav_wp();
break;
case RTL:
// have we reached the desired Altitude?
if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt
if(rtl_reached_alt == false) {
rtl_reached_alt = true;
do_RTL();
}
wp_control = WP_MODE;
// checks if we have made it to home
update_nav_RTL();
} else{
// we need to loiter until we are ready to come home
wp_control = LOITER_MODE;
}
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
#endif
// calculates the desired Roll and Pitch
update_nav_wp();
break;
// switch passthrough to LOITER
case LOITER:
case POSITION:
// This feature allows us to reposition the quad when the user lets
// go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500)
loiter_override = true;
}
// Allow the user to take control temporarily,
if(loiter_override) {
// this sets the copter to not try and nav while we control it
wp_control = NO_NAV_MODE;
// reset LOITER to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
loiter_override = false;
wp_control = LOITER_MODE;
}
}else{
wp_control = LOITER_MODE;
}
if(loiter_timer != 0) {
// If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach
if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()) {
// just to make sure we clear the timer
loiter_timer = 0;
if(g.rtl_approach_alt == 0) {
set_mode(LAND);
if(home_distance < 300) {
next_WP.lat = home.lat;
next_WP.lng = home.lng;
}
}else{
if(g.rtl_approach_alt < current_loc.alt) {
set_new_altitude(g.rtl_approach_alt);
}
}
}
}
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case LAND:
if(g.sonar_enabled)
verify_land_sonar();
else
verify_land_baro();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case CIRCLE:
wp_control = CIRCLE_MODE;
// calculates desired Yaw
update_auto_yaw();
update_nav_wp();
break;
case STABILIZE:
case TOY_A:
case TOY_M:
wp_control = NO_NAV_MODE;
update_nav_wp();
break;
}
// are we in SIMPLE mode?
if(do_simple && g.super_simple) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
//Serial.printf("ISB: %d\n", initial_simple_bearing);
}
}
if(yaw_mode == YAW_LOOK_AT_HOME) {
if(home_is_set) {
nav_yaw = get_bearing_cd(&current_loc, &home);
} else {
nav_yaw = 0;
}
}
}
static void update_nav_RTL()
{
// Have we have reached Home?
if(wp_distance <= 200 || check_missed_wp()) {
// if loiter_timer value > 0, we are set to trigger auto_land or approach
set_mode(LOITER);
// just in case we arrive and we aren't at the lower RTL alt yet.
set_new_altitude(get_RTL_alt());
// force loitering above home
next_WP.lat = home.lat;
next_WP.lng = home.lng;
// If failsafe OR auto approach altitude is set
// we will go into automatic land, (g.rtl_approach_alt) is the lowest point
// -1 means disable feature
if(failsafe || g.rtl_approach_alt >= 0)
loiter_timer = millis();
else
loiter_timer = 0;
}
}
static void read_AHRS(void) static void read_AHRS(void)
{ {
// Perform IMU calculations and get attitude info // Perform IMU calculations and get attitude info
@ -2120,10 +1872,6 @@ static void update_altitude()
#else #else
// This is real life // This is real life
#if INERTIAL_NAV == ENABLED
baro_rate = accels_velocity.z;
#else
// read in Actual Baro Altitude // read in Actual Baro Altitude
baro_alt = read_barometer(); baro_alt = read_barometer();
@ -2141,7 +1889,6 @@ static void update_altitude()
baro_rate = (temp + baro_rate) >> 1; baro_rate = (temp + baro_rate) >> 1;
baro_rate = constrain(baro_rate, -300, 300); baro_rate = constrain(baro_rate, -300, 300);
*/ */
#endif
// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
#endif #endif
@ -2198,11 +1945,6 @@ static void update_altitude()
// calc error // calc error
climb_rate_error = (climb_rate_actual - climb_rate) / 5; climb_rate_error = (climb_rate_actual - climb_rate) / 5;
#if INERTIAL_NAV == ENABLED
// inertial_nav
z_error_correction();
#endif
} }
static void update_altitude_est() static void update_altitude_est()
@ -2366,6 +2108,12 @@ static void tuning(){
break; break;
#endif #endif
#if INERTIAL_NAV == ENABLED
case CH6_INAV_TC:
inertial_nav.set_time_constant_xy(tuning_value);
inertial_nav.set_time_constant_z(tuning_value);
break;
#endif
} }
} }

137
ArduCopter/Attitude.pde
View File

@ -478,7 +478,7 @@ get_throttle_rate(int16_t z_target_speed)
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
z_rate_error = z_target_speed - accels_velocity.z; // calc the speed error z_rate_error = z_target_speed - inertial_nav._velocity.z; // calc the speed error
#else #else
z_rate_error = z_target_speed - climb_rate; // calc the speed error z_rate_error = z_target_speed - climb_rate; // calc the speed error
#endif #endif
@ -629,141 +629,6 @@ static int16_t heli_get_angle_boost(int16_t throttle)
} }
#endif // HELI_FRAME #endif // HELI_FRAME
#define NUM_G_SAMPLES 40
#if ACCEL_ALT_HOLD == 2
// z -14.4306 = going up
// z -6.4306 = going down
static int16_t get_z_damping()
{
int16_t output;
Z_integrator += get_world_Z_accel() - Z_offset;
output = Z_integrator * 3;
Z_integrator = Z_integrator * .8;
output = constrain(output, -100, 100);
return output;
}
float get_world_Z_accel()
{
accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
//Serial.printf("z %1.4f\n", accels_rot.z);
return accels_rot.z;
}
static void init_z_damper()
{
Z_offset = 0;
for (int16_t i = 0; i < NUM_G_SAMPLES; i++) {
delay(5);
read_AHRS();
Z_offset += get_world_Z_accel();
}
Z_offset /= (float)NUM_G_SAMPLES;
}
// Accelerometer Z dampening by Aurelio R. Ramos
// ---------------------------------------------
#elif ACCEL_ALT_HOLD == 1
// contains G and any other DC offset
static float estimatedAccelOffset = 0;
// state
static float synVelo = 0;
static float synPos = 0;
static float synPosFiltered = 0;
static float posError = 0;
static float prevSensedPos = 0;
// tuning for dead reckoning
static const float dt_50hz = 0.02;
static float synPosP = 10 * dt_50hz;
static float synPosI = 15 * dt_50hz;
static float synVeloP = 1.5 * dt_50hz;
static float maxVeloCorrection = 5 * dt_50hz;
static float maxSensedVelo = 1;
static float synPosFilter = 0.5;
// Z damping term.
static float fullDampP = 0.100;
float get_world_Z_accel()
{
accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
return accels_rot.z;
}
static void init_z_damper()
{
estimatedAccelOffset = 0;
for (int16_t i = 0; i < NUM_G_SAMPLES; i++) {
delay(5);
read_AHRS();
estimatedAccelOffset += get_world_Z_accel();
}
estimatedAccelOffset /= (float)NUM_G_SAMPLES;
}
float dead_reckon_Z(float sensedPos, float sensedAccel)
{
// the following algorithm synthesizes position and velocity from
// a noisy altitude and accelerometer data.
// synthesize uncorrected velocity by integrating acceleration
synVelo += (sensedAccel - estimatedAccelOffset) * dt_50hz;
// synthesize uncorrected position by integrating uncorrected velocity
synPos += synVelo * dt_50hz;
// filter synPos, the better this filter matches the filtering and dead time
// of the sensed position, the less the position estimate will lag.
synPosFiltered = synPosFiltered * (1 - synPosFilter) + synPos * synPosFilter;
// calculate error against sensor position
posError = sensedPos - synPosFiltered;
// correct altitude
synPos += synPosP * posError;
// correct integrated velocity by posError
synVelo = synVelo + constrain(posError, -maxVeloCorrection, maxVeloCorrection) * synPosI;
// correct integrated velocity by the sensed position delta in a small proportion
// (i.e., the low frequency of the delta)
float sensedVelo = (sensedPos - prevSensedPos) / dt_50hz;
synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
prevSensedPos = sensedPos;
return synVelo;
}
static int16_t get_z_damping()
{
float sensedAccel = get_world_Z_accel();
float sensedPos = current_loc.alt / 100.0;
float synVelo = dead_reckon_Z(sensedPos, sensedAccel);
return constrain(fullDampP * synVelo * (-1), -300, 300);
}
#else
static int16_t get_z_damping()
{
return 0;
}
static void init_z_damper()
{
}
#endif
// calculate modified roll/pitch depending upon optical flow calculated position // calculate modified roll/pitch depending upon optical flow calculated position
static int32_t static int32_t
get_of_roll(int32_t input_roll) get_of_roll(int32_t input_roll)

140
ArduCopter/Log.pde
View File

@ -87,6 +87,7 @@ print_log_menu(void)
if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW"));
if (g.log_bitmask & MASK_LOG_PID) Serial.printf_P(PSTR(" PID")); if (g.log_bitmask & MASK_LOG_PID) Serial.printf_P(PSTR(" PID"));
if (g.log_bitmask & MASK_LOG_ITERM) Serial.printf_P(PSTR(" ITERM")); if (g.log_bitmask & MASK_LOG_ITERM) Serial.printf_P(PSTR(" ITERM"));
if (g.log_bitmask & MASK_LOG_INAV) Serial.printf_P(PSTR(" INAV"));
} }
Serial.println(); Serial.println();
@ -203,6 +204,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(OPTFLOW); TARG(OPTFLOW);
TARG(PID); TARG(PID);
TARG(ITERM); TARG(ITERM);
TARG(INAV);
#undef TARG #undef TARG
} }
@ -289,61 +291,6 @@ static void Log_Read_GPS()
(long)temp8); // 8 ground course (long)temp8); // 8 ground course
} }
#if INERTIAL_NAV == ENABLED
static void Log_Write_Raw()
{
Vector3f gyro = ins.get_gyro();
Vector3f accel = ins.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
DataFlash.WriteLong(get_int(accels_velocity.x));
DataFlash.WriteInt(x_actual_speed);
DataFlash.WriteLong(get_int(accels_velocity.y));
DataFlash.WriteInt(y_actual_speed);
DataFlash.WriteLong(get_int(accels_velocity.z));
DataFlash.WriteInt(climb_rate_actual);
//DataFlash.WriteLong(get_int(accel.x));
//DataFlash.WriteLong(get_int(accel.y));
//DataFlash.WriteLong(get_int(accel.z));
DataFlash.WriteByte(END_BYTE);
}
// Read a raw accel/gyro packet
static void Log_Read_Raw()
{
/*
* float logvar;
* Serial.printf_P(PSTR("RAW,"));
* for (int16_t y = 0; y < 9; y++) {
* logvar = get_float(DataFlash.ReadLong());
* Serial.print(logvar);
* Serial.print(", ");
* }
* Serial.println(" ");
*/
float vx = get_float(DataFlash.ReadLong());
int16_t sx = DataFlash.ReadInt();
float vy = get_float(DataFlash.ReadLong());
int16_t sy = DataFlash.ReadInt();
float vz = get_float(DataFlash.ReadLong());
int16_t sz = DataFlash.ReadInt();
Serial.printf_P(PSTR("RAW, %1.4f, %d, %1.4f, %d, %1.4f, %d\n"),
vx,
(int)sx,
vy,
(int)sy,
vz,
(int)sz);
}
#else
static void Log_Write_Raw() static void Log_Write_Raw()
{ {
Vector3f gyro = ins.get_gyro(); Vector3f gyro = ins.get_gyro();
@ -395,7 +342,6 @@ static void Log_Read_Raw()
temp2); temp2);
*/ */
} }
#endif
// Write an Current data packet. Total length : 16 bytes // Write an Current data packet. Total length : 16 bytes
@ -622,8 +568,8 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt(nav_pitch); // 5 DataFlash.WriteInt(nav_pitch); // 5
DataFlash.WriteInt(nav_roll); // 6 DataFlash.WriteInt(nav_roll); // 6
DataFlash.WriteInt(x_actual_speed); // 7 DataFlash.WriteInt(lon_speed); // 7
DataFlash.WriteInt(y_actual_speed); // 8 DataFlash.WriteInt(lat_speed); // 8
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -832,6 +778,80 @@ static void Log_Read_Attitude()
(unsigned)temp7); (unsigned)temp7);
} }
// Write an INAV packet. Total length : 36 Bytes
static void Log_Write_INAV(float delta_t)
{
#if INERTIAL_NAV == ENABLED
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INAV_MSG);
DataFlash.WriteInt((int16_t)baro_alt); // 1 barometer altitude
DataFlash.WriteInt((int16_t)inertial_nav._position.z); // 2 accel + baro filtered altitude
DataFlash.WriteInt((int16_t)climb_rate_actual); // 3 barometer based climb rate
DataFlash.WriteInt((int16_t)inertial_nav._velocity.z); // 4 accel + baro based climb rate
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.x)); // 5 accel correction x-axis
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.y)); // 6 accel correction y-axis
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.z)); // 7 accel correction z-axis
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D.comp_k1o_ef.z));// 8 accel correction earth frame
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.x)); // 9 accel earth frame x-axis
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.y)); // 10 accel earth frame y-axis
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.z)); // 11 accel earth frame z-axis
DataFlash.WriteLong(get_int(delta_t)); // 12 time delta of samples
DataFlash.WriteLong(g_gps->latitude-home.lat); // 13 lat from home
DataFlash.WriteLong(g_gps->longitude-home.lng); // 14 lon from home
DataFlash.WriteLong(get_int(inertial_nav.get_latitude_diff())); // 15 accel based lat from home
DataFlash.WriteLong(get_int(inertial_nav.get_longitude_diff())); // 16 accel based lon from home
DataFlash.WriteLong(get_int(inertial_nav.get_latitude_velocity())); // 17 accel based lat velocity
DataFlash.WriteLong(get_int(inertial_nav.get_longitude_velocity())); // 18 accel based lon velocity
DataFlash.WriteByte(END_BYTE);
#endif
}
// Read an INAV packet
static void Log_Read_INAV()
{
int16_t temp1 = DataFlash.ReadInt(); // 1 barometer altitude
int16_t temp2 = DataFlash.ReadInt(); // 2 accel + baro filtered altitude
int16_t temp3 = DataFlash.ReadInt(); // 3 barometer based climb rate
int16_t temp4 = DataFlash.ReadInt(); // 4 accel + baro based climb rate
float temp5 = get_float(DataFlash.ReadLong()); // 5 accel correction x-axis
float temp6 = get_float(DataFlash.ReadLong()); // 6 accel correction y-axis
float temp7 = get_float(DataFlash.ReadLong()); // 7 accel correction z-axis
float temp8 = get_float(DataFlash.ReadLong()); // 8 accel correction earth frame
float temp9 = get_float(DataFlash.ReadLong()); // 9 accel earth frame x-axis
float temp10 = get_float(DataFlash.ReadLong()); // 10 accel earth frame y-axis
float temp11 = get_float(DataFlash.ReadLong()); // 11 accel earth frame z-axis
float temp12 = get_float(DataFlash.ReadLong()); // 12 time delta of samples
int32_t temp13 = DataFlash.ReadLong(); // 13 lat from home
int32_t temp14 = DataFlash.ReadLong(); // 14 lon from home
float temp15 = get_float(DataFlash.ReadLong()); // 15 accel based lat from home
float temp16 = get_float(DataFlash.ReadLong()); // 16 accel based lon from home
float temp17 = get_float(DataFlash.ReadLong()); // 17 accel based lat velocity
float temp18 = get_float(DataFlash.ReadLong()); // 18 accel based lon velocity
// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
Serial.printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
(int)temp1,
(int)temp2,
(int)temp3,
(int)temp4,
temp5,
temp6,
temp7,
temp8,
temp9,
temp10,
temp11,
temp12,
temp13,
temp14,
temp15,
temp16,
temp17,
temp18);
}
// Write a mode packet. Total length : 7 bytes // Write a mode packet. Total length : 7 bytes
static void Log_Write_Mode(byte mode) static void Log_Write_Mode(byte mode)
{ {
@ -1112,6 +1132,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
case LOG_DMP_MSG: case LOG_DMP_MSG:
Log_Read_DMP(); Log_Read_DMP();
break; break;
case LOG_INAV_MSG:
Log_Read_INAV();
break;
} }
break; break;
case 3: case 3:

5
ArduCopter/Parameters.h
View File

@ -88,6 +88,11 @@ public:
// //
k_param_motors = 90, k_param_motors = 90,
//
// 100: Inertial Nav
//
k_param_inertial_nav = 100,
// 110: Telemetry control // 110: Telemetry control
// //
k_param_gcs0 = 110, k_param_gcs0 = 110,

6
ArduCopter/Parameters.pde
View File

@ -368,6 +368,12 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(ins, "INS_", AP_InertialSensor), GOBJECT(ins, "INS_", AP_InertialSensor),
#endif #endif
#if INERTIAL_NAV == ENABLED
// @Group: INAV_
// @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp
GOBJECT(inertial_nav, "INAV_", AP_InertialNav3D),
#endif
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK),

9
ArduCopter/commands.pde
View File

@ -140,7 +140,7 @@ static void set_next_WP(struct Location *wp)
if (next_WP.lat == 0 || command_nav_index <= 1) { if (next_WP.lat == 0 || command_nav_index <= 1) {
prev_WP = current_loc; prev_WP = current_loc;
}else{ }else{
if (get_distance_cm(&filtered_loc, &next_WP) < 500) if (get_distance_cm(&current_loc, &next_WP) < 500)
prev_WP = next_WP; prev_WP = next_WP;
else else
prev_WP = current_loc; prev_WP = current_loc;
@ -168,7 +168,7 @@ static void set_next_WP(struct Location *wp)
// this is handy for the groundstation // this is handy for the groundstation
// ----------------------------------- // -----------------------------------
wp_distance = get_distance_cm(&filtered_loc, &next_WP); wp_distance = get_distance_cm(&current_loc, &next_WP);
target_bearing = get_bearing_cd(&prev_WP, &next_WP); target_bearing = get_bearing_cd(&prev_WP, &next_WP);
// calc the location error: // calc the location error:
@ -199,6 +199,11 @@ static void init_home()
set_cmd_with_index(home, 0); set_cmd_with_index(home, 0);
//print_wp(&home, 0); //print_wp(&home, 0);
#if INERTIAL_NAV == ENABLED
// set inertial nav's home position
inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude);
#endif
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(0, &home); Log_Write_Cmd(0, &home);

12
ArduCopter/config.h
View File

@ -968,6 +968,9 @@
#ifndef LOG_ITERM #ifndef LOG_ITERM
# define LOG_ITERM ENABLED # define LOG_ITERM ENABLED
#endif #endif
#ifndef LOG_INAV
# define LOG_INAV DISABLED
#endif
// calculate the default log_bitmask // calculate the default log_bitmask
#define LOGBIT(_s) (LOG_ ## _s ? MASK_LOG_ ## _s : 0) #define LOGBIT(_s) (LOG_ ## _s ? MASK_LOG_ ## _s : 0)
@ -985,7 +988,9 @@
LOGBIT(CUR) | \ LOGBIT(CUR) | \
LOGBIT(MOTORS) | \ LOGBIT(MOTORS) | \
LOGBIT(OPTFLOW) | \ LOGBIT(OPTFLOW) | \
LOGBIT(PID) LOGBIT(PID) | \
LOGBIT(ITERM) | \
LOGBIT(INAV)
// if we are using fast, Disable Medium // if we are using fast, Disable Medium
//#if LOG_ATTITUDE_FAST == ENABLED //#if LOG_ATTITUDE_FAST == ENABLED
@ -1087,7 +1092,8 @@
#endif #endif
// Inertia based contollers. disabled by default, work in progress // Inertia based contollers. disabled by default, work in progress
#define ACCEL_ALT_HOLD 0 #ifndef INERTIAL_NAV
#define INERTIAL_NAV DISABLED # define INERTIAL_NAV DISABLED
#endif
#endif // __ARDUCOPTER_CONFIG_H__ #endif // __ARDUCOPTER_CONFIG_H__

4
ArduCopter/defines.h
View File

@ -192,6 +192,8 @@
#define CH6_AHRS_YAW_KP 30 #define CH6_AHRS_YAW_KP 30
#define CH6_AHRS_KP 31 #define CH6_AHRS_KP 31
// Inertial Nav
#define CH6_INAV_TC 32
// nav byte mask // nav byte mask
// ------------- // -------------
@ -297,6 +299,7 @@ enum gcs_severity {
#define LOG_PID_MSG 0x0E #define LOG_PID_MSG 0x0E
#define LOG_ITERM_MSG 0x0F #define LOG_ITERM_MSG 0x0F
#define LOG_DMP_MSG 0x10 #define LOG_DMP_MSG 0x10
#define LOG_INAV_MSG 0x11
#define LOG_INDEX_MSG 0xF0 #define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50 #define MAX_NUM_LOGS 50
@ -314,6 +317,7 @@ enum gcs_severity {
#define MASK_LOG_OPTFLOW (1<<11) #define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12) #define MASK_LOG_PID (1<<12)
#define MASK_LOG_ITERM (1<<13) #define MASK_LOG_ITERM (1<<13)
#define MASK_LOG_INAV (1<<14)
// Waypoint Modes // Waypoint Modes

102
ArduCopter/inertia.pde
View File

@ -1,88 +1,20 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// read_inertia - read inertia in from accelerometers
static void read_inertia()
{
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
static uint8_t log_counter_inav = 0;
// generates a new location and velocity in space based on inertia // inertial altitude estimates
// Calc 100 hz inertial_nav.update(G_Dt);
void calc_inertia()
{
// rotate accels based on DCM
// --------------------------
accels_rotated = ahrs.get_dcm_matrix() * ins.get_accel();
//accels_rotated += accels_offset; // skew accels to account for long term error using calibration
accels_rotated.z += 9.805; // remove influence of gravity
// rising = 2 if( motors.armed() && g.log_bitmask & MASK_LOG_INAV ) {
// neutral = 0 log_counter_inav++;
// falling = -2 if( log_counter_inav >= 10 ) {
log_counter_inav = 0;
// ACC Y POS = going EAST Log_Write_INAV(G_Dt);
// ACC X POS = going North }
// ACC Z POS = going DOWN (lets flip this) }
#endif
// Integrate accels to get the velocity }
// ------------------------------------
Vector3f temp = accels_rotated * (G_Dt * 100);
temp.z = -temp.z; // Temp is changed to world frame and we can use it normaly
accels_velocity += temp;
// Integrate velocity to get the Position
// ------------------------------------
accels_position += accels_velocity * G_Dt;
/*
* current_loc.lng += accels_velocity.x * G_Dt;
* current_loc.lat += accels_velocity.y * G_Dt;
* current_loc.alt += accels_velocity.z * G_Dt;
*/
}
void xy_error_correction()
{
// Calculate speed error
// ---------------------
speed_error.x = x_actual_speed - accels_velocity.x;
speed_error.y = y_actual_speed - accels_velocity.y;
// Calculate position error
// ------------------------
//position_error.x = accels_position.x - current_loc.lng;
//position_error.y = accels_position.y - current_loc.lat;
// correct integrated velocity by speed_error
// this number must be small or we will bring back sensor latency
// -------------------------------------------
accels_velocity.x += speed_error.x * 0.03; // g.speed_correction_x;
accels_velocity.y += speed_error.y * 0.03;
// Error correct the accels to deal with calibration, drift and noise
// ------------------------------------------------------------------
//accels_position.x -= position_error.x * 0.08; // g.loiter_offset_correction; //.001;
//accels_position.y -= position_error.y * 0.08; // g.loiter_offset_correction; //.001;
accels_position.x = 0;
accels_position.y = 0;
}
void z_error_correction()
{
// Calculate speed error
// ---------------------
speed_error.z = climb_rate - accels_velocity.z;
//position_error.z = accels_position.z - current_loc.alt;
// correct integrated velocity by speed_error
// this number must be small or we will bring back sensor latency
// -------------------------------------------
accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z;
// ------------------------------------------------------------------
//accels_position.z -= position_error.z * 0.006; //g.alt_offset_correction; // OK
accels_position.z = 0;
// For developement only
// ---------------------
if(motors.armed())
Log_Write_Raw();
}
#endif

6
ArduCopter/motors.pde
View File

@ -120,8 +120,6 @@ static void init_arm_motors()
// -------------------- // --------------------
init_simple_bearing(); init_simple_bearing();
init_z_damper();
// Reset home position // Reset home position
// ------------------- // -------------------
if(home_is_set) if(home_is_set)
@ -173,6 +171,10 @@ static void init_disarm_motors()
g.throttle_cruise.save(); g.throttle_cruise.save();
#if INERTIAL_NAV == ENABLED
inertial_nav.save_params();
#endif
// we are not in the air // we are not in the air
takeoff_complete = false; takeoff_complete = false;

414
ArduCopter/navigation.pde
View File

@ -1,64 +1,146 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//**************************************************************** // update_navigation - checks for new GPS updates and invokes navigation routines
// Function that will calculate the desired direction to fly and distance static void update_navigation()
//****************************************************************
static void navigate()
{ {
// waypoint distance from plane in cm static uint32_t nav_last_gps_update = 0; // the system time of the last gps update
// --------------------------------------- static uint32_t nav_last_gps_time = 0; // the time according to the gps
wp_distance = get_distance_cm(&filtered_loc, &next_WP); bool pos_updated = false;
home_distance = get_distance_cm(&filtered_loc, &home); bool log_output = false;
// target_bearing is where we should be heading // check for new gps data
// -------------------------------------------- if( g_gps->fix && g_gps->time != nav_last_gps_time ) {
target_bearing = get_bearing_cd(&filtered_loc, &next_WP);
home_to_copter_bearing = get_bearing_cd(&home, &current_loc); // used to calculate speed in X and Y, iterms
// ------------------------------------------
dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0;
nav_last_gps_update = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0);
// save GPS time
nav_last_gps_time = g_gps->time;
// signal to run nav controllers
pos_updated = true;
// signal to create log entry
log_output = true;
}
#if INERTIAL_NAV == ENABLED
// TO-DO: clean this up because inertial nav is overwriting the dTnav and pos_updated from above
// check for inertial nav updates
if( inertial_nav.position_ok() ) {
// 50hz
dTnav = 0.02; // To-Do: calculate the time from the mainloop or INS readings?
// signal to run nav controllers
pos_updated = true;
}
#endif
// calc various navigation values and run controllers if we've received a position update
if( pos_updated ) {
// calculate velocity
calc_velocity_and_position();
// calculate distance, angles to target
calc_distance_and_bearing();
// run navigation controllers
run_navigation_contollers();
// Rotate the nav_lon and nav_lat vectors based on Yaw
calc_nav_pitch_roll();
// update log
if (log_output && (g.log_bitmask & MASK_LOG_NTUN) && motors.armed()) {
Log_Write_Nav_Tuning();
}
}
// reduce nav outputs to zero if we have not received a gps update in 2 seconds
if( millis() - nav_last_gps_update > 2000 ) {
// 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
auto_roll >>= 1;
auto_pitch >>= 1;
}
} }
static bool check_missed_wp() //*******************************************************************************************************
{ // calc_velocity_and_filtered_position - velocity in lon and lat directions calculated from GPS position
int32_t temp; // and accelerometer data
temp = target_bearing - original_target_bearing; // lon_speed expressed in cm/s. positive numbers mean moving east
temp = wrap_180(temp); // lat_speed expressed in cm/s. positive numbers when moving north
return (labs(temp) > 9000); // we passed the waypoint by 100 degrees // Note: we use gps locations directly to calculate velocity instead of asking gps for velocity because
} // this is more accurate below 1.5m/s
// Note: even though the positions are projected using a lead filter, the velocities are calculated
// ------------------------------ // from the unaltered gps locations. We do not want noise from our lead filter affecting velocity
static void calc_XY_velocity(){ //*******************************************************************************************************
static int32_t last_longitude = 0; static void calc_velocity_and_position(){
static int32_t last_latitude = 0; static int32_t last_gps_longitude = 0;
static int32_t last_gps_latitude = 0;
// called after GPS read
// offset calculation of GPS speed:
// used for estimations below 1.5m/s
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
// initialise last_longitude and last_latitude // initialise last_longitude and last_latitude
if( last_longitude == 0 && last_latitude == 0 ) { if( last_gps_longitude == 0 && last_gps_latitude == 0 ) {
last_longitude = g_gps->longitude; last_gps_longitude = g_gps->longitude;
last_latitude = g_gps->latitude; last_gps_latitude = g_gps->latitude;
} }
// this speed is ~ in cm because we are using 10^7 numbers from GPS // this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav; float tmp = 1.0/dTnav;
x_actual_speed = (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp;
y_actual_speed = (float)(g_gps->latitude - last_latitude) * tmp;
last_longitude = g_gps->longitude;
last_latitude = g_gps->latitude;
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
// inertial_nav if( inertial_nav.position_ok() ) {
xy_error_correction(); // pull velocity from interial nav library
filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, accels_velocity.x); lon_speed = inertial_nav.get_longitude_velocity();
filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, accels_velocity.y); lat_speed = inertial_nav.get_latitude_velocity();
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}else{
// calculate velocity
lon_speed = (float)(g_gps->longitude - last_gps_longitude) * scaleLongDown * tmp;
lat_speed = (float)(g_gps->latitude - last_gps_latitude) * tmp;
// calculate position from gps + expected travel during gps_lag
current_loc.lng = xLeadFilter.get_position(g_gps->longitude, lon_speed, g_gps->get_lag());
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, lat_speed, g_gps->get_lag());
}
#else #else
filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed, g_gps->get_lag()); // calculate velocity
filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed, g_gps->get_lag()); lon_speed = (float)(g_gps->longitude - last_gps_longitude) * scaleLongDown * tmp;
lat_speed = (float)(g_gps->latitude - last_gps_latitude) * tmp;
// calculate position from gps + expected travel during gps_lag
current_loc.lng = xLeadFilter.get_position(g_gps->longitude, lon_speed, g_gps->get_lag());
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, lat_speed, g_gps->get_lag());
#endif #endif
// store gps lat and lon values for next iteration
last_gps_longitude = g_gps->longitude;
last_gps_latitude = g_gps->latitude;
}
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
static void calc_distance_and_bearing()
{
// waypoint distance from plane in cm
// ---------------------------------------
wp_distance = get_distance_cm(&current_loc, &next_WP);
home_distance = get_distance_cm(&current_loc, &home);
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing_cd(&current_loc, &next_WP);
home_to_copter_bearing = get_bearing_cd(&home, &current_loc);
} }
static void calc_location_error(struct Location *next_loc) static void calc_location_error(struct Location *next_loc)
@ -79,6 +161,189 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
} }
// called after a GPS read
static void run_navigation_contollers()
{
// wp_distance is in CM
// --------------------
switch(control_mode) {
case AUTO:
// note: wp_control is handled by commands_logic
verify_commands();
// calculates desired Yaw
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case GUIDED:
wp_control = WP_MODE;
// check if we are close to point > loiter
wp_verify_byte = 0;
verify_nav_wp();
if (wp_control == WP_MODE) {
update_auto_yaw();
} else {
set_mode(LOITER);
}
update_nav_wp();
break;
case RTL:
// have we reached the desired Altitude?
if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt
if(rtl_reached_alt == false) {
rtl_reached_alt = true;
do_RTL();
}
wp_control = WP_MODE;
// checks if we have made it to home
update_nav_RTL();
} else{
// we need to loiter until we are ready to come home
wp_control = LOITER_MODE;
}
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
#endif
// calculates the desired Roll and Pitch
update_nav_wp();
break;
// switch passthrough to LOITER
case LOITER:
case POSITION:
// This feature allows us to reposition the quad when the user lets
// go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500)
loiter_override = true;
}
// Allow the user to take control temporarily,
if(loiter_override) {
// this sets the copter to not try and nav while we control it
wp_control = NO_NAV_MODE;
// reset LOITER to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
loiter_override = false;
wp_control = LOITER_MODE;
}
}else{
wp_control = LOITER_MODE;
}
if(loiter_timer != 0) {
// If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach
if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()) {
// just to make sure we clear the timer
loiter_timer = 0;
if(g.rtl_approach_alt == 0) {
set_mode(LAND);
if(home_distance < 300) {
next_WP.lat = home.lat;
next_WP.lng = home.lng;
}
}else{
if(g.rtl_approach_alt < current_loc.alt) {
set_new_altitude(g.rtl_approach_alt);
}
}
}
}
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case LAND:
if(g.sonar_enabled)
verify_land_sonar();
else
verify_land_baro();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case CIRCLE:
wp_control = CIRCLE_MODE;
// calculates desired Yaw
update_auto_yaw();
update_nav_wp();
break;
case STABILIZE:
case TOY_A:
case TOY_M:
wp_control = NO_NAV_MODE;
update_nav_wp();
break;
}
// are we in SIMPLE mode?
if(do_simple && g.super_simple) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
//Serial.printf("ISB: %d\n", initial_simple_bearing);
}
}
if(yaw_mode == YAW_LOOK_AT_HOME) {
if(home_is_set) {
nav_yaw = get_bearing_cd(&current_loc, &home);
} else {
nav_yaw = 0;
}
}
}
static void update_nav_RTL()
{
// Have we have reached Home?
if(wp_distance <= 200 || check_missed_wp()) {
// if loiter_timer value > 0, we are set to trigger auto_land or approach
set_mode(LOITER);
// just in case we arrive and we aren't at the lower RTL alt yet.
set_new_altitude(get_RTL_alt());
// force loitering above home
next_WP.lat = home.lat;
next_WP.lng = home.lng;
// If failsafe OR auto approach altitude is set
// we will go into automatic land, (g.rtl_approach_alt) is the lowest point
// -1 means disable feature
if(failsafe || g.rtl_approach_alt >= 0)
loiter_timer = millis();
else
loiter_timer = 0;
}
}
static bool check_missed_wp()
{
int32_t temp;
temp = target_bearing - original_target_bearing;
temp = wrap_180(temp);
return (labs(temp) > 9000); // we passed the waypoint by 100 degrees
}
#define NAV_ERR_MAX 600 #define NAV_ERR_MAX 600
#define NAV_RATE_ERR_MAX 250 #define NAV_RATE_ERR_MAX 250
static void calc_loiter(int16_t x_error, int16_t y_error) static void calc_loiter(int16_t x_error, int16_t y_error)
@ -97,14 +362,8 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
} }
#endif #endif
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED x_rate_error = x_target_speed - lon_speed; // calc the speed error
x_rate_error = x_target_speed - accels_velocity.x; // calc the speed error
#else
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error
#endif
p = g.pid_loiter_rate_lon.get_p(x_rate_error); p = g.pid_loiter_rate_lon.get_p(x_rate_error);
i = g.pid_loiter_rate_lon.get_i(x_rate_error + x_error, dTnav); i = g.pid_loiter_rate_lon.get_i(x_rate_error + x_error, dTnav);
@ -112,7 +371,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
d = constrain(d, -2000, 2000); d = constrain(d, -2000, 2000);
// get rid of noise // get rid of noise
if(abs(x_actual_speed) < 50) { if(abs(lon_speed) < 50) {
d = 0; d = 0;
} }
@ -137,11 +396,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
#endif #endif
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED y_rate_error = y_target_speed - lat_speed; // calc the speed error
y_rate_error = y_target_speed - accels_velocity.y; // calc the speed error
#else
y_rate_error = y_target_speed - y_actual_speed; // calc the speed error
#endif
p = g.pid_loiter_rate_lat.get_p(y_rate_error); p = g.pid_loiter_rate_lat.get_p(y_rate_error);
i = g.pid_loiter_rate_lat.get_i(y_rate_error + y_error, dTnav); i = g.pid_loiter_rate_lat.get_i(y_rate_error + y_error, dTnav);
@ -149,7 +404,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
d = constrain(d, -2000, 2000); d = constrain(d, -2000, 2000);
// get rid of noise // get rid of noise
if(abs(y_actual_speed) < 50) { if(abs(lat_speed) < 50) {
d = 0; d = 0;
} }
@ -189,11 +444,7 @@ static void calc_nav_rate(int16_t max_speed)
// East / West // East / West
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED x_rate_error = x_target_speed - lon_speed;
x_rate_error = x_target_speed - accels_velocity.x;
#else
x_rate_error = x_target_speed - x_actual_speed;
#endif
x_rate_error = constrain(x_rate_error, -500, 500); x_rate_error = constrain(x_rate_error, -500, 500);
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav); nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
@ -205,11 +456,7 @@ static void calc_nav_rate(int16_t max_speed)
// North / South // North / South
// calculate rate error // calculate rate error
#if INERTIAL_NAV == ENABLED y_rate_error = y_target_speed - lat_speed;
y_rate_error = y_target_speed - accels_velocity.y;
#else
y_rate_error = y_target_speed - y_actual_speed;
#endif
y_rate_error = constrain(y_rate_error, -500, 500); // added a rate error limit to keep pitching down to a minimum y_rate_error = constrain(y_rate_error, -500, 500); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav); nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
@ -226,7 +473,7 @@ static void calc_nav_rate(int16_t max_speed)
// this calculation rotates our World frame of reference to the copter's frame of reference // this calculation rotates our World frame of reference to the copter's frame of reference
// We use the DCM's matrix to precalculate these trig values at 50hz // We use the DCM's matrix to precalculate these trig values at 50hz
static void calc_loiter_pitch_roll() static void calc_nav_pitch_roll()
{ {
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector // rotate the vector
@ -297,7 +544,13 @@ static int32_t get_altitude_error()
// Next_WP alt is our target alt // Next_WP alt is our target alt
// It changes based on climb rate // It changes based on climb rate
// until it reaches the target_altitude // until it reaches the target_altitude
#if INERTIAL_NAV == ENABLED
// use inertial nav for altitude error
return next_WP.alt - inertial_nav._position.z;
#else
return next_WP.alt - current_loc.alt; return next_WP.alt - current_loc.alt;
#endif
} }
static void clear_new_altitude() static void clear_new_altitude()
@ -358,3 +611,24 @@ static int32_t wrap_180(int32_t error)
return error; return error;
} }
static float wrap_360f(float angle_in_degrees)
{
if (angle_in_degrees > 36000) angle_in_degrees -= 36000;
if (angle_in_degrees < 0) angle_in_degrees += 36000;
return angle_in_degrees;
}
static float wrap_180f(float angle_in_degrees)
{
if (angle_in_degrees > 18000) angle_in_degrees -= 36000;
if (angle_in_degrees < -18000) angle_in_degrees += 36000;
return angle_in_degrees;
}
static float wrap_PI(float angle_in_radians)
{
if (angle_in_radians > M_PI) angle_in_radians -= 2.0*M_PI;
if (angle_in_radians < -M_PI) angle_in_radians += 2.0*M_PI;
return angle_in_radians;
}

6
ArduCopter/system.pde
View File

@ -283,12 +283,6 @@ static void init_ardupilot()
// --------------------------- // ---------------------------
reset_control_switch(); reset_control_switch();
// init the Z damopener
// --------------------
#if ACCEL_ALT_HOLD != 0
init_z_damper();
#endif
startup_ground(); startup_ground();

2
ArduCopter/test.pde
View File

@ -1006,7 +1006,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
next_WP.lng = -1199541248; next_WP.lng = -1199541248;
// got 23506;, should be 22800 // got 23506;, should be 22800
navigate(); update_navigation();
Serial.printf_P(PSTR("bear: %ld\n"), target_bearing); Serial.printf_P(PSTR("bear: %ld\n"), target_bearing);
return 0; return 0;
} }