mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: incorporate new version of inertial navigation
Moved several navigation functions from ArduCopter.pde to navigation.pde
This commit is contained in:
parent
1c7118a2d1
commit
1f801714e8
|
@ -105,6 +105,8 @@
|
|||
#include <AP_Camera.h> // Photo or video camera
|
||||
#include <AP_Mount.h> // Camera/Antenna mount
|
||||
#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>
|
||||
|
||||
// Configuration
|
||||
|
@ -371,10 +373,10 @@ AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
|
|||
//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
|
||||
static int16_t x_actual_speed;
|
||||
static int16_t y_actual_speed;
|
||||
static int16_t lon_speed; // expressed in cm/s. positive numbers mean moving east
|
||||
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
|
||||
|
@ -505,7 +507,7 @@ static float scaleLongDown = 1;
|
|||
// Used by Mavlink for unknow reasons
|
||||
static const float radius_of_earth = 6378100; // meters
|
||||
// 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
|
||||
union float_int {
|
||||
|
@ -517,9 +519,6 @@ union float_int {
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
static int32_t target_bearing;
|
||||
// Status of the Waypoint tracking mode. Options include:
|
||||
|
@ -730,8 +729,6 @@ static struct Location home;
|
|||
static boolean home_is_set;
|
||||
// Current location of the copter
|
||||
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
|
||||
static struct Location next_WP;
|
||||
// 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
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
// The rotated accelerometer values
|
||||
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;
|
||||
|
||||
AP_InertialNav3D inertial_nav(&ahrs, &ins, &barometer, &g_gps);
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -884,9 +867,6 @@ static Vector3f speed_error;
|
|||
static int16_t perf_mon_counter;
|
||||
// The number of GPS fixes we have had
|
||||
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
|
||||
// --------------
|
||||
|
@ -902,8 +882,6 @@ static byte slow_loopCounter;
|
|||
static byte counter_one_herz;
|
||||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
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
|
||||
static float dTnav;
|
||||
// Counters for branching from 4 minute control loop used to save Compass offsets
|
||||
|
@ -998,7 +976,7 @@ void loop()
|
|||
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
fast_loop();////
|
||||
fast_loop();
|
||||
|
||||
// run the 50hz loop 1/2 the time
|
||||
run_50hz_loop = !run_50hz_loop;
|
||||
|
@ -1015,18 +993,13 @@ void loop()
|
|||
// port manipulation for external timing of main loops
|
||||
//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
|
||||
// --------------------------
|
||||
update_GPS();
|
||||
|
||||
// run navigation routines
|
||||
update_navigation();
|
||||
|
||||
// perform 10hz tasks
|
||||
// ------------------
|
||||
medium_loop();
|
||||
|
@ -1098,11 +1071,13 @@ static void fast_loop()
|
|||
// --------------------
|
||||
read_AHRS();
|
||||
|
||||
// reads all of the necessary trig functions for cameras, throttle, etc.
|
||||
// --------------------------------------------------------------------
|
||||
update_trig();
|
||||
|
||||
// Inertial Nav
|
||||
// --------------------
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
calc_inertia();
|
||||
#endif
|
||||
read_inertia();
|
||||
|
||||
// optical flow
|
||||
// --------------------
|
||||
|
@ -1159,23 +1134,6 @@ static void medium_loop()
|
|||
case 1:
|
||||
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;
|
||||
|
||||
// command processing
|
||||
|
@ -1492,15 +1450,6 @@ static void update_GPS(void)
|
|||
g_gps->update();
|
||||
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) {
|
||||
// clear new data flag
|
||||
g_gps->new_data = false;
|
||||
|
@ -1508,27 +1457,10 @@ static void update_GPS(void)
|
|||
// check for duiplicate GPS messages
|
||||
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
|
||||
// --------------------------
|
||||
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) {
|
||||
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()) {
|
||||
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(¤t_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)
|
||||
{
|
||||
// Perform IMU calculations and get attitude info
|
||||
|
@ -2120,10 +1872,6 @@ static void update_altitude()
|
|||
#else
|
||||
// This is real life
|
||||
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
baro_rate = accels_velocity.z;
|
||||
|
||||
#else
|
||||
// read in Actual Baro Altitude
|
||||
baro_alt = read_barometer();
|
||||
|
||||
|
@ -2141,7 +1889,6 @@ static void update_altitude()
|
|||
baro_rate = (temp + baro_rate) >> 1;
|
||||
baro_rate = constrain(baro_rate, -300, 300);
|
||||
*/
|
||||
#endif
|
||||
|
||||
// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
|
||||
#endif
|
||||
|
@ -2198,11 +1945,6 @@ static void update_altitude()
|
|||
|
||||
// calc error
|
||||
climb_rate_error = (climb_rate_actual - climb_rate) / 5;
|
||||
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
// inertial_nav
|
||||
z_error_correction();
|
||||
#endif
|
||||
}
|
||||
|
||||
static void update_altitude_est()
|
||||
|
@ -2366,6 +2108,12 @@ static void tuning(){
|
|||
break;
|
||||
#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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -478,7 +478,7 @@ get_throttle_rate(int16_t z_target_speed)
|
|||
|
||||
// calculate rate error
|
||||
#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
|
||||
z_rate_error = z_target_speed - climb_rate; // calc the speed error
|
||||
#endif
|
||||
|
@ -629,141 +629,6 @@ static int16_t heli_get_angle_boost(int16_t throttle)
|
|||
}
|
||||
#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
|
||||
static int32_t
|
||||
get_of_roll(int32_t input_roll)
|
||||
|
|
|
@ -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_PID) Serial.printf_P(PSTR(" PID"));
|
||||
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();
|
||||
|
@ -203,6 +204,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||
TARG(OPTFLOW);
|
||||
TARG(PID);
|
||||
TARG(ITERM);
|
||||
TARG(INAV);
|
||||
#undef TARG
|
||||
}
|
||||
|
||||
|
@ -289,61 +291,6 @@ static void Log_Read_GPS()
|
|||
(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()
|
||||
{
|
||||
Vector3f gyro = ins.get_gyro();
|
||||
|
@ -395,7 +342,6 @@ static void Log_Read_Raw()
|
|||
temp2);
|
||||
*/
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// 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_roll); // 6
|
||||
DataFlash.WriteInt(x_actual_speed); // 7
|
||||
DataFlash.WriteInt(y_actual_speed); // 8
|
||||
DataFlash.WriteInt(lon_speed); // 7
|
||||
DataFlash.WriteInt(lat_speed); // 8
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
@ -832,6 +778,80 @@ static void Log_Read_Attitude()
|
|||
(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
|
||||
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:
|
||||
Log_Read_DMP();
|
||||
break;
|
||||
|
||||
case LOG_INAV_MSG:
|
||||
Log_Read_INAV();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
|
|
|
@ -88,6 +88,11 @@ public:
|
|||
//
|
||||
k_param_motors = 90,
|
||||
|
||||
//
|
||||
// 100: Inertial Nav
|
||||
//
|
||||
k_param_inertial_nav = 100,
|
||||
|
||||
// 110: Telemetry control
|
||||
//
|
||||
k_param_gcs0 = 110,
|
||||
|
|
|
@ -368,6 +368,12 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
GOBJECT(ins, "INS_", AP_InertialSensor),
|
||||
#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(gcs3, "SR3_", GCS_MAVLINK),
|
||||
|
||||
|
|
|
@ -140,7 +140,7 @@ static void set_next_WP(struct Location *wp)
|
|||
if (next_WP.lat == 0 || command_nav_index <= 1) {
|
||||
prev_WP = current_loc;
|
||||
}else{
|
||||
if (get_distance_cm(&filtered_loc, &next_WP) < 500)
|
||||
if (get_distance_cm(¤t_loc, &next_WP) < 500)
|
||||
prev_WP = next_WP;
|
||||
else
|
||||
prev_WP = current_loc;
|
||||
|
@ -168,7 +168,7 @@ static void set_next_WP(struct Location *wp)
|
|||
|
||||
// this is handy for the groundstation
|
||||
// -----------------------------------
|
||||
wp_distance = get_distance_cm(&filtered_loc, &next_WP);
|
||||
wp_distance = get_distance_cm(¤t_loc, &next_WP);
|
||||
target_bearing = get_bearing_cd(&prev_WP, &next_WP);
|
||||
|
||||
// calc the location error:
|
||||
|
@ -199,6 +199,11 @@ static void init_home()
|
|||
set_cmd_with_index(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)
|
||||
Log_Write_Cmd(0, &home);
|
||||
|
||||
|
|
|
@ -968,6 +968,9 @@
|
|||
#ifndef LOG_ITERM
|
||||
# define LOG_ITERM ENABLED
|
||||
#endif
|
||||
#ifndef LOG_INAV
|
||||
# define LOG_INAV DISABLED
|
||||
#endif
|
||||
|
||||
// calculate the default log_bitmask
|
||||
#define LOGBIT(_s) (LOG_ ## _s ? MASK_LOG_ ## _s : 0)
|
||||
|
@ -985,7 +988,9 @@
|
|||
LOGBIT(CUR) | \
|
||||
LOGBIT(MOTORS) | \
|
||||
LOGBIT(OPTFLOW) | \
|
||||
LOGBIT(PID)
|
||||
LOGBIT(PID) | \
|
||||
LOGBIT(ITERM) | \
|
||||
LOGBIT(INAV)
|
||||
|
||||
// if we are using fast, Disable Medium
|
||||
//#if LOG_ATTITUDE_FAST == ENABLED
|
||||
|
@ -1087,7 +1092,8 @@
|
|||
#endif
|
||||
|
||||
// Inertia based contollers. disabled by default, work in progress
|
||||
#define ACCEL_ALT_HOLD 0
|
||||
#define INERTIAL_NAV DISABLED
|
||||
#ifndef INERTIAL_NAV
|
||||
# define INERTIAL_NAV DISABLED
|
||||
#endif
|
||||
|
||||
#endif // __ARDUCOPTER_CONFIG_H__
|
||||
|
|
|
@ -192,6 +192,8 @@
|
|||
#define CH6_AHRS_YAW_KP 30
|
||||
#define CH6_AHRS_KP 31
|
||||
|
||||
// Inertial Nav
|
||||
#define CH6_INAV_TC 32
|
||||
|
||||
// nav byte mask
|
||||
// -------------
|
||||
|
@ -297,6 +299,7 @@ enum gcs_severity {
|
|||
#define LOG_PID_MSG 0x0E
|
||||
#define LOG_ITERM_MSG 0x0F
|
||||
#define LOG_DMP_MSG 0x10
|
||||
#define LOG_INAV_MSG 0x11
|
||||
#define LOG_INDEX_MSG 0xF0
|
||||
#define MAX_NUM_LOGS 50
|
||||
|
||||
|
@ -314,6 +317,7 @@ enum gcs_severity {
|
|||
#define MASK_LOG_OPTFLOW (1<<11)
|
||||
#define MASK_LOG_PID (1<<12)
|
||||
#define MASK_LOG_ITERM (1<<13)
|
||||
#define MASK_LOG_INAV (1<<14)
|
||||
|
||||
|
||||
// Waypoint Modes
|
||||
|
|
|
@ -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
|
||||
static uint8_t log_counter_inav = 0;
|
||||
|
||||
// generates a new location and velocity in space based on inertia
|
||||
// Calc 100 hz
|
||||
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
|
||||
// inertial altitude estimates
|
||||
inertial_nav.update(G_Dt);
|
||||
|
||||
// rising = 2
|
||||
// neutral = 0
|
||||
// falling = -2
|
||||
|
||||
// ACC Y POS = going EAST
|
||||
// ACC X POS = going North
|
||||
// ACC Z POS = going DOWN (lets flip this)
|
||||
|
||||
// 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
|
||||
if( motors.armed() && g.log_bitmask & MASK_LOG_INAV ) {
|
||||
log_counter_inav++;
|
||||
if( log_counter_inav >= 10 ) {
|
||||
log_counter_inav = 0;
|
||||
Log_Write_INAV(G_Dt);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
|
@ -120,8 +120,6 @@ static void init_arm_motors()
|
|||
// --------------------
|
||||
init_simple_bearing();
|
||||
|
||||
init_z_damper();
|
||||
|
||||
// Reset home position
|
||||
// -------------------
|
||||
if(home_is_set)
|
||||
|
@ -173,6 +171,10 @@ static void init_disarm_motors()
|
|||
|
||||
g.throttle_cruise.save();
|
||||
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
inertial_nav.save_params();
|
||||
#endif
|
||||
|
||||
// we are not in the air
|
||||
takeoff_complete = false;
|
||||
|
||||
|
|
|
@ -1,64 +1,146 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
//****************************************************************
|
||||
// Function that will calculate the desired direction to fly and distance
|
||||
//****************************************************************
|
||||
static void navigate()
|
||||
// update_navigation - checks for new GPS updates and invokes navigation routines
|
||||
static void update_navigation()
|
||||
{
|
||||
// waypoint distance from plane in cm
|
||||
// ---------------------------------------
|
||||
wp_distance = get_distance_cm(&filtered_loc, &next_WP);
|
||||
home_distance = get_distance_cm(&filtered_loc, &home);
|
||||
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
|
||||
bool pos_updated = false;
|
||||
bool log_output = false;
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing_cd(&filtered_loc, &next_WP);
|
||||
home_to_copter_bearing = get_bearing_cd(&home, ¤t_loc);
|
||||
// check for new gps data
|
||||
if( g_gps->fix && g_gps->time != nav_last_gps_time ) {
|
||||
|
||||
// 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()
|
||||
{
|
||||
int32_t temp;
|
||||
temp = target_bearing - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
return (labs(temp) > 9000); // we passed the waypoint by 100 degrees
|
||||
}
|
||||
|
||||
// ------------------------------
|
||||
static void calc_XY_velocity(){
|
||||
static int32_t last_longitude = 0;
|
||||
static int32_t last_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
|
||||
//*******************************************************************************************************
|
||||
// calc_velocity_and_filtered_position - velocity in lon and lat directions calculated from GPS position
|
||||
// and accelerometer data
|
||||
// lon_speed expressed in cm/s. positive numbers mean moving east
|
||||
// lat_speed expressed in cm/s. positive numbers when moving north
|
||||
// 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_velocity_and_position(){
|
||||
static int32_t last_gps_longitude = 0;
|
||||
static int32_t last_gps_latitude = 0;
|
||||
|
||||
// initialise last_longitude and last_latitude
|
||||
if( last_longitude == 0 && last_latitude == 0 ) {
|
||||
last_longitude = g_gps->longitude;
|
||||
last_latitude = g_gps->latitude;
|
||||
if( last_gps_longitude == 0 && last_gps_latitude == 0 ) {
|
||||
last_gps_longitude = g_gps->longitude;
|
||||
last_gps_latitude = g_gps->latitude;
|
||||
}
|
||||
|
||||
// this speed is ~ in cm because we are using 10^7 numbers from GPS
|
||||
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
|
||||
// inertial_nav
|
||||
xy_error_correction();
|
||||
filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, accels_velocity.x);
|
||||
filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, accels_velocity.y);
|
||||
if( inertial_nav.position_ok() ) {
|
||||
// pull velocity from interial nav library
|
||||
lon_speed = inertial_nav.get_longitude_velocity();
|
||||
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
|
||||
filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed, g_gps->get_lag());
|
||||
filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed, g_gps->get_lag());
|
||||
// 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());
|
||||
#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(¤t_loc, &next_WP);
|
||||
home_distance = get_distance_cm(¤t_loc, &home);
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing_cd(¤t_loc, &next_WP);
|
||||
home_to_copter_bearing = get_bearing_cd(&home, ¤t_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
|
||||
}
|
||||
|
||||
// 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(¤t_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_RATE_ERR_MAX 250
|
||||
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
|
||||
|
||||
|
||||
// calculate rate error
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
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
|
||||
|
||||
x_rate_error = x_target_speed - lon_speed; // calc the speed 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);
|
||||
|
@ -112,7 +371,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
|
|||
d = constrain(d, -2000, 2000);
|
||||
|
||||
// get rid of noise
|
||||
if(abs(x_actual_speed) < 50) {
|
||||
if(abs(lon_speed) < 50) {
|
||||
d = 0;
|
||||
}
|
||||
|
||||
|
@ -137,11 +396,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
|
|||
#endif
|
||||
|
||||
// calculate rate error
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
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
|
||||
y_rate_error = y_target_speed - lat_speed; // calc the speed 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);
|
||||
|
@ -149,7 +404,7 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
|
|||
d = constrain(d, -2000, 2000);
|
||||
|
||||
// get rid of noise
|
||||
if(abs(y_actual_speed) < 50) {
|
||||
if(abs(lat_speed) < 50) {
|
||||
d = 0;
|
||||
}
|
||||
|
||||
|
@ -189,11 +444,7 @@ static void calc_nav_rate(int16_t max_speed)
|
|||
|
||||
// East / West
|
||||
// calculate rate error
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
x_rate_error = x_target_speed - accels_velocity.x;
|
||||
#else
|
||||
x_rate_error = x_target_speed - x_actual_speed;
|
||||
#endif
|
||||
x_rate_error = x_target_speed - lon_speed;
|
||||
|
||||
x_rate_error = constrain(x_rate_error, -500, 500);
|
||||
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
|
||||
// calculate rate error
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
y_rate_error = y_target_speed - accels_velocity.y;
|
||||
#else
|
||||
y_rate_error = y_target_speed - y_actual_speed;
|
||||
#endif
|
||||
y_rate_error = y_target_speed - lat_speed;
|
||||
|
||||
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);
|
||||
|
@ -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
|
||||
// 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);
|
||||
// rotate the vector
|
||||
|
@ -297,7 +544,13 @@ static int32_t get_altitude_error()
|
|||
// Next_WP alt is our target alt
|
||||
// It changes based on climb rate
|
||||
// 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;
|
||||
#endif
|
||||
}
|
||||
|
||||
static void clear_new_altitude()
|
||||
|
@ -358,3 +611,24 @@ static int32_t wrap_180(int32_t 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;
|
||||
}
|
||||
|
||||
|
|
|
@ -283,12 +283,6 @@ static void init_ardupilot()
|
|||
// ---------------------------
|
||||
reset_control_switch();
|
||||
|
||||
// init the Z damopener
|
||||
// --------------------
|
||||
#if ACCEL_ALT_HOLD != 0
|
||||
init_z_damper();
|
||||
#endif
|
||||
|
||||
|
||||
startup_ground();
|
||||
|
||||
|
|
|
@ -1006,7 +1006,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
|
|||
next_WP.lng = -1199541248;
|
||||
|
||||
// got 23506;, should be 22800
|
||||
navigate();
|
||||
update_navigation();
|
||||
Serial.printf_P(PSTR("bear: %ld\n"), target_bearing);
|
||||
return 0;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue