AP_Mount: accumulate delta_angles properly for joint angle complement…

…ary filter
3drobotics · Jul 16, 2015 · 7ae229e · 7ae229e
1 parent cc59a03
commit 7ae229e
Show file tree

Hide file tree

Showing 7 changed files with 56 additions and 5 deletions.
diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
@@ -43,6 +43,35 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
     }
 }
 
+void AP_Gimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
+    const AP_InertialSensor &ins = _ahrs.get_ins();
+
+    if (ins_index < ins.get_gyro_count()) {
+        if (!ins.get_delta_angle(ins_index,dAng)) {
+            dAng = ins.get_gyro(ins_index) / ins.get_sample_rate();
+        }
+    }
+}
+
+void AP_Gimbal::update_fast() {
+    const AP_InertialSensor &ins = _ahrs.get_ins();
+
+    if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) {
+        // dual gyro mode - average first two gyros
+        Vector3f dAng;
+        readVehicleDeltaAngle(0, dAng);
+        vehicle_delta_angles += dAng*0.5f;
+        readVehicleDeltaAngle(1, dAng);
+        vehicle_delta_angles += dAng*0.5f;
+    } else {
+        // single gyro mode - one of the first two gyros are unhealthy or don't exist
+        // just read primary gyro
+        Vector3f dAng;
+        readVehicleDeltaAngle(ins.get_primary_gyro(), dAng);
+        vehicle_delta_angles += dAng;
+    }
+}
+
 void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
 {
     _last_report_msg_ms = hal.scheduler->millis();
@@ -71,8 +100,6 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
 
     // get complementary filter inputs
     vehicle_to_gimbal_quat.from_vector312(_measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
-    vehicle_delta_angles = (last_vehicle_gyro + _ahrs.get_gyro())*_measurement.delta_time*0.5f;
-    last_vehicle_gyro = _ahrs.get_gyro();
 }
 
 void AP_Gimbal::update_state()
@@ -119,6 +146,8 @@ void AP_Gimbal::update_joint_angle_est()
     filtered_joint_angles += (_measurement.joint_angles-filtered_joint_angles)*alpha;
 
     vehicle_to_gimbal_quat_filt.from_vector312(filtered_joint_angles.x,filtered_joint_angles.y,filtered_joint_angles.z);
+
+    vehicle_delta_angles.zero();
 }
 
 void AP_Gimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates)

diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h
@@ -37,14 +37,15 @@ class AP_Gimbal
         vehicle_delta_angles(),
         vehicle_to_gimbal_quat(),
         vehicle_to_gimbal_quat_filt(),
-        filtered_joint_angles(),
-        last_vehicle_gyro()
+        filtered_joint_angles()
     {
     }
 
     void    update_target(Vector3f newTarget);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
 
+    void update_fast();
+
     bool present();
 
     Vector3f getGimbalEstimateEF();
@@ -99,13 +100,14 @@ class AP_Gimbal
     void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates);
     void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel);
 
+    void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng);
+
     // joint angle filter states
     Vector3f vehicle_delta_angles;
 
     Quaternion vehicle_to_gimbal_quat;
     Quaternion vehicle_to_gimbal_quat_filt;
     Vector3f filtered_joint_angles;
-    Vector3f last_vehicle_gyro;
 
     uint32_t _last_report_msg_ms;
 };

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
@@ -549,6 +549,15 @@ void AP_Mount::control_msg(uint8_t instance, mavlink_message_t *msg)
     _backends[instance]->control_msg(msg);
 }
 
+void AP_Mount::update_fast()
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != NULL) {
+            _backends[instance]->update_fast();
+        }
+    }
+}
+
 /// Return mount status information
 void AP_Mount::status_msg(mavlink_channel_t chan)
 {

diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
@@ -92,6 +92,8 @@ class AP_Mount
     void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); }
     void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode);
 
+    void update_fast();
+
     // set_mode_to_default - restores the mode to it's default mode held in the MNT_DEFLT_MODE parameter
     //      this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
     void set_mode_to_default() { set_mode_to_default(_primary); }

diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -71,6 +71,8 @@ class AP_Mount_Backend
     // send a GIMBAL_REPORT message to the GCS
     virtual void send_gimbal_report(mavlink_channel_t chan) {}
 
+    virtual void update_fast() {}
+
 protected:
 
     // update_targets_from_rc - updates angle targets (i.e. _angle_ef_target_rad) using input from receiver

diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp
@@ -67,6 +67,11 @@ void AP_Mount_MAVLink::Log_Write_Gimbal(AP_Gimbal &gimbal)
     _frontend._dataflash->WriteBlock(&pkt2, sizeof(pkt2));
 }
 
+void AP_Mount_MAVLink::update_fast()
+{
+    _gimbal.update_fast();
+}
+
 // update mount position - should be called periodically
 void AP_Mount_MAVLink::update()
 {

diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h
@@ -47,6 +47,8 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
     // send a GIMBAL_REPORT message to the GCS
     virtual void send_gimbal_report(mavlink_channel_t chan);
 
+    virtual void update_fast();
+
 private:
     // internal variables
     bool _initialised;              // true once the driver has been initialised