diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 00:28:15 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-25 00:28:15 +0100 |
commit | dc72d467d4abe3d18bbf02091eb4eaddb4f491d2 (patch) | |
tree | a247e1288fe66e7307c71e9a918b118d56a9d52a /apps | |
parent | 9f35de51a60fcbe207974be43648aa523926f115 (diff) | |
download | px4-firmware-dc72d467d4abe3d18bbf02091eb4eaddb4f491d2.tar.gz px4-firmware-dc72d467d4abe3d18bbf02091eb4eaddb4f491d2.tar.bz2 px4-firmware-dc72d467d4abe3d18bbf02091eb4eaddb4f491d2.zip |
fixed wing manual setpoints in manual mode
Diffstat (limited to 'apps')
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 50 |
1 files changed, 35 insertions, 15 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index ad0f201e1..38f58ac33 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -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); - - /* Attitude Rate Control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - //REMOVEME XXX - actuators.control[3] = 0.7f; + 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); + + //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"); |