From c42f9ece4342b9c09164663198a9329b716729b5 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 13 Jun 2012 22:27:03 -0700 Subject: [PATCH] Inertial Control I added inertial navigation based on the simulator data. This is an option only available if you compile with Arduino and set #define INERTIAL_NAV ENABLED in the APM_Config.h file. This has been tested for one real flight and did not crash my quad, but consider it very alpha. The quad may be unpredictable at first until the error correction fixes poorly calibrated accels. Be Careful. Most of the real work is in the inertia file, but the error correction, new variable defines and calibration calls are sprinkled throughout. The Log should record RAW messages with special debugging values. --- ArduCopter/APM_Config.h | 22 +++++++- ArduCopter/ArduCopter.pde | 28 ++++++++++ ArduCopter/Attitude.pde | 8 ++- ArduCopter/Log.pde | 45 +++++++++++++-- ArduCopter/inertia.pde | 114 ++++++++++++++++++++++++++++++++++++++ ArduCopter/motors.pde | 8 ++- ArduCopter/navigation.pde | 21 ++++++- 7 files changed, 234 insertions(+), 12 deletions(-) create mode 100644 ArduCopter/inertia.pde diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 9a9aaec63f..540f505875 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -43,6 +43,26 @@ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress +#define INERTIAL_NAV ENABLED + + +#if INERTIAL_NAV == ENABLED + #define ALT_HOLD_P 0.5 + #define ALT_HOLD_I 0.007 + #define ALT_HOLD_IMAX 300 + + // RATE control + #define THROTTLE_P 2 // + #define THROTTLE_I 0.5 // Don't edit + #define THROTTLE_D 0.0 // + + #define LOITER_P 0.50 + #define LOITER_I 0.0 + #define LOITER_RATE_P 12 // + #define LOITER_RATE_I 1.0 // Wind control + #define LOITER_RATE_D 0.0 // try 2 or 3 for LOITER_RATE 1 +#endif + //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 @@ -77,7 +97,7 @@ // MOT_n variables are used by the Frame mixing code. You must define // MOT_1 through MOT_m where m is the number of motors on your frame. // CH_n variables are used for RC output. These can be CH_1 through CH_8, -// and CH_10 or CH_12. +// and CH_10 or CH_12. // Sample channel config. Must define all MOT_ chanels used by // your FRAME_TYPE. // #define CONFIG_CHANNELS CHANNEL_CONFIG_CUSTOM diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b9c9a01ff6..a8795052a2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -831,6 +831,23 @@ static uint32_t condition_start; // Updated with the fast loop static float G_Dt = 0.02; +//////////////////////////////////////////////////////////////////////////////// +// Inertial Navigation +//////////////////////////////////////////////////////////////////////////////// +#if INERTIAL_NAV == ENABLED +// The rotated accelerometer values +static Vector3f accels_velocity; + +// accels rotated to world frame +static Vector3f accels_rotated; + +// error correction +static Vector3f speed_error; + +// Manage accel drift +static Vector3f accels_offset; +#endif + //////////////////////////////////////////////////////////////////////////////// // Performance monitoring //////////////////////////////////////////////////////////////////////////////// @@ -979,6 +996,12 @@ static void fast_loop() // -------------------- read_AHRS(); + // Inertial Nav + // -------------------- + #if INERTIAL_NAV == ENABLED + calc_inertia(); + #endif + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -2002,6 +2025,11 @@ 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() diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index b53b27ad87..7fd4bd8fca 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -259,7 +259,7 @@ get_rate_yaw(int32_t target_rate) #else int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); #endif - + // constrain output output = constrain(output, -yaw_limit, yaw_limit); @@ -297,7 +297,11 @@ get_nav_throttle(int32_t z_error) int16_t i_hold = g.pi_alt_hold.get_i(z_error, .02); // calculate rate error - z_rate_error = z_target_speed - climb_rate; + #if INERTIAL_NAV == ENABLED + z_rate_error = z_target_speed - accels_velocity.z; // calc the speed error + #else + z_rate_error = z_target_speed - climb_rate; // calc the speed error + #endif // limit the rate output = constrain(g.pid_throttle.get_pid(z_rate_error, .02), -80, 120); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 718325a485..5622e90a05 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -287,16 +287,47 @@ static void Log_Read_GPS() temp8); // 8 ground course } -// Write an raw accel/gyro data packet. Total length : 28 bytes +#if INERTIAL_NAV == ENABLED +static void Log_Write_Raw() +{ + Vector3f accel = imu.get_accel(); + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_RAW_MSG); + + DataFlash.WriteLong(get_int(accels_offset.x)); + DataFlash.WriteLong(get_int(accels_velocity.x)); + DataFlash.WriteLong(get_int(speed_error.x)); + + DataFlash.WriteLong(get_int(accels_offset.z)); + DataFlash.WriteLong(get_int(accels_velocity.z)); + DataFlash.WriteLong(get_int(speed_error.z)); + + 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 (int y = 0; y < 9; y++) { + logvar = get_float(DataFlash.ReadLong()); + Serial.print(logvar); + Serial.print(", "); + } + Serial.println(" "); +} +#else static void Log_Write_Raw() { Vector3f gyro = imu.get_gyro(); Vector3f accel = imu.get_accel(); - //Vector3f accel_filt = imu.get_accel_filtered(); - - //gyro *= t7; // Scale up for storage as long integers - //accel *= t7; - //accel_filt *= t7; DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -325,6 +356,8 @@ static void Log_Read_Raw() } Serial.println(" "); } +#endif + // Write an Current data packet. Total length : 16 bytes static void Log_Write_Current() diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde new file mode 100644 index 0000000000..45ebfcc5e9 --- /dev/null +++ b/ArduCopter/inertia.pde @@ -0,0 +1,114 @@ +#if INERTIAL_NAV == ENABLED + +// 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() * imu.get_accel(); + accels_rotated += accels_offset; // skew accels to account for long term error using calibration + accels_rotated.z += 9.805; // remove influence of gravity + + // rising = 2 + // 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 + + // Integrate accels to get the velocity + // ------------------------------------ + accels_velocity += temp; +} + +void z_error_correction() +{ + speed_error.z = climb_rate - accels_velocity.z; + accels_velocity.z += speed_error.z * 0.0350; //speed_correction_z; + accels_velocity.z -= g.pid_throttle.get_integrator() * 0.0045; //g.alt_offset_correction; // OK + accels_offset.z -= g.pid_throttle.get_integrator() * 0.000003; //g.alt_i_correction ; // .000002; +} + +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; + + // 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.0175; // g.speed_correction_x; + accels_velocity.y += speed_error.y * 0.0175; + + // Error correct the accels to deal with calibration, drift and noise + // ------------------------------------------------------------------ + accels_velocity.x -= g.pid_loiter_rate_lon.get_integrator() * 0.007; // g.loiter_offset_correction; //.001; + accels_velocity.y -= g.pid_loiter_rate_lat.get_integrator() * 0.007; // g.loiter_offset_correction; //.001; + + // update our accel offsets + // ------------------------- + accels_offset.x -= g.pid_loiter_rate_lon.get_integrator() * 0.000003; // g.loiter_i_correction; + accels_offset.y -= g.pid_loiter_rate_lat.get_integrator() * 0.000003; // g.loiter_i_correction; + + + // For developement only + // --------------------- + if(motors.armed()) + Log_Write_Raw(); +} + +static void calibrate_accels() +{ + // sets accels_velocity to 0,0,0 + zero_accels(); + + accels_offset.x = 0; + accels_offset.y = 0; + accels_offset.z = 0; + + for (int i = 0; i < 200; i++){ + delay(10); + read_AHRS(); + } + + for (int i = 0; i < 100; i++){ + delay(10); + read_AHRS(); + calc_inertia(); + Serial.printf("call accels: %1.5f, %1.5f, %1.5f,\n", accels_rotated.x, accels_rotated.y, accels_rotated.z); + } + + accels_velocity /= 100; + accels_offset = accels_velocity; + zero_accels(); + calc_inertia(); + + Log_Write_Data(25, (float)accels_offset.x); + Log_Write_Data(26, (float)accels_offset.y); + Log_Write_Data(27, (float)accels_offset.z); +} + +void zero_accels() +{ + accels_rotated.x = 0; + accels_rotated.y = 0; + accels_rotated.z = 0; + + accels_velocity.x = 0; + accels_velocity.y = 0; + accels_velocity.z = 0; +} + + +#endif \ No newline at end of file diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2186dc5ac3..ee3809ec17 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -85,7 +85,7 @@ static void init_arm_motors() motors.armed(true); #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ){ + if ( bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); delay(50); piezo_beep(); @@ -112,6 +112,10 @@ static void init_arm_motors() startup_ground(); } + #if INERTIAL_NAV == ENABLED + calibrate_accels(); + #endif + #if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground - // this resets Baro for more accuracy @@ -141,7 +145,7 @@ static void init_disarm_motors() takeoff_complete = false; #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ){ + if ( bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); } #endif diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 38f50ff218..5443f93952 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -64,6 +64,11 @@ static void calc_XY_velocity(){ last_longitude = g_gps->longitude; last_latitude = g_gps->latitude; + #if INERTIAL_NAV == ENABLED + // inertial_nav + xy_error_correction(); + #endif + /*if(g_gps->ground_speed > 150){ float temp = radians((float)g_gps->ground_course/100.0); x_actual_speed = (float)g_gps->ground_speed * sin(temp); @@ -107,7 +112,15 @@ static void calc_loiter(int x_error, int 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 + + 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); d = g.pid_loiter_rate_lon.get_d(x_error, dTnav); @@ -138,7 +151,13 @@ static void calc_loiter(int x_error, int y_error) } #endif - y_rate_error = y_target_speed - y_actual_speed; + // 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 + 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); d = g.pid_loiter_rate_lat.get_d(y_error, dTnav);