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

fixed wing manual setpoints in manual mode

Browse Source
This commit is contained in:
Lorenz Meier 2012-11-25 00:28:15 +01:00
parent 9f35de51a6
commit dc72d467d4
1 changed files with 33 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -135,6 +135,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
// static struct debug_key_value_s debug_output;
// memset(&debug_output, 0, sizeof(debug_output));
@ -144,16 +149,19 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1';
/* subscribe */
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
@ -164,10 +172,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/*Get Local Copies */
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
@ -175,20 +184,31 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* Control */
/* Attitude Control */
fixedwing_att_control_attitude(&att_sp,
&att,
&rates_sp);
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
/* Attitude Control */
fixedwing_att_control_attitude(&att_sp,
&att,
&rates_sp);
/* Attitude Rate Control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* Attitude Rate Control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX
actuators.control[3] = 0.7f;
//REMOVEME XXX
actuators.control[3] = 0.7f;
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = -manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
}
/* publish output */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");