From 1aeb1391575532986e704d320b22d663d9b1bada Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 7 Apr 2016 20:17:51 +0200 Subject: [PATCH] added motor ramp app, draft impl --- cmake/configs/posix_sitl_default.cmake | 1 + src/systemcmds/motor_ramp/CMakeLists.txt | 44 ++++ src/systemcmds/motor_ramp/motor_ramp.cpp | 299 +++++++++++++++++++++++ 3 files changed, 344 insertions(+) create mode 100644 src/systemcmds/motor_ramp/CMakeLists.txt create mode 100644 src/systemcmds/motor_ramp/motor_ramp.cpp diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 3adc2a425c..6457ea5a2b 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -30,6 +30,7 @@ set(config_module_list systemcmds/topic_listener systemcmds/ver systemcmds/top + systemcmds/motor_ramp modules/attitude_estimator_ekf modules/attitude_estimator_q diff --git a/src/systemcmds/motor_ramp/CMakeLists.txt b/src/systemcmds/motor_ramp/CMakeLists.txt new file mode 100644 index 0000000000..601d4a9199 --- /dev/null +++ b/src/systemcmds/motor_ramp/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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 systemcmds__motor_ramp + MAIN motor_ramp + STACK 1200 + COMPILE_FLAGS + -Wno-write-strings + SRCS + motor_ramp.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp new file mode 100644 index 0000000000..bdaa319b68 --- /dev/null +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -0,0 +1,299 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 motor_ramp.cpp + * Application to test motor ramp up. + * + * @author Andreas Antener + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +static bool thread_should_exit = false; /**< motor_ramp exit flag */ +static bool thread_running = false; /**< motor_ramp status flag */ +static int motor_ramp_task; /**< Handle of motor_ramp task / thread */ + +enum RampState { + RAMP_INIT, + RAMP_MIN, + RAMP_RAMP, + RAMP_WAIT +}; + +/** + * motor_ramp management function. + */ +extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]); + +/** + * Mainloop of motor_ramp. + */ +int motor_ramp_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PX4_WARN("usage: motor_ramp {start|stop|status} [-p ]\n\n"); +} + +/** + * The motor_ramp 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 motor_ramp_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage("missing command"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + PX4_WARN("motor_ramp already running\n"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + motor_ramp_task = px4_task_spawn_cmd("motor_ramp", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + motor_ramp_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) { + PX4_WARN("\trunning\n"); + + } else { + PX4_WARN("\tnot started\n"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +void set_min_pwm(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = px4_open(dev, 0); + + if (fd < 0) {PX4_WARN("can't open %s", dev);} + + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + pwm_values.channel_count = servo_count; + + for (int i = 0; i < servo_count; i++) { + pwm_values.values[i] = pwm_value; + } + + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {PX4_WARN("failed setting min values");} + + px4_close(fd); +} + +void publish(orb_advert_t actuator_outputs_pub, struct actuator_outputs_s &actuator_outputs, float output) +{ + //PX4_WARN("[motor_ramp] publish %f", (double)output); + for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + actuator_outputs.output[i] = output; + } + + orb_publish(ORB_ID(actuator_outputs), actuator_outputs_pub, &actuator_outputs); +} + +int motor_ramp_thread_main(int argc, char *argv[]) +{ + PX4_WARN("[motor_ramp] starting"); + + thread_running = true; + + struct actuator_outputs_s actuator_outputs = {}; + orb_advert_t actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs); + int ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); + + /* wakeup source */ + px4_pollfd_struct_t fds[1]; + + /* Setup of loop */ + fds[0].fd = ctrl_state_sub; + fds[0].events = POLLIN; + + float dt = 0.01f; // prevent division with 0 + float timer = 0.0f; + hrt_abstime start = 0; + hrt_abstime last_run = 0; + + enum RampState ramp_state = RAMP_INIT; + unsigned min_pwm = 1100; + float ramp_time = 0.5f; + float output = 0.0f; + + publish(actuator_outputs_pub, actuator_outputs, 0.0f); + + while (!thread_should_exit) { + if (last_run > 0) { + dt = hrt_elapsed_time(&last_run) * 1e-6; + + } else { + start = hrt_absolute_time(); + } + + last_run = hrt_absolute_time(); + timer = hrt_elapsed_time(&start) * 1e-6; + + /* wait for up to 500ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + PX4_WARN("[motor_ramp] poll error %d", pret); + continue; + } + + switch (ramp_state) { + case RAMP_INIT: { + if (min_pwm > 1350) { + thread_should_exit = true; + PX4_WARN("[motor_ramp] pwm min too high %d", min_pwm); + break; + } + + PX4_WARN("[motor_ramp] setting pwm min %d", min_pwm); + set_min_pwm(min_pwm); + ramp_state = RAMP_MIN; + break; + } + + case RAMP_MIN: { + if (timer > 3.0f) { + PX4_WARN("[motor_ramp] min set, starting ramp"); + start = hrt_absolute_time(); + ramp_state = RAMP_RAMP; + } + + break; + } + + case RAMP_RAMP: { + output += dt / ramp_time; + + if (output > 1.0f) { + output = 1.0f; + start = hrt_absolute_time(); + ramp_state = RAMP_WAIT; + PX4_WARN("[motor_ramp] ramp finished, waiting"); + } + + publish(actuator_outputs_pub, actuator_outputs, output); + break; + } + + case RAMP_WAIT: { + if (timer > 3.0f) { + thread_should_exit = true; + break; + } + + break; + } + } + } + + publish(actuator_outputs_pub, actuator_outputs, 0.0f); + + PX4_WARN("[motor_ramp] exiting"); + + thread_running = false; + + return 0; +}