Attempt to figure out AltHold issue

ArduCopter/mode_loiter.cpp

@@ -106,6 +106,9 @@ void ModeLoiter::run()
 
         // get pilot desired climb rate
         target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+/*        if (copter.heli_flags.inverted_flight) {
+            target_climb_rate = -target_climb_rate;
+        } */
         target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -189,7 +189,7 @@ class AC_AttitudeControl {
     // Command a thrust vector in the earth frame and a heading angle and/or rate
     virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true);
     virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);
-    void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
+    virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
 
     // Converts thrust vector and heading angle to quaternion rotation in the earth frame
     Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const;

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -523,3 +523,37 @@ void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_
     }
     AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
 }
+
+// Command a thrust vector and heading rate
+void AC_AttitudeControl_Heli::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
+{
+    // convert thrust vector to a roll and pitch angles
+    // this negates the advantage of using thrust vector control, but works just fine
+    Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
+
+    float euler_roll_angle_cd = degrees(angle_target.x) * 100.0f;
+    if (_inverted_flight) {
+        euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
+        AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, degrees(angle_target.y) * 100.0f, heading_rate_cds);
+    } else {
+        AC_AttitudeControl::input_thrust_vector_rate_heading(thrust_vector, heading_rate_cds, slew_yaw);
+    }
+
+}
+
+// Command a thrust vector, heading and heading rate
+void AC_AttitudeControl_Heli::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
+{
+    // convert thrust vector to a roll and pitch angles
+    Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
+
+    float euler_roll_angle_cd = degrees(angle_target.x) * 100.0f;
+    if (_inverted_flight) {
+        euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
+        // note that we are throwing away heading rate here
+        AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, degrees(angle_target.y) * 100.0f, heading_angle_cd, true);
+    } else {
+        AC_AttitudeControl::input_thrust_vector_heading(thrust_vector, heading_angle_cd, heading_rate_cds);
+    }
+
+}

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -102,6 +102,11 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
     // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
     void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
 
+    // Command a thrust vector in the earth frame and a heading angle and/or rate
+    void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true) override;
+    void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override;
+    void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) override {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
+
     // enable/disable inverted flight
     void set_inverted_flight(bool inverted) override {
         if (_inverted_flight != inverted) {

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -5,6 +5,7 @@
 #include <AP_Motors/AP_Motors.h>    // motors library
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -823,20 +824,20 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output
 void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
 {
     // remove terrain offsets for flat earth assumption
-    _pos_target.z -= _pos_offset_z;
+/*    _pos_target.z -= _pos_offset_z;
     _vel_desired.z -= _vel_offset_z;
     _accel_desired.z -= _accel_offset_z;
-
+*/
     float vel_temp = vel;
     input_vel_accel_z(vel_temp, 0.0);
 
-    // update the vertical position, velocity and acceleration offsets
+/*    // update the vertical position, velocity and acceleration offsets
     update_pos_offset_z(_pos_offset_target_z);
 
     // add terrain offsets
     _pos_target.z += _pos_offset_z;
     _vel_desired.z += _vel_offset_z;
-    _accel_desired.z += _accel_offset_z;
+    _accel_desired.z += _accel_offset_z; */
 }
 
 /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
@@ -940,7 +941,10 @@ void AC_PosControl::update_z_controller()
 
     // Velocity Controller
 
-    const float curr_vel_z = _inav.get_velocity_z_up_cms();
+    float curr_vel_z = _inav.get_velocity_z_up_cms();
+        if (_ahrs.roll_sensor > 9000 && _ahrs.roll_sensor < -9000) {
+            curr_vel_z = -curr_vel_z;
+        }
     _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
     _accel_target.z *= AP::ahrs().getControlScaleZ();
 
@@ -952,6 +956,14 @@ void AC_PosControl::update_z_controller()
     // Calculate vertical acceleration
     const float z_accel_meas = get_z_accel_cmss();
 
+    static float prntcnt = 2000;
+    if (prntcnt < 1) {
+        gcs().send_text(MAV_SEVERITY_INFO, "z_accel = %f", z_accel_meas);
+        prntcnt = 2000;
+    } else {
+        prntcnt -= 1;
+    }
+
     // ensure imax is always large enough to overpower hover throttle
     if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
         _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -393,7 +393,13 @@ class AC_PosControl
     void standby_xyz_reset();
 
     // get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
-    float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; }
+    float get_z_accel_cmss() const {
+        if (_ahrs.roll_sensor < 9000 && _ahrs.roll_sensor > -9000) {
+            return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f;
+        } else {
+            return (_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f;
+        }
+    }
 
     static const struct AP_Param::GroupInfo var_info[];