From c5237f7f6f3600e762a867669e9645cca09d42a7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 14:43:11 -0700 Subject: [PATCH] Removed extra abstracton layer in systemlib The calls to task_spawn_cmd, kill_all, and systemreset were wrappers around the px4_{task_spawn_cmd|kill_all|systemreset} implementations. Removed the wrappers and changed all calls to the px4_ equivalents. NuttX specific code was moved into px4_tasks.h Signed-off-by: Mark Charlebois --- .../ardrone_interface/ardrone_interface.c | 2 +- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/gps/gps.cpp | 2 +- .../hott/hott_sensors/hott_sensors.cpp | 2 +- .../hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/md25/md25_main.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/examples/fixedwing_control/main.c | 4 ++-- .../flow_position_estimator_main.c | 4 ++-- .../matlab_csv_serial/matlab_csv_serial.c | 4 ++-- .../publisher/publisher_start_nuttx.cpp | 2 +- src/examples/rover_steering_control/main.cpp | 4 ++-- .../subscriber/subscriber_start_nuttx.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 4 ++-- .../attitude_estimator_so3_main.cpp | 4 ++-- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/commander/commander.cpp | 6 ++--- .../ekf_att_pos_estimator_main.cpp | 2 +- .../fixedwing_backside_main.cpp | 2 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 2 +- .../land_detector/land_detector_main.cpp | 2 +- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../mc_att_control_start_nuttx.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- .../mc_pos_control_start_nuttx.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- .../position_estimator_inav_main.c | 2 +- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/module.mk | 1 - src/modules/systemlib/systemlib.h | 23 ++----------------- src/modules/uavcan/uavcan_main.cpp | 2 +- .../vtol_att_control_main.cpp | 2 +- src/platforms/px4_tasks.h | 10 ++++++++ 38 files changed, 55 insertions(+), 65 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index cfc80ace22..1777e7f27c 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) } thread_should_exit = false; - ardrone_interface_task = task_spawn_cmd("ardrone_interface", + ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 1100, diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 5035600ef2..75d1124a61 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -216,7 +216,7 @@ int frsky_telemetry_main(int argc, char *argv[]) errx(0, "frsky_telemetry already running"); thread_should_exit = false; - frsky_task = task_spawn_cmd("frsky_telemetry", + frsky_task = px4_task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index da92c851bc..47a6e4c965 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,7 +229,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_spawn_cmd("gps", SCHED_DEFAULT, + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 7bc3ca8353..29680a279f 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -209,7 +209,7 @@ hott_sensors_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index df6fe950b9..a1a5b080c5 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -235,7 +235,7 @@ hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index afbd10c5cf..a20bf7c7c6 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -110,7 +110,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("md25", + deamon_task = px4_task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5fc1dd6b21..d1484a2f01 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -302,7 +302,7 @@ MK::init(unsigned motors) } /* start the IO interface task */ - _task = task_spawn_cmd("mkblctrl", + _task = px4_task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 1500, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f0412584fa..e876ac75ce 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -341,7 +341,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = task_spawn_cmd("fmuservo", + _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1600, diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 92e6dde7ff..e0e866555b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -854,7 +854,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_spawn_cmd("px4io", + _task = px4_task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1800, diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index e8462ef5f8..aa5e6dcc0b 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -105,7 +105,7 @@ int roboclaw_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("roboclaw", + deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 168c2bfa92..97f9b412a7 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -414,7 +414,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int ex_fixedwing_control_main(int argc, char *argv[]) { @@ -431,7 +431,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("ex_fixedwing_control", + deamon_task = px4_px4_task_spawn_cmd("ex_fixedwing_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index db21a9cb31..a1a81d6361 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -95,7 +95,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int flow_position_estimator_main(int argc, char *argv[]) { @@ -111,7 +111,7 @@ int flow_position_estimator_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("flow_position_estimator", + daemon_task = px4_px4_task_spawn_cmd("flow_position_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 98d2849580..b840283b6d 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -87,7 +87,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int matlab_csv_serial_main(int argc, char *argv[]) { @@ -103,7 +103,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("matlab_csv_serial", + daemon_task = px4_px4_task_spawn_cmd("matlab_csv_serial", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index aaf28fdb7f..2379d7aa68 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -68,7 +68,7 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("publisher", + daemon_task = px4_task_spawn_cmd("publisher", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index a64c169863..e607acc2c2 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -408,7 +408,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int rover_steering_control_main(int argc, char *argv[]) { @@ -425,7 +425,7 @@ int rover_steering_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("rover_steering_control", + deamon_task = px4_px4_task_spawn_cmd("rover_steering_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 91543d5d23..424e49e9c1 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -68,7 +68,7 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("subscriber", + daemon_task = px4_task_spawn_cmd("subscriber", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6c90f74a8f..4a086be5dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -113,7 +113,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { @@ -131,7 +131,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", + attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 7700, diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 11c302a8c8..76dc209c12 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -132,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", + attitude_estimator_so3_task = px4_px4_task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 00e618609c..987b930d28 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -225,7 +225,7 @@ BottleDrop::start() ASSERT(_main_task == -1); /* start the task */ - _main_task = task_spawn_cmd("bottle_drop", + _main_task = px4_task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, 1500, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a349cfca95..dcf7cdddf0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -269,7 +269,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", + daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3400, @@ -2652,13 +2652,13 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot */ - systemreset(false); + px4_systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot to bootloader */ - systemreset(true); + px4_systemreset(true); } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 15a663bc1e..1b6e17ac37 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1038,7 +1038,7 @@ int AttitudePositionEstimatorEKF::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 447e229b1b..06559de979 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -111,7 +111,7 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_backside", + deamon_task = px4_task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 70ac4b74ea..fb6f74f357 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1117,7 +1117,7 @@ FixedwingAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_att_control", + _control_task = px4_task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 20c2bd46d2..3525c5674b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1621,7 +1621,7 @@ FixedwingPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_pos_control_l1", + _control_task = px4_task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index b4b7c33a20..09b5fd799b 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -136,7 +136,7 @@ static int land_detector_start(const char *mode) } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("land_detector", + _landDetectorTaskID = px4_task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b432ecad6c..c40ebdb4ed 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -906,7 +906,7 @@ MulticopterAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", + _control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index dec1e1745d..93efd90e7f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -68,7 +68,7 @@ int mc_att_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_att_control_m", + daemon_task = px4_task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6ffb37d970..fe6db135f8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1423,7 +1423,7 @@ MulticopterPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_pos_control", + _control_task = px4_task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 0db67d83ff..92416249d8 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -68,7 +68,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_pos_control_m", + daemon_task = px4_task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc6d3ea9f1..e9733b40ce 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -515,7 +515,7 @@ Navigator::start() ASSERT(_navigator_task == -1); /* start the task */ - _navigator_task = task_spawn_cmd("navigator", + _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, 1700, diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 84ae34df2b..5660bcc194 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -153,7 +153,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } thread_should_exit = false; - position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", + position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (char * const *) &argv[2] : (char * const *) NULL); diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index d633f2a420..67acd94bf7 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -106,7 +106,7 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("segway", + deamon_task = px4_task_spawn_cmd("segway", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b2867eeffa..c1e5720aec 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2252,7 +2252,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn_cmd("sensors_task", + _sensors_task = px4_task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 4d805c57c0..e648c577d9 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -42,7 +42,6 @@ SRCS = err.c \ cpuload.c \ getopt_long.c \ pid/pid.c \ - systemlib.c \ airspeed.c \ system_params.c \ mavlink_log.c \ diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2196133f8d..29b6eb5cef 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,31 +42,12 @@ #include #include #include + +// OS specific settings defined in px4_tasks.h #include __BEGIN_DECLS -/** Reboots the board */ -__EXPORT void systemreset(bool to_bootloader) noreturn_function; - -/** Sends SIGUSR1 to all processes */ -__EXPORT void killall(void); - -/** Default scheduler type */ -#if CONFIG_RR_INTERVAL > 0 -# define SCHED_DEFAULT SCHED_RR -#else -# define SCHED_DEFAULT SCHED_FIFO -#endif - -/** Starts a task and performs any specific accounting, scheduler setup, etc. */ -__EXPORT int task_spawn_cmd(const char *name, - int priority, - int scheduler, - int stack_size, - px4_main_t entry, - char * const argv[]); - enum MULT_PORTS { MULT_0_US2_RXTX = 0, MULT_1_US2_FLOW, diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bc89b956b3..75e8eeff95 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -194,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, + _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5fd7985d04..51304a046c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -861,7 +861,7 @@ VtolAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", + _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 817fbbbb61..d03f17b85d 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -43,16 +43,26 @@ #include #ifdef __PX4_ROS + #error "PX4 tasks not supported in ROS" + #elif defined(__PX4_NUTTX) typedef int px4_task_t; +/** Default scheduler type */ +#if CONFIG_RR_INTERVAL > 0 +# define SCHED_DEFAULT SCHED_RR +#else +# define SCHED_DEFAULT SCHED_FIFO +#endif + #define px4_task_exit(x) _exit(x) #elif defined(__PX4_POSIX) || defined(__PX4_QURT) #include #include +/** Default scheduler type */ #define SCHED_DEFAULT SCHED_FIFO #ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO)