From 2a06b66845542b05e3cad3d21099e33adc213227 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Oct 2012 10:56:55 +0200 Subject: [PATCH] Fixed inner yaw rate loop --- apps/commander/commander.c | 6 - apps/commander/state_machine_helper.c | 13 +- .../multirotor_att_control_main.c | 124 +++++++----------- .../multirotor_rate_control.c | 79 +++++------ 4 files changed, 85 insertions(+), 137 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 16f74f43c9..36e484f107 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1307,18 +1307,12 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1d24d6a6d..1e82bc417b 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -501,9 +501,10 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = true; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -516,7 +517,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -529,7 +532,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5e492268e3..4b386ffd4f 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -82,64 +82,6 @@ static orb_advert_t actuator_pub; static struct vehicle_status_s state; -/** - * Perform rate control right after gyro reading - */ -static void *rate_control_thread_main(void *arg) -{ - prctl(PR_SET_NAME, "mc rate control", getpid()); - - struct actuator_controls_s actuators; - - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - struct vehicle_attitude_s vehicle_attitude; - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; - - while (!thread_should_exit) { - /* rate control at maximum rate */ - /* wait for a sensor update, check for exit condition every 1000 ms */ - int ret = poll(&fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); - } else { - /* get current angular rate */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude); - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); - } - - /* perform local lowpass */ - - /* apply controller */ - if (state.flag_control_rates_enabled) { - /* lowpass gyros */ - // XXX - gyro_lp[0] = vehicle_attitude.rollspeed; - gyro_lp[1] = vehicle_attitude.pitchspeed; - gyro_lp[2] = vehicle_attitude.yawspeed; - - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - } - - return NULL; -} - static int mc_thread_main(int argc, char *argv[]) { @@ -186,6 +128,7 @@ mc_thread_main(int argc, char *argv[]) actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -193,13 +136,6 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); - /* ready, spawn pthread */ - pthread_attr_t rate_control_attr; - pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); - pthread_t rate_control_thread; - pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -232,13 +168,24 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_manual_enabled) { /* manual inputs, from RC control or joystick */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller - /* set yaw rate */ - rates_sp.yaw = manual.yaw; - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); + + if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } + + if (state.flag_control_attitude_enabled) { + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + } /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -277,12 +224,33 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /* run attitude controller */ - if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + // if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + // multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + // orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + // } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + // multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + // orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + // } + + + if (state.flag_control_rates_enabled) { + + float gyro[3] = {0.0f, 0.0f, 0.0f}; + + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* apply controller */ + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } @@ -306,8 +274,6 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - pthread_join(rate_control_thread, NULL); - fflush(stdout); exit(0); } diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index b4eb3469b5..d332660f68 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -141,11 +141,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; - // static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; - static PID_t pitch_controller; - static PID_t roll_controller; - static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -155,37 +150,25 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (initialized == false) { parameters_init(&h); parameters_update(&h, &p); - - pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - initialized = true; } /* load new parameters with lower rate */ - if (motor_skip_counter % 250 == 0) { + if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - /* apply parameters */ - pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu); - pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); - pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); + printf("p.yawrate_p: %8.4f", (double)p.yawrate_p); } /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, - rates[1], 0.0f, deltaT); + float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch); /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll, - rates[0], 0.0f, deltaT); + float roll_control = p.attrate_p * deltaT * (rates[0] - rate_sp->roll); + /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + float yaw_rate_control = p.yawrate_p * deltaT * (rates[2] - rate_sp->yaw); /* * compensate the vertical loss of thrust @@ -215,37 +198,37 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* compensate thrust vector for roll / pitch contributions */ motor_thrust *= zcompensation; - /* limit yaw rate output */ - if (yaw_rate_control > p.yawrate_lim) { - yaw_rate_control = p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } + // /* limit yaw rate output */ + // if (yaw_rate_control > p.yawrate_lim) { + // yaw_rate_control = p.yawrate_lim; + // yaw_speed_controller.saturated = 1; + // } - if (yaw_rate_control < -p.yawrate_lim) { - yaw_rate_control = -p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } + // if (yaw_rate_control < -p.yawrate_lim) { + // yaw_rate_control = -p.yawrate_lim; + // yaw_speed_controller.saturated = 1; + // } - if (pitch_control > p.attrate_lim) { - pitch_control = p.attrate_lim; - pitch_controller.saturated = 1; - } + // if (pitch_control > p.attrate_lim) { + // pitch_control = p.attrate_lim; + // pitch_controller.saturated = 1; + // } - if (pitch_control < -p.attrate_lim) { - pitch_control = -p.attrate_lim; - pitch_controller.saturated = 1; - } + // if (pitch_control < -p.attrate_lim) { + // pitch_control = -p.attrate_lim; + // pitch_controller.saturated = 1; + // } - if (roll_control > p.attrate_lim) { - roll_control = p.attrate_lim; - roll_controller.saturated = 1; - } + // if (roll_control > p.attrate_lim) { + // roll_control = p.attrate_lim; + // roll_controller.saturated = 1; + // } - if (roll_control < -p.attrate_lim) { - roll_control = -p.attrate_lim; - roll_controller.saturated = 1; - } + // if (roll_control < -p.attrate_lim) { + // roll_control = -p.attrate_lim; + // roll_controller.saturated = 1; + // } actuators->control[0] = roll_control; actuators->control[1] = pitch_control;