forked from Archive/PX4-Autopilot
Fixed inner yaw rate loop
This commit is contained in:
parent
dfae108e6a
commit
2a06b66845
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue