ArduCopter: allow INERTIAL_NAV to be enabled separately for horizontal and vertical position

ArduCopter/APM_Config.h

@@ -54,10 +54,11 @@
 
 // Inertia based contollers.  disabled by default, work in progress
 //#define ACCEL_ALT_HOLD 0
-#define INERTIAL_NAV ENABLED
+#define INERTIAL_NAV_XY ENABLED
+#define INERTIAL_NAV_Z ENABLED
 
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_Z == ENABLED
  //#define ALT_HOLD_P                      3
  //#define ALT_HOLD_I                      0
  //#define ALT_HOLD_IMAX                  300
@@ -66,7 +67,8 @@
  //#define THROTTLE_P                      2.0
  //#define THROTTLE_I                      0.4
  //#define THROTTLE_D                      0.0
-
+#endif
+#if INERTIAL_NAV_XY == ENABLED
  //#define LOITER_P                        0.50
  //#define LOITER_I                        0.0
  //#define LOITER_RATE_P           5                      //

ArduCopter/ArduCopter.pde

@@ -866,7 +866,7 @@ static float G_Dt = 0.02;
 ////////////////////////////////////////////////////////////////////////////////
 // Inertial Navigation
 ////////////////////////////////////////////////////////////////////////////////
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
 AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps);
 #endif
 
@@ -2150,7 +2150,7 @@ static void tuning(){
         break;
 #endif
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
     case CH6_INAV_TC:
         inertial_nav.set_time_constant_xy(tuning_value);
         inertial_nav.set_time_constant_z(tuning_value);

ArduCopter/Attitude.pde

@@ -929,7 +929,7 @@ get_throttle_rate(int16_t z_target_speed)
     int16_t target_accel;
 
     // calculate rate error
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_Z == ENABLED
     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
@@ -1043,7 +1043,7 @@ get_throttle_land()
         get_throttle_rate_stabilized(-abs(g.land_speed));
 
         // detect whether we have landed
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_Z == ENABLED
         if (abs(inertial_nav._velocity.z) < 20) {
 #else
         if (abs(climb_rate) < 20) {

ArduCopter/Log.pde

@@ -787,7 +787,7 @@ static void Log_Read_Attitude()
 // Write an INAV packet. Total length : 36 Bytes
 static void Log_Write_INAV(float delta_t)
 {
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
     DataFlash.WriteByte(HEAD_BYTE1);
     DataFlash.WriteByte(HEAD_BYTE2);
     DataFlash.WriteByte(LOG_INAV_MSG);

ArduCopter/Parameters.pde

@@ -407,7 +407,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(ins,            "INS_", AP_InertialSensor),
 #endif
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
     // @Group: INAV_
     // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp
     GOBJECT(inertial_nav,           "INAV_",    AP_InertialNav),

ArduCopter/commands.pde

@@ -199,7 +199,7 @@ static void init_home()
     set_cmd_with_index(home, 0);
     //print_wp(&home, 0);
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED
     // set inertial nav's home position
     inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude);
 #endif

ArduCopter/config.h

@@ -1133,9 +1133,12 @@
  # define ALTERNATIVE_YAW_MODE DISABLED
 #endif
 
-// Inertia based contollers.  disabled by default, work in progress
-#ifndef INERTIAL_NAV
- # define INERTIAL_NAV DISABLED
+// Inertia based contollers.
+#ifndef INERTIAL_NAV_XY
+ # define INERTIAL_NAV_XY DISABLED
+#endif
+#ifndef INERTIAL_NAV_Z
+ # define INERTIAL_NAV_Z DISABLED
 #endif
 
 #endif // __ARDUCOPTER_CONFIG_H__

ArduCopter/inertia.pde

@@ -3,7 +3,7 @@
 // read_inertia - read inertia in from accelerometers
 static void read_inertia()
 {
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
     static uint8_t log_counter_inav = 0;
 
     // inertial altitude estimates

ArduCopter/motors.pde

@@ -184,7 +184,7 @@ static void init_disarm_motors()
 
     g.throttle_cruise.save();
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
     inertial_nav.save_params();
 #endif
 

ArduCopter/navigation.pde

@@ -29,7 +29,7 @@ static void update_navigation()
         log_output = true;
     }
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == 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() ) {
@@ -97,7 +97,7 @@ static void calc_velocity_and_position(){
     // this speed is ~ in cm because we are using 10^7 numbers from GPS
     float tmp = 1.0/dTnav;
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_XY == ENABLED
     if( inertial_nav.position_ok() ) {
         // pull velocity from interial nav library
         lon_speed = inertial_nav.get_longitude_velocity();
@@ -544,7 +544,7 @@ static int32_t get_altitude_error()
     // It changes based on climb rate
     // until it reaches the target_altitude
 
-#if INERTIAL_NAV == ENABLED
+#if INERTIAL_NAV_Z == ENABLED
     // use inertial nav for altitude error
     return next_WP.alt - inertial_nav._position.z;
 #else