diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 09:28:04 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 09:28:04 +0200 |
commit | 733975ed2d7b5906e35dbdebad52ee8fa9d92fd6 (patch) | |
tree | c567798e5310693457137bfd0f55fb94bdc8dd53 /apps | |
parent | 147c5bb66429c3d1b7c693d7419ca153ae49336c (diff) | |
download | px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.tar.gz px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.tar.bz2 px4-firmware-733975ed2d7b5906e35dbdebad52ee8fa9d92fd6.zip |
fixed Rate controller
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 15 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 75 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 27 |
3 files changed, 61 insertions, 56 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 77e4da850..124ac8aeb 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1307,18 +1307,19 @@ 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; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; 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); + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + //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; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ba7f08bc8..6b32a8ca3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -94,24 +94,26 @@ static void *rate_control_thread_main(void *arg) 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 }; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + +// 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 { + while (true) { +// /* 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 */ @@ -124,18 +126,19 @@ static void *rate_control_thread_main(void *arg) /* perform local lowpass */ /* apply controller */ - if (state.flag_control_rates_enabled) { +// 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); + //multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + printf(".\n"); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - usleep(2000); +// } +// } + usleep(5000); } return NULL; @@ -184,7 +187,7 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t 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); @@ -197,7 +200,7 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_attr_setstacksize(&rate_control_attr, 4096); pthread_t rate_control_thread; pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); @@ -277,15 +280,31 @@ 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); - 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); - } +// /* run attitude controller */ +// 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); +// } + + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + gyro_lp[0] = att.rollspeed; + gyro_lp[1] = att.pitchspeed; + gyro_lp[2] = att.yawspeed; + + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); perf_end(mc_loop_perf); } @@ -356,7 +375,7 @@ int multirotor_att_control_main(int argc, char *argv[]) mc_task = task_spawn("multirotor_att_control", SCHED_RR, SCHED_PRIORITY_MAX - 15, - 4096, + 6000, mc_thread_main, NULL); exit(0); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 2a57f93f0..1d400f51b 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -150,11 +150,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; @@ -166,13 +161,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 500 == 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); } /* calculate current control outputs */ @@ -183,15 +171,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, float setPitchRate=rate_sp->pitch; float setYawRate=rate_sp->yaw; - - //x-axis - setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); - //Y-axis - setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); - //Z-axis - setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]); - - + //x-axis + setpointXrate=p.attrate_p*(setRollRate-rates[0]); + //Y-axis + setpointYrate=p.attrate_p*(setPitchRate-rates[1]); + //Z-axis + setpointZrate=p.yawrate_p*(setYawRate-rates[2]); actuators->control[0] = setpointXrate; //roll actuators->control[1] = setpointYrate; //pitch |