From 129e6d73debca5653911867e9db54990c02591bb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Nov 2012 16:17:52 -0800 Subject: Changed yaw position control to yaw speed control for multirotors, tested with ardrone --- apps/mavlink/mavlink_receiver.c | 5 ++++ .../multirotor_att_control_main.c | 14 +++++++--- .../multirotor_attitude_control.c | 12 +++++++- apps/sensors/sensors.cpp | 32 ++++++++++++---------- 4 files changed, 44 insertions(+), 19 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 3e485274e..4790a673d 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -335,6 +335,11 @@ handle_message(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* get a copy first, to prevent altering values that are not sent by the mavlink command */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); + mc.timestamp = rc_hil.timestamp; mc.roll = man.x / 1000.0f; mc.pitch = man.y / 1000.0f; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 337a45215..91439d36d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -155,6 +155,9 @@ mc_thread_main(int argc, char *argv[]) /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; + /* store if we stopped a yaw movement */ + bool first_time_after_yaw_speed_control = true; + /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; @@ -249,7 +252,7 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; + att_sp.yaw_tait_bryan = att.yaw; } static bool rc_loss_first_time = true; @@ -294,16 +297,19 @@ mc_thread_main(int argc, char *argv[]) if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { rates_sp.yaw = manual.yaw; control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { - att_sp.yaw_body = 0.0f; + rates_sp.yaw = 0.0f; + if(first_time_after_yaw_speed_control) { + att_sp.yaw_tait_bryan = att.yaw; + first_time_after_yaw_speed_control = false; + } control_yaw_position = true; } } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); - - //rates_sp.yaw = manual.yaw; } } /* STEP 2: publish the result to the vehicle actuators */ diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index bf908c6dd..87eeaced5 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static bool initialized = false; + static float yaw_error; + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -218,7 +220,15 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if(control_yaw_position) { /* control yaw rate */ - rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); + //rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed); + yaw_error = att->yaw - att_sp->yaw_tait_bryan; + if ((double)yaw_error > M_PI) { + yaw_error -= M_PI; + } else if ((double)yaw_error < -M_PI) { + yaw_error += M_PI; + } + + rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); } rates_sp->thrust = att_sp->thrust; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eea51cc1e..aa1362d3a 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -151,6 +151,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ @@ -341,6 +342,7 @@ Sensors::Sensors() : _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), + _manual_control_sub(-1), /* publications */ _sensor_pub(-1), @@ -903,6 +905,9 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; + /* get a copy first, to prevent altering values */ + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) return; @@ -1023,6 +1028,7 @@ Sensors::task_main() _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -1052,20 +1058,18 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* advertise the manual_control topic */ - { - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - } + struct manual_control_setpoint_s manual_control; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; + manual_control.roll = 0.0f; + manual_control.pitch = 0.0f; + manual_control.yaw = 0.0f; + manual_control.throttle = 0.0f; + manual_control.aux1_cam_pan_flaps = 0.0f; + manual_control.aux2_cam_tilt = 0.0f; + manual_control.aux3_cam_zoom = 0.0f; + manual_control.aux4_cam_roll = 0.0f; + + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); /* advertise the rc topic */ { -- cgit v1.2.3