fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 11:13:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_main.c
+33 -13
View File
@@ -135,6 +135,11 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&att_sp, 0, sizeof(att_sp)); memset(&att_sp, 0, sizeof(att_sp));
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));
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; // static struct debug_key_value_s debug_output;
// memset(&debug_output, 0, sizeof(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 */ /* 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; actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); 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); // orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1'; // debug_output.key[0] = '1';
/* subscribe */ /* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); 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 */ /* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f}; 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 */ /* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500); poll(&fds, 1, 500);
/*Get Local Copies */
/* get a local copy of attitude */ /* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); 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(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[0] = att.rollspeed;
gyro[1] = att.pitchspeed; gyro[1] = att.pitchspeed;
@@ -175,20 +184,31 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* Control */ /* Control */
/* Attitude Control */ if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
fixedwing_att_control_attitude(&att_sp, /* Attitude Control */
&att, fixedwing_att_control_attitude(&att_sp,
&rates_sp); &att,
&rates_sp);
/* Attitude Rate Control */ /* Attitude Rate Control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators); fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX //REMOVEME XXX
actuators.control[3] = 0.7f; 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); 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"); printf("[fixedwing_att_control] exiting, stopping all motors.\n");