From 93c200d281e7488db95806840a6976b02d1afbe0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 1 Oct 2012 00:02:38 -0700 Subject: Add new 'task_spawn' interface for starting new tasks in the PX4 world --- apps/ardrone_interface/ardrone_interface.c | 9 ++++++- .../attitude_estimator_ekf_main.c | 9 ++++++- apps/commander/commander.c | 7 ++++- apps/examples/px4_deamon_app/px4_deamon_app.c | 7 ++++- apps/fixedwing_control/fixedwing_control.c | 8 +++++- apps/gps/gps.c | 9 ++++++- apps/mavlink/mavlink.c | 15 ++++++++--- .../multirotor_att_control_main.c | 8 +++++- apps/px4/fmu/fmu.cpp | 9 ++++++- apps/sdlog/sdlog.c | 9 ++++++- apps/sensors/sensors.cpp | 11 ++++---- apps/systemcmds/led/led.c | 14 +++++++--- apps/systemlib/systemlib.c | 31 +++++++++++++++++++--- apps/systemlib/systemlib.h | 8 ++++++ 14 files changed, 130 insertions(+), 24 deletions(-) (limited to 'apps') diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8ed6db664..74b32c4af 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -56,6 +56,8 @@ #include #include +#include + #include "ardrone_motor_control.h" __EXPORT int ardrone_interface_main(int argc, char *argv[]); @@ -112,7 +114,12 @@ int ardrone_interface_main(int argc, char *argv[]) } thread_should_exit = false; - ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + ardrone_interface_task = task_spawn("ardrone_interface", + SCHED_RR, + SCHED_PRIORITY_MAX - 15, + 4096, + ardrone_interface_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 867b484e1..e5907f961 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -60,6 +60,8 @@ #include #include +#include + #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" @@ -150,7 +152,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + SCHED_RR, + SCHED_PRIORITY_DEFAULT, + 20000, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 960c5d383..3e188eb63 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -949,7 +949,12 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("commander", SCHED_PRIORITY_MAX - 50, 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn("commander", + SCHED_RR, + SCHED_PRIORITY_MAX - 50, + 4096, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c index 053163789..6068df5a3 100644 --- a/apps/examples/px4_deamon_app/px4_deamon_app.c +++ b/apps/examples/px4_deamon_app/px4_deamon_app.c @@ -91,7 +91,12 @@ int px4_deamon_app_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, px4_deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn("deamon", + SCHED_RR, + SCHED_PRIORITY_DEFAULT, + 4096, + px4_deamon_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index beabc1142..81ec4ca0b 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -60,6 +60,7 @@ #include #include #include +#include #include static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -414,7 +415,12 @@ int fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn("fixedwing_control", + SCHED_RR, + SCHED_PRIORITY_MAX - 20, + 4096, + fixedwing_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 976beeaab..8304c72e1 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -57,6 +57,8 @@ #include #include +#include + static bool thread_should_exit; /**< Deamon status flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -140,7 +142,12 @@ int gps_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn("gps", + SCHED_RR, + SCHED_PRIORITY_DEFAULT, + 4096, + gps_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ea1c511f9..117387f36 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -75,7 +75,9 @@ #include #include #include + #include +#include #include "waypoints.h" #include "mavlink_log.h" @@ -203,7 +205,7 @@ int mavlink_missionlib_send_gcs_string(const char *string); uint64_t mavlink_missionlib_get_system_timestamp(void); void handleMessage(mavlink_message_t *msg); -static void mavlink_update_system(); +static void mavlink_update_system(void); /** * Enable / disable Hardware in the Loop simulation mode. @@ -1549,9 +1551,9 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } -void mavlink_update_system() +void mavlink_update_system(void) { - static initialized = false; + static bool initialized = false; param_t param_system_id; param_t param_component_id; param_t param_system_type; @@ -1866,7 +1868,12 @@ int mavlink_main(int argc, char *argv[]) } thread_should_exit = false; - mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + mavlink_task = task_spawn("mavlink", + SCHED_RR, + SCHED_PRIORITY_DEFAULT, + 6000, + mavlink_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ebd9911a3..9acdc6fd3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -67,6 +67,7 @@ #include #include +#include #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" @@ -350,7 +351,12 @@ int multirotor_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1+optioncount], "start")) { thread_should_exit = false; - mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); + mc_task = task_spawn("multirotor_att_control", + SCHED_RR, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 48d338c99..eeef2f914 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -66,6 +66,8 @@ #include #include +#include + class FMUServo : public device::CDev { public: @@ -169,7 +171,12 @@ FMUServo::init() return ret; /* start the IO interface task */ - _task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr); + _task = task_spawn("fmuservo", + SCHED_RR, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&FMUServo::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f1c1f9a23..f4af17597 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -61,6 +61,8 @@ #include #include +#include + 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 */ @@ -120,7 +122,12 @@ int sdlog_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn("sdlog", + SCHED_RR, + SCHED_PRIORITY_DEFAULT - 30, + 4096, + sdlog_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index f81dfa9b8..8c943cca2 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1175,11 +1175,12 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_create("sensors_task", - SCHED_PRIORITY_MAX - 5, - 6000, /* XXX may be excesssive */ - (main_t)&Sensors::task_main_trampoline, - nullptr); + _sensors_task = task_spawn("sensors_task", + SCHED_RR, + SCHED_PRIORITY_MAX - 5, + 6000, /* XXX may be excesssive */ + (main_t)&Sensors::task_main_trampoline, + nullptr); if (_sensors_task < 0) { warn("task start failed"); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index a6d73210a..da0d99964 100644 --- a/apps/systemcmds/led/led.c +++ b/apps/systemcmds/led/led.c @@ -53,6 +53,9 @@ #include #include +#include +#include + __EXPORT int led_main(int argc, char *argv[]); @@ -61,7 +64,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int led_task; /**< Handle of deamon task / thread */ static int leds; -static int led_init() +static int led_init(void) { leds = open("/dev/led", O_RDONLY | O_NONBLOCK); @@ -76,7 +79,7 @@ static int led_init() return 0; } -static void led_deinit() +static void led_deinit(void) { close(leds); } @@ -144,7 +147,12 @@ int led_main(int argc, char *argv[]) } thread_should_exit = false; - led_task = task_create("led", SCHED_PRIORITY_MAX - 15, 4096, led_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + led_task = task_spawn("led", + SCHED_RR, + SCHED_PRIORITY_MAX - 15, + 4096, + led_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; exit(0); } diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 4c7aae83e..3d46a17a8 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -68,10 +68,10 @@ const struct __multiport_info multiport_info = { ****************************************************************************/ /**************************************************************************** - * Public Functions + * Private Functions ****************************************************************************/ -void kill_task(FAR _TCB *tcb, FAR void *arg); +static void kill_task(FAR _TCB *tcb, FAR void *arg); /**************************************************************************** * user_start @@ -116,11 +116,36 @@ void killall() sched_foreach(kill_task, NULL); } -void kill_task(FAR _TCB *tcb, FAR void *arg) +static void kill_task(FAR _TCB *tcb, FAR void *arg) { kill(tcb->pid, SIGUSR1); } +int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +{ + int pid; + + sched_lock(); + + /* create the task */ + pid = task_create(name, priority, stack_size, entry, argv); + + if (pid > 0) { + + /* configure the scheduler */ + struct sched_param param; + + param.sched_priority = priority; + sched_setscheduler(pid, scheduler, ¶m); + + /* XXX do any other private task accounting here */ + } + + sched_unlock(); + + return pid; +} + #define PX4_BOARD_ID_FMU (5) int fmu_get_board_info(struct fmu_board_info_s *info) diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h index add47f440..da1524435 100644 --- a/apps/systemlib/systemlib.h +++ b/apps/systemlib/systemlib.h @@ -50,6 +50,14 @@ __EXPORT int reboot(void); /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); +/** Starts a task and performs any specific accounting, scheduler setup, etc. */ +__EXPORT int task_spawn(const char *name, + int priority, + int scheduler, + int stack_size, + main_t entry, + const char *argv[]); + enum MULT_PORTS { MULT_0_US2_RXTX = 0, MULT_1_US2_FLOW, -- cgit v1.2.3