diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-18 18:28:49 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-18 18:28:49 +0200 |
commit | b0b72b11eb6c112d3fb58385f5681af55dd5605a (patch) | |
tree | b5c9548cc4619919fab7015df2a390de70578ac3 /apps/multirotor_att_control/multirotor_att_control_main.c | |
parent | 3816327977166cbb68ba94aae4b316651cd70ba3 (diff) | |
parent | df034330340aa1f938adbc1ed8840689ead41d89 (diff) | |
download | px4-firmware-b0b72b11eb6c112d3fb58385f5681af55dd5605a.tar.gz px4-firmware-b0b72b11eb6c112d3fb58385f5681af55dd5605a.tar.bz2 px4-firmware-b0b72b11eb6c112d3fb58385f5681af55dd5605a.zip |
Reworking control infrastructure for inner rate loop, preparing offboard interface
Diffstat (limited to 'apps/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 59 |
1 files changed, 41 insertions, 18 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index a9f27197d..530033701 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -32,7 +32,7 @@ * ****************************************************************************/ -/* +/** * @file multirotor_att_control_main.c * * Implementation of multirotor attitude control main loop. @@ -103,6 +103,8 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* * Do not rate-limit the loop to prevent aliasing @@ -143,28 +145,49 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of the current sensor values */ + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - att_sp.yaw_rate_body = manual.yaw; - att_sp.timestamp = hrt_absolute_time(); - - if (motor_test_mode) { - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.roll_rate_body = 0.0f; - att_sp.pitch_rate_body = 0.0f; - att_sp.yaw_rate_body = 0.0f; - att_sp.thrust = 0.1f; - } else { - att_sp.thrust = manual.throttle; + + /** STEP 1: Define which input is the dominating control input */ + + if (state.flag_control_manual_enabled) { + /* manual inputs, from RC control or joystick */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + att_sp.yaw_rate_body = manual.yaw; + att_sp.timestamp = hrt_absolute_time(); + if (motor_test_mode) { + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.roll_rate_body = 0.0f; + att_sp.pitch_rate_body = 0.0f; + att_sp.yaw_rate_body = 0.0f; + att_sp.thrust = 0.1f; + } else { + att_sp.thrust = manual.throttle; + } + } else if (state.flag_control_offboard_enabled) { + /* offboard inputs */ + + /* decide wether we want rate or position input */ } - multirotor_control_attitude(&att_sp, &att, &actuators); + /** STEP 2: Identify the controller setup to run and set up the inputs correctly */ + + /* run attitude controller */ + if (state.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &actuators); + } + + /* XXX could be also run in an independent loop */ + if (state.flag_control_rates_enabled) { + multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators); + } - /* publish the result */ + /* STEP 3: publish the result to the vehicle actuators */ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); |