From 1f801714e8090300665006f6c0d2a1052b9cbf7d Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 7 Nov 2012 19:03:30 +0900 Subject: [PATCH] ArduCopter: incorporate new version of inertial navigation Moved several navigation functions from ArduCopter.pde to navigation.pde --- ArduCopter/ArduCopter.pde | 296 ++------------------------- ArduCopter/Attitude.pde | 137 +------------ ArduCopter/Log.pde | 140 +++++++------ ArduCopter/Parameters.h | 5 + ArduCopter/Parameters.pde | 6 + ArduCopter/commands.pde | 9 +- ArduCopter/config.h | 12 +- ArduCopter/defines.h | 4 + ArduCopter/inertia.pde | 102 ++-------- ArduCopter/motors.pde | 6 +- ArduCopter/navigation.pde | 414 +++++++++++++++++++++++++++++++------- ArduCopter/system.pde | 6 - ArduCopter/test.pde | 2 +- 13 files changed, 502 insertions(+), 637 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index acf3cf2862..3de067ad62 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -105,6 +105,8 @@ #include // Photo or video camera #include // Camera/Antenna mount #include // needed for AHRS build +#include // ArduPilot Mega inertial navigation library +#include // Complementary filter for combining barometer altitude with accelerometers #include // 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 } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 76bb3127b4..fbcbbcd8b6 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 689d96c037..7b30ed981c 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3c1ae38321..8dc8f60952 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -88,6 +88,11 @@ public: // k_param_motors = 90, + // + // 100: Inertial Nav + // + k_param_inertial_nav = 100, + // 110: Telemetry control // k_param_gcs0 = 110, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 5a54941600..db7903fa4b 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index d5d2842750..1fcfc840ab 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -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); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5de78db136..e7237beb65 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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__ diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 23a29cdb60..eecf59b46b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.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 diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde index bda5952a33..8107bff222 100644 --- a/ArduCopter/inertia.pde +++ b/ArduCopter/inertia.pde @@ -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 \ No newline at end of file + 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 +} \ No newline at end of file diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index b73e760c40..d074452852 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 31c386a5a7..df722c748e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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; +} + diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a8cf2eba9b..f32827b7aa 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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(); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 0608075a22..820e7b40b7 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -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; }