AP_Mount: Create new copter parameters to hold gimbal settings

3drobotics · Mar 31, 2015 · ab1ef41 · ab1ef41
1 parent d333e69
commit ab1ef41
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 1 deletion.
diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
@@ -199,6 +199,11 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
     // @User: Standard
     AP_GROUPINFO("_TYPE", 19, AP_Mount, state[0]._type, 0),
 
+    AP_GROUPINFO("_OFF_JNT", 20, AP_Mount, state[0]._gimbalParams.joint_angles_offsets, 0),
+    AP_GROUPINFO("_OFF_ACC",  21, AP_Mount, state[0]._gimbalParams.delta_velocity_offsets, 0),
+    AP_GROUPINFO("_OFF_GYRO", 22, AP_Mount, state[0]._gimbalParams.delta_angles_offsets, 0),
+    AP_GROUPINFO("_K_RATE", 23, AP_Mount, state[0]._gimbalParams.K_gimbalRate, 5.0f),
+
     // 20 ~ 24 reserved for future parameters
 
 #if AP_MOUNT_MAX_INSTANCES > 1

diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
@@ -65,6 +65,13 @@ class AP_Mount
         Mount_Type_SToRM32 = 4          /// SToRM32 mount
     };
 
+    struct gimbal_params {
+        AP_Vector3f     delta_angles_offsets;
+        AP_Vector3f     delta_velocity_offsets;
+        AP_Vector3f     joint_angles_offsets;
+        AP_Float        K_gimbalRate;
+    };
+
     // Constructor
     AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc);
 
@@ -168,6 +175,9 @@ class AP_Mount
 
         MAV_MOUNT_MODE  _mode;              // current mode (see MAV_MOUNT_MODE enum)
         struct Location _roi_target;        // roi target location
+
+        struct gimbal_params _gimbalParams;
+
     } state[AP_MOUNT_MAX_INSTANCES];
 };
 

diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp
@@ -13,7 +13,7 @@
 AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
     AP_Mount_Backend(frontend, state, instance),
     _initialised(false),
-    _gimbal(frontend._ahrs, 1, MAV_COMP_ID_GIMBAL)
+    _gimbal(frontend._ahrs, 1, MAV_COMP_ID_GIMBAL, _state._gimbalParams)
 {}
 
 // init - performs any required initialisation for this instance