Smooth manual flight to be used with the fox-glove-joy bluetooth controller support.

Signed-off-by: Chase Hamdan <chamdan@purdue.edu>

Author: chamdanedu

app/rdd2/src/manual.c

@@ -80,9 +80,9 @@ static void rdd2_manual_entry_point(void* p0, void* p1, void* p2)
         // set normalized inputs
         ctx->actuators_manual.normalized_count = 4;
         ctx->actuators_manual.normalized[0] = -ctx->joy.axes[JOY_AXES_ROLL];
-        ctx->actuators_manual.normalized[1] = ctx->joy.axes[JOY_AXES_PITCH];
-        ctx->actuators_manual.normalized[2] = ctx->joy.axes[JOY_AXES_YAW];
-        ctx->actuators_manual.normalized[3] = ctx->joy.axes[JOY_AXES_THRUST];
+        ctx->actuators_manual.normalized[1] = -ctx->joy.axes[JOY_AXES_PITCH];
+        ctx->actuators_manual.normalized[2] = -ctx->joy.axes[JOY_AXES_YAW];
+        ctx->actuators_manual.normalized[3] = ctx->joy.axes[JOY_AXES_THRUST]*0.5 + 0.5;
 
         // saturation
         for (int i = 0; i < 4; i++) {

app/rdd2/src/mixing.c

@@ -12,7 +12,7 @@ void rdd2_set_actuators(synapse_msgs_Actuators* msg, double roll, double pitch,
     msg->header.seq++;
     strncpy(msg->header.frame_id, "odom", sizeof(msg->header.frame_id) - 1);
 
-    const float k = 10;
+    const float k = 1600;
 
     msg->position_count = 0;
     msg->velocity_count = 4;

app/rdd2/src/velocity.c

@@ -82,14 +82,14 @@ static void update_cmd_vel(context* ctx)
     }
     */
 
-    double roll_rate_cmd = ctx->actuators_manual.velocity[0];
-    double pitch_rate_cmd = ctx->actuators_manual.velocity[1];
-    double yaw_rate_cmd = ctx->actuators_manual.velocity[2];
-    double thrust_cmd = ctx->actuators_manual.velocity[3];
-
-    double roll = 1.0 * (roll_rate_cmd - ctx->imu.angular_velocity.x);
-    double pitch = 1.0 * (pitch_rate_cmd - ctx->imu.angular_velocity.y);
-    double yaw = 1.0 * (yaw_rate_cmd - ctx->imu.angular_velocity.z);
+    double roll_rate_cmd = ctx->actuators_manual.normalized[0];
+    double pitch_rate_cmd = ctx->actuators_manual.normalized[1];
+    double yaw_rate_cmd = ctx->actuators_manual.normalized[2];
+    double thrust_cmd = ctx->actuators_manual.normalized[3];
+
+    double roll = 0.01 * (roll_rate_cmd - ctx->imu.angular_velocity.x);
+    double pitch = 0.01 * (pitch_rate_cmd + ctx->imu.angular_velocity.y);
+    double yaw = 0.05 * (yaw_rate_cmd + ctx->imu.angular_velocity.z);
 
     rdd2_set_actuators(&ctx->actuators, roll, pitch, yaw, thrust_cmd);
 }