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);