diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 14:50:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 14:50:34 +0200 |
commit | 607e902b886b39b5e9b58999dee97c2ea8938151 (patch) | |
tree | ef17512bea9d23d706f134d0a25fb52486f4b7e7 /apps/multirotor_pos_control/multirotor_pos_control.c | |
parent | 2a06b66845542b05e3cad3d21099e33adc213227 (diff) | |
download | px4-firmware-607e902b886b39b5e9b58999dee97c2ea8938151.tar.gz px4-firmware-607e902b886b39b5e9b58999dee97c2ea8938151.tar.bz2 px4-firmware-607e902b886b39b5e9b58999dee97c2ea8938151.zip |
Cleaned up / simplified position control, attacking pos control implementation next
Diffstat (limited to 'apps/multirotor_pos_control/multirotor_pos_control.c')
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 112 |
1 files changed, 94 insertions, 18 deletions
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 3f9c72517..52f90bb93 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -49,24 +49,92 @@ #include <time.h> #include <sys/prctl.h> #include <arch/board/up_hrt.h> -#include "ardrone_control.h" -#include "attitude_control.h" -#include "rate_control.h" -#include "ardrone_motor_control.h" -#include "position_control.h" #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude.h> #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 <systemlib/systemlib.h> + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ __EXPORT int multirotor_pos_control_main(int argc, char *argv[]); -static bool thread_should_exit; -static bool thread_running = false; -static int mpc_task; +/** + * Mainloop of position controller. + */ +static int multirotor_pos_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn(). + */ +int multirotor_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("multirotor pos control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("multirotor pos control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tmultirotor pos control app is running\n"); + } else { + printf("\tmultirotor pos control app not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} static int -mpc_thread_main(int argc, char *argv[]) +multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ printf("[multirotor pos control] Control started, taking over position control\n"); @@ -91,6 +159,8 @@ mpc_thread_main(int argc, char *argv[]) /* publish attitude setpoint */ orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + thread_running = true; + while (1) { /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &state); @@ -103,11 +173,19 @@ mpc_thread_main(int argc, char *argv[]) /* get a local copy of local position setpoint */ orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (state.state_machine == SYSTEM_STATE_AUTO) { - position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp); + // if (state.state_machine == SYSTEM_STATE_AUTO) { + + // XXX IMPLEMENT POSITION CONTROL HERE + + att_sp.roll_body = 0.1f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.4f; + att_sp.timestamp = hrt_absolute_time(); + /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } else if (state.state_machine == SYSTEM_STATE_STABILIZE) { + // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { /* set setpoint to current position */ // XXX select pos reset channel on remote /* reset setpoint to current position (position hold) */ @@ -117,19 +195,17 @@ mpc_thread_main(int argc, char *argv[]) // local_pos_sp.z = local_pos.z; // local_pos_sp.yaw = att.yaw; // } - } + // } /* run at approximately 50 Hz */ usleep(20000); - counter++; } - /* close uarts */ - close(ardrone_write); - ar_multiplexing_deinit(gpios); + printf("[multirotor pos control] ending now...\n"); + + thread_running = false; - printf("[multirotor pos control] ending now...\r\n"); fflush(stdout); return 0; } |