diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 15:33:39 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 15:33:39 +0200 |
commit | 67a2c8a173819018ba2155688aa2a7bb682d8a77 (patch) | |
tree | b3cf26e1ac002c3eb144ee50254b874bc5f0b506 /apps/multirotor_pos_control/multirotor_pos_control.c | |
parent | 607e902b886b39b5e9b58999dee97c2ea8938151 (diff) | |
download | px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.tar.gz px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.tar.bz2 px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.zip |
Added controller parameters, added vicon position reading
Diffstat (limited to 'apps/multirotor_pos_control/multirotor_pos_control.c')
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 34 |
1 files changed, 28 insertions, 6 deletions
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 52f90bb93..7cf687306 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -55,9 +55,11 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_vicon_position.h> #include <systemlib/systemlib.h> +#include "multirotor_pos_control_params.h" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -144,7 +146,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) struct vehicle_attitude_s att; //struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_local_position_s local_pos; + struct vehicle_vicon_position_s local_pos; struct manual_control_setpoint_s manual; struct vehicle_attitude_setpoint_s att_sp; @@ -152,7 +154,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); @@ -161,6 +163,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) thread_running = true; + int loopcounter = 0; + + struct multirotor_position_control_params p; + struct multirotor_position_control_param_handles h; + parameters_init(&h); + parameters_update(&h, &p); + + while (1) { /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &state); @@ -169,16 +179,27 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); /* get a local copy of local position setpoint */ orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + if (loopcounter == 500) { + parameters_update(&h, &p); + loopcounter = 0; + } + // if (state.state_machine == SYSTEM_STATE_AUTO) { // XXX IMPLEMENT POSITION CONTROL HERE - att_sp.roll_body = 0.1f; - att_sp.pitch_body = 0.0f; + float dT = 1.0f / 50.0f; + + float x_setpoint = 0.0f; + + /* local pos is the Vicon position */ + + att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT; + att_sp.roll_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.4f; att_sp.timestamp = hrt_absolute_time(); @@ -199,6 +220,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) /* run at approximately 50 Hz */ usleep(20000); + loopcounter++; } |