fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 09:53:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed Rate controller

Browse Source
This commit is contained in:
tnaegeli
2012-10-04 09:28:04 +02:00
parent 147c5bb664
commit 733975ed2d
4 changed files with 63 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+8 -7
View File
@@ -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); //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) { if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
current_status.flag_control_attitude_enabled = true; current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false; current_status.flag_control_rates_enabled = true;
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
current_status.flag_control_attitude_enabled = true; current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false; current_status.flag_control_rates_enabled = true;
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
//update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else { } else {
current_status.flag_control_attitude_enabled = true; current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false; current_status.flag_control_rates_enabled = true;
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} }
apps/multirotor_att_control/multirotor_att_control_main.c
+47 -28
View File
@@ -94,24 +94,26 @@ static void *rate_control_thread_main(void *arg)
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); 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_attitude_s vehicle_attitude;
struct vehicle_rates_setpoint_s rates_sp; struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp)); memset(&rates_sp, 0, sizeof(rates_sp));
float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
while (!thread_should_exit) { while (true) {
/* rate control at maximum rate */ // /* rate control at maximum rate */
/* wait for a sensor update, check for exit condition every 1000 ms */ // /* wait for a sensor update, check for exit condition every 1000 ms */
int ret = poll(&fds, 1, 1000); // int ret = poll(&fds, 1, 1000);
//
if (ret < 0) { // if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */ // /* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) { // } else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */ // /* XXX this means no sensor data - should be critical or emergency */
printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); // printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
} else { // } else {
/* get current angular rate */ /* get current angular rate */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude); orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude);
/* get current rate setpoint */ /* get current rate setpoint */
@@ -124,18 +126,19 @@ static void *rate_control_thread_main(void *arg)
/* perform local lowpass */ /* perform local lowpass */
/* apply controller */ /* apply controller */
if (state.flag_control_rates_enabled) { // if (state.flag_control_rates_enabled) {
/* lowpass gyros */ /* lowpass gyros */
// XXX // XXX
gyro_lp[0] = vehicle_attitude.rollspeed; gyro_lp[0] = vehicle_attitude.rollspeed;
gyro_lp[1] = vehicle_attitude.pitchspeed; gyro_lp[1] = vehicle_attitude.pitchspeed;
gyro_lp[2] = vehicle_attitude.yawspeed; 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); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} // }
} // }
usleep(2000); usleep(5000);
} }
return NULL; return NULL;
@@ -184,7 +187,7 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f; 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 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); 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 */ /* ready, spawn pthread */
pthread_attr_t rate_control_attr; pthread_attr_t rate_control_attr;
pthread_attr_init(&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_t rate_control_thread;
pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); 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 */ /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */ // /* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { // if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators); // multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { // } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); // multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); // 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); 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", mc_task = task_spawn("multirotor_att_control",
SCHED_RR, SCHED_RR,
SCHED_PRIORITY_MAX - 15, SCHED_PRIORITY_MAX - 15,
4096, 6000,
mc_thread_main, mc_thread_main,
NULL); NULL);
exit(0); exit(0);
apps/multirotor_att_control/multirotor_rate_control.c
+6 -21
View File
@@ -150,11 +150,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static int motor_skip_counter = 0; 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_params p;
static struct mc_rate_control_param_handles h; 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) { if (motor_skip_counter % 500 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); 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 */ /* 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 setPitchRate=rate_sp->pitch;
float setYawRate=rate_sp->yaw; float setYawRate=rate_sp->yaw;
//x-axis
//x-axis setpointXrate=p.attrate_p*(setRollRate-rates[0]);
setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); //Y-axis
//Y-axis setpointYrate=p.attrate_p*(setPitchRate-rates[1]);
setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); //Z-axis
//Z-axis setpointZrate=p.yawrate_p*(setYawRate-rates[2]);
setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]);
actuators->control[0] = setpointXrate; //roll actuators->control[0] = setpointXrate; //roll
actuators->control[1] = setpointYrate; //pitch actuators->control[1] = setpointYrate; //pitch
nuttx/configs/px4fmu/nsh/defconfig
+2 -2
View File
@@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256
CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_TERMIOS=y
CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_SERIAL_CONSOLE_REINIT=y
CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n
CONFIG_UART5_SERIAL_CONSOLE=n CONFIG_UART5_SERIAL_CONSOLE=y
CONFIG_USART6_SERIAL_CONSOLE=n CONFIG_USART6_SERIAL_CONSOLE=n
#Mavlink messages can be bigger than 128 #Mavlink messages can be bigger than 128