diff --git a/app/rdd2/src/manual.c b/app/rdd2/src/manual.c index 83c96053..d8daae48 100644 --- a/app/rdd2/src/manual.c +++ b/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++) { diff --git a/app/rdd2/src/mixing.c b/app/rdd2/src/mixing.c index 72bfb396..f0e3364a 100644 --- a/app/rdd2/src/mixing.c +++ b/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; diff --git a/app/rdd2/src/velocity.c b/app/rdd2/src/velocity.c index 32a71337..659e7452 100644 --- a/app/rdd2/src/velocity.c +++ b/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); }