diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 0228115f99..e304eec5c2 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -82,5 +82,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 17be50b5fc..c632eb5601 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -85,5 +85,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 00ac572798..b0b52a5158 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -81,5 +81,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 3a7e15afc9..ff8dda3a5c 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -69,5 +69,4 @@ CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index a62f5fb37a..797ea34c08 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -75,5 +75,4 @@ CONFIG_EXAMPLES_FAKE_GPS=y CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/src/examples/rover_steering_control/CMakeLists.txt b/src/examples/rover_steering_control/CMakeLists.txt deleted file mode 100644 index 74213b7729..0000000000 --- a/src/examples/rover_steering_control/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE examples__rover_steering_control - MAIN rover_steering_control - STACK_MAX 1300 - SRCS - main.cpp - DEPENDS - ) diff --git a/src/examples/rover_steering_control/Kconfig b/src/examples/rover_steering_control/Kconfig deleted file mode 100644 index 73e34d98f8..0000000000 --- a/src/examples/rover_steering_control/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig EXAMPLES_ROVER_STEERING_CONTROL - bool "rover_steering_control" - default n - ---help--- - Enable support for rover_steering_control \ No newline at end of file diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp deleted file mode 100644 index ae00604e60..0000000000 --- a/src/examples/rover_steering_control/main.cpp +++ /dev/null @@ -1,454 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file main.cpp - * - * Example implementation of a rover steering controller. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* process-specific header files */ -#include "params.h" - -using namespace time_literals; - -/* Prototypes */ - -/** - * Daemon management function. - * - * This function allows to start / stop the background task (daemon). - * The purpose of it is to be able to start the controller on the - * command line, query its status and stop it, without giving up - * the command line to one particular process or the need for bg/fg - * ^Z support by the shell. - */ -extern "C" __EXPORT int rover_steering_control_main(int argc, char *argv[]); - -struct params { - float yaw_p; -}; - -struct param_handles { - param_t yaw_p; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct param_handles *h); - -/** - * Update all parameters - * - */ -int parameter_update(const struct param_handles *h, struct params *p); - -/** - * Mainloop of daemon. - */ -int rover_steering_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/** - * Control roll and pitch angle. - * - * This very simple roll and pitch controller takes the current roll angle - * of the system and compares it to a reference. Pitch is controlled to zero and yaw remains - * uncontrolled (tutorial code, not intended for flight). - * - * @param att_sp The current attitude setpoint - the values the system would like to reach. - * @param att The current attitude. The controller should make the attitude match the setpoint - */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - struct actuator_controls_s *actuators); - -/* Variables */ -static bool thread_should_exit = false; /**< Daemon exit flag */ -static bool thread_running = false; /**< Daemon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static struct params pp; -static struct param_handles ph; - -int parameters_init(struct param_handles *h) -{ - /* PID parameters */ - h->yaw_p = param_find("RV_YAW_P"); - - return OK; -} - -int parameter_update(const struct param_handles *h, struct params *p) -{ - param_get(h->yaw_p, &(p->yaw_p)); - - return OK; -} - -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - struct actuator_controls_s *actuators) -{ - /* - * The PX4 architecture provides a mixer outside of the controller. - * The mixer is fed with a default vector of actuator controls, representing - * moments applied to the vehicle frame. This vector - * is structured as: - * - * Control Group 0 (attitude): - * - * 0 - roll (-1..+1) - * 1 - pitch (-1..+1) - * 2 - yaw (-1..+1) - * 3 - thrust ( 0..+1) - * 4 - flaps (-1..+1) - * ... - * - * Control Group 1 (payloads / special): - * - * ... - */ - - /* set r/p zero */ - actuators->control[0] = 0.0f; - actuators->control[1] = 0.0f; - - /* - * Calculate yaw error and apply P gain - */ - float yaw_err = matrix::Eulerf(matrix::Quatf(att->q)).psi() - matrix::Eulerf(matrix::Quatf(att_sp->q_d)).psi(); - actuators->control[2] = yaw_err * pp.yaw_p; - - /* copy throttle */ - actuators->control[3] = att_sp->thrust_body[0]; - - actuators->timestamp = hrt_absolute_time(); -} - -/* Main Thread */ -int rover_steering_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* initialize parameters, first the handles, then the values */ - parameters_init(&ph); - parameter_update(&ph, &pp); - - - /* - * PX4 uses a publish/subscribe design pattern to enable - * multi-threaded communication. - * - * The most elegant aspect of this is that controllers and - * other processes can either 'react' to new data, or run - * at their own pace. - * - * PX4 developer guide: - * https://pixhawk.ethz.ch/px4/dev/shared_object_communication - * - * Wikipedia description: - * http://en.wikipedia.org/wiki/Publish–subscribe_pattern - * - */ - - - - - /* - * Declare and safely initialize all structs to zero. - * - * These structs contain the system state and things - * like attitude, position, the current waypoint, etc. - */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_control_setpoint; - memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - struct position_setpoint_s global_sp; - memset(&global_sp, 0, sizeof(global_sp)); - - /* output structs - this is what is sent to the mixer */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - - /* publish actuator controls with zero values */ - for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { - actuators.control[i] = 0.0f; - } - - struct vehicle_attitude_setpoint_s _att_sp = {}; - - /* - * Advertise that this controller will publish actuator - * control values and the rate setpoint - */ - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - - /* subscribe to topics. */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - - uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - /* Setup of loop */ - - struct pollfd fds[1] {}; - - fds[0].fd = att_sub; - - fds[0].events = POLLIN; - - while (!thread_should_exit) { - - /* - * Wait for a sensor or param update, check for exit condition every 500 ms. - * This means that the execution will block here without consuming any resources, - * but will continue to execute the very moment a new attitude measurement or - * a param update is published. So no latency in contrast to the polling - * design pattern (do not confuse the poll() system call with polling). - * - * This design pattern makes the controller also agnostic of the attitude - * update speed - it runs as fast as the attitude updates with minimal latency. - */ - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* - * Poll error, this will not really happen in practice, - * but its good design practice to make output an error message. - */ - warnx("poll error"); - - } else if (ret == 0) { - /* no return value = nothing changed for 500 ms, ignore */ - } else { - - // check for parameter updates - if (parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - parameter_update_sub.copy(&pupdate); - - // if a param update occured, re-read our parameters - parameter_update(&ph, &pp); - } - - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { - - - /* Check if there is a new position measurement or position setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool att_sp_updated; - orb_check(att_sp_sub, &att_sp_updated); - bool manual_control_setpoint_updated; - orb_check(manual_control_setpoint_sub, &manual_control_setpoint_updated); - - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &_att_sp); - } - - /* control attitude / heading */ - control_attitude(&_att_sp, &att, &actuators); - - if (manual_control_setpoint_updated) - /* get the RC (or otherwise user based) input */ - { - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint); - } - - // XXX copy from manual depending on flight / usage mode to override - - /* get the system status and the flight mode we're in */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - - /* sanity check and publish actuator outputs */ - if (PX4_ISFINITE(actuators.control[0]) && - PX4_ISFINITE(actuators.control[1]) && - PX4_ISFINITE(actuators.control[2]) && - PX4_ISFINITE(actuators.control[3])) { - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - if (verbose) { - warnx("published"); - } - } - } - } - } - - warnx("exiting, stopping all motors."); - thread_running = false; - - /* kill all outputs */ - for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { - actuators.control[i] = 0.0f; - } - - actuators.timestamp = hrt_absolute_time(); - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - fflush(stdout); - - return 0; -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: rover_steering_control {start|stop|status}\n\n"); -} - -/** - * 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"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("running"); - /* this is not an error */ - return 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 *)nullptr); - thread_running = true; - return 0; - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("running"); - - } else { - warnx("not started"); - } - - return 0; - } - - usage("unrecognized command"); - return 1; -} - - - diff --git a/src/examples/rover_steering_control/params.c b/src/examples/rover_steering_control/params.c deleted file mode 100644 index 7eff0b0a28..0000000000 --- a/src/examples/rover_steering_control/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file params.c - * - * Parameters for rover demo - * - * @author Lorenz Meier - */ - -#include "params.h" - -/* controller parameters, use max. 15 characters for param name! */ - -/** - * - */ -PARAM_DEFINE_FLOAT(RV_YAW_P, 0.1f); diff --git a/src/examples/rover_steering_control/params.h b/src/examples/rover_steering_control/params.h deleted file mode 100644 index 7b858f2765..0000000000 --- a/src/examples/rover_steering_control/params.h +++ /dev/null @@ -1,43 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file params.h - * - * Definition of parameters for fixedwing example - */ - -#include - -