Incorporated feedforward moment into all aspects of the software

KumarRobotics · Feb 10, 2017 · d335e01 · d335e01
1 parent 58cea18
commit d335e01
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 3 deletions.
diff --git a/asctec_firmware/sdk.c b/asctec_firmware/sdk.c
@@ -135,6 +135,11 @@ void SDK_mainloop(void)
     const float fy = 2e-3f*SO3_cmd_input.force[1];
     const float fz = 2e-3f*SO3_cmd_input.force[2];
 
+    // Feedforward moments
+    const float ffmomx = 8e-6f*SO3_cmd_input.ff_moment[0];
+    const float ffmomy = 8e-6f*SO3_cmd_input.ff_moment[1];
+    const float ffmomz = 8e-6f*SO3_cmd_input.ff_moment[2];
+
     float qw = 8e-3f*SO3_cmd_input.des_qw;
     float qx = 8e-3f*SO3_cmd_input.des_qx;
     float qy = 8e-3f*SO3_cmd_input.des_qy;
@@ -254,9 +259,9 @@ void SDK_mainloop(void)
     const float in3 = Om1*(I[1][0]*Om1 + I[1][1]*Om2 + I[1][2]*Om3) - Om2*(I[0][0]*Om1 + I[0][1]*Om2 + I[0][2]*Om3);
 
     const float uF = force;
-    const float uM1 = -kR1*eR1 - kOm1*eOm1 + in1;
-    const float uM2 = -kR2*eR2 - kOm2*eOm2 + in2;
-    const float uM3 = -kR3*eR3 - kOm3*eOm3 + in3;
+    const float uM1 = -kR1*eR1 - kOm1*eOm1 + in1 + ffmomx;
+    const float uM2 = -kR2*eR2 - kOm2*eOm2 + in2 + ffmomy;
+    const float uM3 = -kR3*eR3 - kOm3*eOm3 + in3 + ffmomz;
 
     const float kf_inv = 1/kf;
     const float kfl_inv = kf_inv*(1/LENGTH);

diff --git a/asctec_firmware/sdk.h b/asctec_firmware/sdk.h
@@ -42,6 +42,7 @@ struct SO3_CMD_INPUT
 {
   // Scaling factors when decoding
   int16_t force[3]; // /500 (range: +-65 N on each axis)
+  int16_t ff_moment[3]
   int8_t des_qx, des_qy, des_qz, des_qw; // /125
   int16_t angvel_x, angvel_y, angvel_z; // /1000 (range: +-32 rad/s)
   uint8_t kR[3]; // /50 (range: 0-5.1)

diff --git a/quadrotor_msgs/include/quadrotor_msgs/comm_types.h b/quadrotor_msgs/include/quadrotor_msgs/comm_types.h
@@ -6,6 +6,7 @@ struct SO3_CMD_INPUT
 {
   // Scaling factors when decoding
   int16_t force[3]; // /500 (range: +-65 N on each axis)
+  int16_t ff_moment[3]; // 12500
   int8_t des_qx, des_qy, des_qz, des_qw; // /125
   int16_t angvel_x, angvel_y, angvel_z; // /1000 (range: +-32 rad/s)
   uint8_t kR[3]; // /50 (range: 0-5.1)

diff --git a/quadrotor_msgs/msg/SO3Command.msg b/quadrotor_msgs/msg/SO3Command.msg
@@ -1,5 +1,6 @@
 Header header
 geometry_msgs/Vector3 force
+geometry_msgs/Vector3 ff_moment
 geometry_msgs/Quaternion orientation
 geometry_msgs/Vector3 angular_velocity
 quadrotor_msgs/AuxCommand aux

diff --git a/quadrotor_msgs/src/encode_msgs.cpp b/quadrotor_msgs/src/encode_msgs.cpp
@@ -13,6 +13,10 @@ void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command,
   so3_cmd_input.force[1] = so3_command.force.y*500;
   so3_cmd_input.force[2] = so3_command.force.z*500;
 
+  so3_cmd_input.ff_moment[0] = so3_command.ff_moment.x*12500;
+  so3_cmd_input.ff_moment[1] = so3_command.ff_moment.y*12500;
+  so3_cmd_input.ff_moment[2] = so3_command.ff_moment.z*12500;
+
   so3_cmd_input.des_qx = so3_command.orientation.x*125;
   so3_cmd_input.des_qy = so3_command.orientation.y*125;
   so3_cmd_input.des_qz = so3_command.orientation.z*125;