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

ardrone in the air again (workaround: rate controller disabled)

Browse Source
This commit is contained in:
Julian Oes
2012-09-25 21:35:02 +02:00
parent 0eae48d480
commit abbe998506
4 changed files with 35 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+13 -3
View File
@@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
static struct actuator_controls_s actuators;
static orb_advert_t actuator_pub;
/**
@@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg)
{
prctl(PR_SET_NAME, "mc rate control", getpid());
struct actuator_controls_s actuators;
int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
@@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg)
gyro_lp[1] = gyro_report.y;
gyro_lp[2] = gyro_report.z;
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// }
}
}
@@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct actuator_controls_s actuators;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[])
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
//printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
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);
@@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[])
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
// printf("publish actuator\n");
// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]);
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);
// printf("publish attitude\n");
}
apps/multirotor_att_control/multirotor_attitude_control.c
+7
View File
@@ -312,6 +312,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
actuators->control[3] = motor_thrust;
}
// if(motor_skip_counter%20 == 0)
// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]);
// XXX change yaw rate to yaw pos controller
if (rates_sp) {
rates_sp->roll = roll_control;
@@ -320,5 +324,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
rates_sp->thrust = motor_thrust;
}
// if(motor_skip_counter%20 == 0)
// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust);
motor_skip_counter++;
}