/****************************************************************************
* Public Functions
****************************************************************************/
void work_queues_init(void)
{
sem_init(&_work_lock[HPWORK], 0, 1);
sem_init(&_work_lock[LPWORK], 0, 1);
#ifdef CONFIG_SCHED_USRWORK
sem_init(&_work_lock[USRWORK], 0, 1);
#endif
// Create high priority worker thread
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 1,
2000,
work_hpthread,
(char *const *)NULL);
// Create low priority worker thread
g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
SCHED_DEFAULT,
SCHED_PRIORITY_MIN,
2000,
work_lpthread,
(char *const *)NULL);
}
int test_dataman(int argc, char *argv[])
{
int i, num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1) {
num_tasks = atoi(argv[1]);
}
sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
warnx("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
sprintf(a, "%d", i);
const char *av[2];
av[0] = a;
av[1] = 0;
px4_sem_init(sems + i, 1, 0);
/* sems use case is a signal */
px4_sem_setprotocol(&sems, SEM_PRIO_NONE);
/* start the task */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, (void *)av)) <= 0) {
warn("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}
}
return 0;
}
int Ekf2::start()
{
ASSERT(_control_task == -1);
#ifdef __PX4_QURT
// On the DSP we seem to get random crashes with a stack size below 13000.
const unsigned stack_size = 15000;
#else
const unsigned stack_size = 9000;
#endif
/* start the task */
_control_task = px4_task_spawn_cmd("ekf2",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
stack_size,
(px4_main_t)&Ekf2::task_main_trampoline,
nullptr);
if (_control_task < 0) {
PX4_WARN("task start failed");
return -errno;
}
return OK;
}
int uORBTest::UnitTest::test_multi2()
{
test_note("Testing multi-topic 2 test (queue simulation)");
//test: first subscribe, then advertise
_thread_should_exit = false;
const int num_instances = 3;
int orb_data_fd[num_instances];
int orb_data_next = 0;
for (int i = 0; i < num_instances; ++i) {
// PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
}
char *const args[1] = { NULL };
int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
args);
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
hrt_abstime last_time = 0;
while (!_thread_should_exit) {
bool updated = false;
int orb_data_cur_fd = orb_data_fd[orb_data_next];
orb_check(orb_data_cur_fd, &updated);
if (updated) {
struct orb_test_medium msg;
orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
usleep(1000);
if (last_time >= msg.time && last_time != 0) {
return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
}
last_time = msg.time;
// PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
orb_data_next = (orb_data_next + 1) % num_instances;
}
}
for (int i = 0; i < num_instances; ++i) {
orb_unsubscribe(orb_data_fd[i]);
}
return test_note("PASS multi-topic 2 test (queue simulation)");
}
int micrortps_client_main(int argc, char *argv[])
{
if (argc < 2) {
usage(argv[0]);
return -1;
}
if (!strcmp(argv[1], "start")) {
if (_rtps_task != -1) {
PX4_INFO("Already running");
return -1;
}
_rtps_task = px4_task_spawn_cmd("rtps",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
(px4_main_t) micrortps_start,
(char *const *)argv);
if (_rtps_task < 0) {
PX4_WARN("Could not start task");
_rtps_task = -1;
return -1;
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (_rtps_task == -1) {
PX4_INFO("Not running");
} else {
PX4_INFO("Running");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (_rtps_task == -1) {
PX4_INFO("Not running");
return -1;
}
_should_exit_task = true;
if (nullptr != transport_node) { transport_node->close(); }
return 0;
}
usage(argv[0]);
return -1;
}
/**
* The position_estimator_inav_thread 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_create().
*/
int position_estimator_inav_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
return 0;
}
inav_verbose_mode = false;
if ((argc > 2) && (!strcmp(argv[2], "-v"))) {
inav_verbose_mode = true;
}
thread_should_exit = false;
position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
position_estimator_inav_thread_main,
(argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (thread_running) {
warnx("stop");
thread_should_exit = true;
} else {
warnx("not started");
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
return 0;
}
usage("unrecognized command");
return 1;
}
int start()
{
if (_is_running) {
PX4_WARN("bebop_flow already running");
return -1;
}
// Prepare the I2C device
image_sensor = new MT9V117(IMAGE_DEVICE_PATH);
if (image_sensor == nullptr) {
PX4_ERR("failed instantiating image sensor object");
return -1;
}
int ret = image_sensor->start();
if (ret != 0) {
PX4_ERR("Image sensor start failed");
return ret;
}
// Start the video device
g_dev = new VideoDevice(dev_name, 6);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating video device object");
return -1;
}
ret = g_dev->start();
if (ret != 0) {
PX4_ERR("Video device start failed");
return ret;
}
/* start the task */
_task_handle = px4_task_spawn_cmd("bebop_flow",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
(px4_main_t)&task_main,
nullptr);
if (_task_handle < 0) {
PX4_WARN("task start failed");
return -1;
}
return 0;
}
/**
* Main entry point for this module
**/
int landing_target_estimator_main(int argc, char *argv[])
{
if (argc < 2) {
goto exiterr;
}
if (argc >= 2 && !strcmp(argv[1], "start")) {
if (thread_running) {
PX4_INFO("already running");
/* this is not an error */
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("landing_target_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
landing_target_estimator_thread_main,
(argv) ? (char *const *)&argv[2] : nullptr);
return 0;
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
if (!thread_running) {
PX4_WARN("landing_target_estimator not running");
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
PX4_INFO("running");
} else {
PX4_INFO("not started");
}
return 0;
}
exiterr:
PX4_WARN("usage: landing_target_estimator {start|stop|status}");
return 1;
}
int
Syslink::start()
{
_task_running = true;
_syslink_task = px4_task_spawn_cmd(
"syslink",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1500,
Syslink::task_main_trampoline,
NULL
);
return 0;
}
int IridiumSBD::start(int argc, char *argv[])
{
PX4_INFO("starting");
if (IridiumSBD::instance != nullptr) {
PX4_WARN("already started");
return PX4_ERROR;
}
IridiumSBD::instance = new IridiumSBD();
IridiumSBD::task_handle = px4_task_spawn_cmd("iridiumsbd", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 1300, (main_t)&IridiumSBD::main_loop_helper, argv);
return OK;
}
/**
* The attitude_estimator_ekf 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 px4_task_spawn_cmd().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running\n");
/* this is not an error */
return 0;
}
thread_should_exit = false;
attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
7700,
attitude_estimator_ekf_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
return 0;
} else {
warnx("not started");
return 1;
}
return 0;
}
usage("unrecognized command");
return 1;
}
int hello_main(int argc, char *argv[])
{
if (argc < 2) {
PX4_WARN("usage: hello {start|stop|status}\n");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (HelloExample::appState.isRunning()) {
PX4_INFO("already running\n");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("hello",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
PX4_MAIN,
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
return 0;
}
if (!strcmp(argv[1], "stop")) {
HelloExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (HelloExample::appState.isRunning()) {
PX4_INFO("is running\n");
} else {
PX4_INFO("not started\n");
}
return 0;
}
PX4_WARN("usage: hello_main {start|stop|status}\n");
return 1;
}
int wqueue_test_main(int argc, char *argv[])
{
if (argc < 2) {
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (WQueueTest::appState.isRunning()) {
PX4_INFO("already running\n");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("wqueue",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
PX4_MAIN,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
WQueueTest::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (WQueueTest::appState.isRunning()) {
PX4_INFO("is running\n");
} else {
PX4_INFO("not started\n");
}
return 0;
}
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
return 1;
}
int GPS::init()
{
char gps_num[2] = {(char)('0' + _gps_num), 0};
char *const args[2] = { gps_num, NULL };
/* start the GPS driver worker task */
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 1400, (px4_main_t)&GPS::task_main_trampoline, args);
if (_task < 0) {
PX4_WARN("task start failed: %d", errno);
_task = -1;
return -errno;
}
return OK;
}
/**
* The daemon 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 px4_task_spawn_cmd().
*/
int rover_steering_control_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("rover_steering_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
rover_steering_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
} else {
warnx("not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int muorb_test_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("muorb_test",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char* const*)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
MuorbTestExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}
/**
* The daemon 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 px4_task_spawn_cmd().
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ex_fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tex_fixedwing_control is running\n");
} else {
printf("\tex_fixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/**
* The daemon 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_create().
*/
int px4_daemon_app_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("daemon already running\n");
/* this is not an error */
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
px4_daemon_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\trunning\n");
} else {
warnx("\tnot started\n");
}
return 0;
}
usage("unrecognized command");
return 1;
}
int Ekf2Replay::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("ekf2_replay",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
3000,
(px4_main_t)&Ekf2Replay::task_main_trampoline,
nullptr);
if (_control_task < 0) {
PX4_WARN("task start failed");
return -errno;
}
return OK;
}
int AttitudePositionEstimatorEKF::start()
{
ASSERT(_estimator_task == -1);
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
7500,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
if (_estimator_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("mpu9x50_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("mpu9x50_main task start failed");
return;
}
_is_running = true;
}
static int
start(void)
{
int task;
px4_sem_init(&g_init_sema, 1, 0);
/* start the worker thread with low priority for disk IO */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, 1200, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
/* wait for the thread to actually initialize */
px4_sem_wait(&g_init_sema);
px4_sem_destroy(&g_init_sema);
return 0;
}
/**
* 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_create().
*/
int ardrone_interface_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
1100,
ardrone_interface_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
} else {
warnx("not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/**
* Process command line arguments and start the daemon.
*/
int
hott_telemetry_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "missing command\n%s", commandline_usage);
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
exit(0);
}
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
hott_telemetry_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
errx(1, "unrecognized command\n%s", commandline_usage);
}
/**
* The daemon 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 px4_task_spawn_cmd().
*/
int matlab_csv_serial_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
} else {
warnx("stopped");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int
Navigator::start()
{
ASSERT(_navigator_task == -1);
/* start the task */
_navigator_task = px4_task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 5,
1300,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
if (_navigator_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int
CameraFeedback::start()
{
/* start the task */
_main_task = px4_task_spawn_cmd("camera_feedback",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
1600,
(px4_main_t)&CameraFeedback::task_main_trampoline,
nullptr);
if (_main_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int
MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int
FixedwingAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("fw_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1400,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return PX4_OK;
}
int
VtolAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
