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

Merge branch 'master' of https://github.com/PX4/Firmware

Browse Source 
Conflicts:
	apps/multirotor_att_control/multirotor_att_control_main.c
This commit is contained in:
tnaegeli
2012-10-03 15:15:05 +02:00
parent f3cb2cf8a3 beca2b072e
commit 147c5bb664
9 changed files with 523 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+19 -18
View File
@@ -91,12 +91,12 @@ static void *rate_control_thread_main(void *arg)
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));
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;
memset(&rates_sp, 0, sizeof(rates_sp));
float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
@@ -104,16 +104,17 @@ static void *rate_control_thread_main(void *arg)
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 data */
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
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) {
@@ -126,14 +127,14 @@ static void *rate_control_thread_main(void *arg)
if (state.flag_control_rates_enabled) {
/* lowpass gyros */
// XXX
gyro_lp[0] = gyro_report.x;
gyro_lp[1] = gyro_report.y;
gyro_lp[2] = gyro_report.z;
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);
}
// }
}
usleep(2000);
}
@@ -355,7 +356,7 @@ int multirotor_att_control_main(int argc, char *argv[])
mc_task = task_spawn("multirotor_att_control",
SCHED_RR,
SCHED_PRIORITY_MAX - 15,
2048,
4096,
mc_thread_main,
NULL);
exit(0);