fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 00:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved from raw gyro to estimated angular rate from EKF for rate control

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-03 15:02:47 +02:00
parent 053ce0e2f8
commit beca2b072e
1 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+10 -9
View File
@@ -91,12 +91,12 @@ static void *rate_control_thread_main(void *arg)
struct actuator_controls_s actuators; struct actuator_controls_s actuators;
int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); 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 = gyro_sub, .events = POLLIN }; struct pollfd fds = { .fd = att_sub, .events = POLLIN };
struct gyro_report gyro_report; 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};
@@ -110,10 +110,11 @@ static void *rate_control_thread_main(void *arg)
/* 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 data */ /* get current angular rate */
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude);
/* get current rate setpoint */
bool rates_sp_valid = false; bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid); orb_check(rates_sp_sub, &rates_sp_valid);
if (rates_sp_valid) { if (rates_sp_valid) {
@@ -126,9 +127,9 @@ static void *rate_control_thread_main(void *arg)
if (state.flag_control_rates_enabled) { if (state.flag_control_rates_enabled) {
/* lowpass gyros */ /* lowpass gyros */
// XXX // XXX
gyro_lp[0] = gyro_report.x; gyro_lp[0] = vehicle_attitude.rollspeed;
gyro_lp[1] = gyro_report.y; gyro_lp[1] = vehicle_attitude.pitchspeed;
gyro_lp[2] = gyro_report.z; gyro_lp[2] = vehicle_attitude.yawspeed;
multirotor_control_rates(&rates_sp, gyro_lp, &actuators); multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);