zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Minor cleanups in attitude control

sbg
Lorenz Meier 12 years ago
parent
d9d002f3aa
commit
c2abe3997c
2 changed files with 146 additions and 119 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 261
      apps/multirotor_att_control/multirotor_att_control_main.c
  2. 4
      apps/multirotor_att_control/multirotor_attitude_control.c

261
apps/multirotor_att_control/multirotor_att_control_main.c
Unescape View File

@ -65,6 +65,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
@ -104,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */ /* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -118,7 +120,10 @@ mc_thread_main(int argc, char *argv[])
* rate-limit the attitude subscription to 200Hz to pace our loop * rate-limit the attitude subscription to 200Hz to pace our loop
* orb_set_interval(att_sub, 5); * orb_set_interval(att_sub, 5);
*/ */
struct pollfd fds = { .fd = att_sub, .events = POLLIN }; struct pollfd fds[2] = {
{ .fd = att_sub, .events = POLLIN },
{ .fd = param_sub, .events = POLLIN }
};
/* publish actuator controls */ /* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@ -131,7 +136,9 @@ mc_thread_main(int argc, char *argv[])
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */ /* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */ /* welcome user */
printf("[multirotor_att_control] starting\n"); printf("[multirotor_att_control] starting\n");
@ -140,143 +147,163 @@ mc_thread_main(int argc, char *argv[])
bool flag_control_manual_enabled = false; bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false; bool flag_control_attitude_enabled = false;
bool flag_system_armed = false; bool flag_system_armed = false;
bool man_yaw_zero_once = false;
while (!thread_should_exit) { while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */ /* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500); int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */
// XXX no params here yet
}
perf_begin(mc_loop_perf); /* only run controller if attitude changed */
if (fds[0].revents & POLLIN) {
/* get a local copy of system state */ perf_begin(mc_loop_perf);
bool updated;
orb_check(state_sub, &updated); /* get a local copy of system state */
if (updated) { bool updated;
orb_copy(ORB_ID(vehicle_status), state_sub, &state); orb_check(state_sub, &updated);
} if (updated) {
/* get a local copy of manual setpoint */ orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); }
/* get a local copy of attitude */ /* get a local copy of manual setpoint */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude setpoint */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of rates setpoint */ /* get a local copy of attitude setpoint */
orb_check(setpoint_sub, &updated); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
if (updated) { /* get a local copy of rates setpoint */
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); orb_check(setpoint_sub, &updated);
} if (updated) {
/* get a local copy of the current sensor values */ orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); }
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/** STEP 1: Define which input is the dominating control input */
if (state.flag_control_offboard_enabled) {
/* offboard inputs */ /** STEP 1: Define which input is the dominating control input */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { if (state.flag_control_offboard_enabled) {
rates_sp.roll = offboard_sp.p1; /* offboard inputs */
rates_sp.pitch = offboard_sp.p2; if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.yaw = offboard_sp.p3; rates_sp.roll = offboard_sp.p1;
rates_sp.thrust = offboard_sp.p4; rates_sp.pitch = offboard_sp.p2;
// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
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);
}
/* decide wether we want rate or position input */
}
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
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(); rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); }
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1; if (state.flag_control_attitude_enabled) {
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3; /* initialize to current yaw if switching to manual or att control */
att_sp.thrust = offboard_sp.p4; if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
// printf("thrust_att=%8.4f\n",offboard_sp.p4); state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) {
att_sp.yaw_body = att.yaw;
}
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* only move setpoint if manual input is != 0 */
// XXX turn into param
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
} else if (manual.throttle <= 0.3f) {
att_sp.yaw_body = att.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);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */ /* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} }
/* decide wether we want rate or position input */
} }
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
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) {
/* initialize to current yaw if switching to manual or att control */ /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || if (state.flag_control_attitude_enabled) {
state.flag_control_manual_enabled != flag_control_manual_enabled || multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
state.flag_system_armed != flag_system_armed) { orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
att_sp.yaw_body = att.yaw;
} }
att_sp.roll_body = manual.roll; /* measure in what intervals the controller runs */
att_sp.pitch_body = manual.pitch; perf_count(mc_interval_perf);
/* only move setpoint if manual input is != 0 */ float gyro[3];
// XXX turn into param
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
} else if (manual.throttle <= 0.3f) {
att_sp.yaw_body = att.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);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
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);
}
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* 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);
}
if (state.flag_control_rates_enabled) { /* apply controller */
gyro[0] = att.rollspeed;
float gyro[3]; gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* 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); multirotor_control_rates(&rates_sp, gyro, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
/* update state */ /* update state */
flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled;
flag_system_armed = state.flag_system_armed; flag_system_armed = state.flag_system_armed;
perf_end(mc_loop_perf); perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* end of poll return value check */
} }
printf("[multirotor att control] stopping, disarming motors.\n"); printf("[multirotor att control] stopping, disarming motors.\n");

4
apps/multirotor_att_control/multirotor_attitude_control.c
Unescape View File

@ -199,9 +199,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
/* apply parameters */ //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
} }

Write Preview
Loading…
Cancel
Save