fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 00:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up / simplified position control, attacking pos control implementation next

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-04 14:50:34 +02:00
parent 2a06b66845
commit 607e902b88
5 changed files with 288 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_pos_control/multirotor_pos_control.c
+94 -18
View File
@@ -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;
}