diff options
38 files changed, 1860 insertions, 193 deletions
diff --git a/Documentation/code_structure_diagrams.odg b/Documentation/code_structure_diagrams.odg Binary files differindex 7bab5f4c8..2b9037103 100644 --- a/Documentation/code_structure_diagrams.odg +++ b/Documentation/code_structure_diagrams.odg diff --git a/Documentation/commander_app.odg b/Documentation/commander_app.odg Binary files differindex c05fba3c5..17459f7cf 100644 --- a/Documentation/commander_app.odg +++ b/Documentation/commander_app.odg diff --git a/Documentation/commander_app.pdf b/Documentation/commander_app.pdf Binary files differdeleted file mode 100644 index 6d55e2809..000000000 --- a/Documentation/commander_app.pdf +++ /dev/null diff --git a/Documentation/commander_app.png b/Documentation/commander_app.png Binary files differindex b71f1b8a7..6503817da 100644 --- a/Documentation/commander_app.png +++ b/Documentation/commander_app.png diff --git a/Documentation/inter_app_communication.odg b/Documentation/inter_app_communication.odg Binary files differdeleted file mode 100644 index 9f7b68440..000000000 --- a/Documentation/inter_app_communication.odg +++ /dev/null diff --git a/Documentation/inter_app_communication.pdf b/Documentation/inter_app_communication.pdf Binary files differdeleted file mode 100644 index 384894832..000000000 --- a/Documentation/inter_app_communication.pdf +++ /dev/null diff --git a/Documentation/inter_app_communication.png b/Documentation/inter_app_communication.png Binary files differdeleted file mode 100644 index 6681fe47d..000000000 --- a/Documentation/inter_app_communication.png +++ /dev/null diff --git a/Documentation/position_control.odg b/Documentation/position_control.odg Binary files differnew file mode 100644 index 000000000..5fb002c5e --- /dev/null +++ b/Documentation/position_control.odg diff --git a/Documentation/position_control.png b/Documentation/position_control.png Binary files differnew file mode 100644 index 000000000..d8d1a8b0c --- /dev/null +++ b/Documentation/position_control.png diff --git a/Documentation/state_machine.odg b/Documentation/state_machine.odg Binary files differnew file mode 100644 index 000000000..2f55a13dd --- /dev/null +++ b/Documentation/state_machine.odg diff --git a/Documentation/state_machine.png b/Documentation/state_machine.png Binary files differindex 9fef9057a..4daeddfc9 100644 --- a/Documentation/state_machine.png +++ b/Documentation/state_machine.png diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 532e95fb5..927406108 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -57,15 +57,23 @@ #define RC_INPUT_DEVICE_PATH "/dev/input_rc" /** - * Maximum number of R/C input channels in the system. + * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 16 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 * percent. */ -typedef uint8_t rc_input_t; +typedef uint16_t rc_input_t; + +enum RC_INPUT_SOURCE { + RC_INPUT_SOURCE_UNKNOWN = 0, + RC_INPUT_SOURCE_PX4FMU_PPM, + RC_INPUT_SOURCE_PX4IO_PPM, + RC_INPUT_SOURCE_PX4IO_SPEKTRUM, + RC_INPUT_SOURCE_PX4IO_SBUS +}; /** * R/C input status structure. @@ -74,10 +82,16 @@ typedef uint8_t rc_input_t; * on the board involved. */ struct rc_input_values { + /** decoding time */ + uint64_t timestamp; + /** number of channels actually being seen */ uint32_t channel_count; - /** desired pulse widths for each of the supported channels */ + /** Input source */ + enum RC_INPUT_SOURCE input_source; + + /** measured pulse widths for each of the supported channels */ rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 5050c6ce7..4687df2aa 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,9 +61,10 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> -#include <systemlib/mixer/mixer.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> +#include <systemlib/mixer/mixer.h> #include <systemlib/perf_counter.h> #include <systemlib/hx_stream.h> #include <systemlib/err.h> @@ -87,6 +88,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + void set_rx_mode(unsigned mode); + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -103,6 +106,9 @@ private: int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + orb_advert_t _to_input_rc; ///< rc inputs from io + rc_input_values _input_rc; ///< rc input values + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -114,6 +120,9 @@ private: // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO + bool _config_needed; ///< if true, we need to set a config update to IO + + uint8_t _rx_mode; ///< the current RX mode on IO /** * Trampoline to the worker task @@ -146,6 +155,11 @@ private: void io_send(); /** + * Send a config packet to PX4IO + */ + void config_send(); + + /** * Mixer control callback; invoked to fetch a control from a specific * group/index during mixing. */ @@ -176,7 +190,9 @@ PX4IO::PX4IO() : _mixers(nullptr), _primary_pwm_device(false), _switch_armed(false), - _send_needed(false) + _send_needed(false), + _config_needed(false), + _rx_mode(RX_MODE_PPM_ONLY) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -305,9 +321,14 @@ PX4IO::task_main() orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ + memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &_outputs); + /* advertise the rc inputs */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -373,6 +394,12 @@ PX4IO::task_main() _send_needed = false; io_send(); } + + /* send a config packet to IO if required */ + if (_config_needed) { + _config_needed = false; + config_send(); + } } out: @@ -438,7 +465,14 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) } _connected = true; - /* XXX handle R/C inputs here ... needs code sharing/library */ + /* publish raw rc channel values from IO */ + _input_rc.timestamp = hrt_absolute_time(); + for (int i = 0; i < rep->channel_count; i++) + { + _input_rc.values[i] = rep->rc_channel[i]; + } + + orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); /* remember the latched arming switch state */ _switch_armed = rep->armed; @@ -473,6 +507,20 @@ PX4IO::io_send() debug("send error %d", ret); } +void +PX4IO::config_send() +{ + px4io_config cfg; + int ret; + + cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + cfg.serial_rx_mode = _rx_mode; + + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + if (ret) + debug("config error %d", ret); +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -586,6 +634,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } +void +PX4IO::set_rx_mode(unsigned mode) +{ + if (mode != _rx_mode) { + _rx_mode = mode; + _config_needed = true; + } +} + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace @@ -642,9 +699,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "test")) - test(); - /* note, stop not currently implemented */ if (!strcmp(argv[1], "update")) { @@ -690,5 +744,25 @@ px4io_main(int argc, char *argv[]) return ret; } - errx(1, "need a verb, only support 'start' and 'update'"); + if (!strcmp(argv[1], "rx_spektrum6")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); + } + if (!strcmp(argv[1], "rx_spektrum7")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); + } + if (!strcmp(argv[1], "rx_sbus")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + } + + if (!strcmp(argv[1], "test")) + test(); + + + errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); } diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 6ac46092b..cd9cb45b1 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -338,7 +338,7 @@ static void hrt_call_invoke(void); /* decoded PPM buffer */ #define PPM_MAX_CHANNELS 12 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -__EXPORT unsigned ppm_decoded_channels; +__EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index e3801a417..50aa34d81 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -171,10 +171,8 @@ pwm_channel_init(unsigned channel) int up_pwm_servo_set(unsigned channel, servo_position_t value) { - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_set: bogus channel %u\n", channel); + if (channel >= PWM_SERVO_MAX_CHANNELS) return -1; - } unsigned timer = pwm_channels[channel].timer_index; @@ -214,17 +212,15 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) servo_position_t up_pwm_servo_get(unsigned channel) { - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_get: bogus channel %u\n", channel); + if (channel >= PWM_SERVO_MAX_CHANNELS) return 0; - } unsigned timer = pwm_channels[channel].timer_index; servo_position_t value = 0; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].gpio == 0)) + (pwm_channels[channel].timer_channel == 0)) return 0; /* configure the channel */ @@ -246,7 +242,7 @@ up_pwm_servo_get(unsigned channel) break; } - return value; + return value + 1; } int @@ -261,7 +257,7 @@ up_pwm_servo_init(uint32_t channel_mask) /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) pwm_channel_init(i); } diff --git a/apps/fixedwing_att_control/Makefile b/apps/fixedwing_att_control/Makefile new file mode 100644 index 000000000..01465fa9e --- /dev/null +++ b/apps/fixedwing_att_control/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Fixedwing Control application +# + +APPNAME = fixedwing_att_control +PRIORITY = SCHED_PRIORITY_MAX - 30 +STACKSIZE = 2048 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk + diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c new file mode 100644 index 000000000..18b290f99 --- /dev/null +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 fixedwing_att_control_rate.c + * Implementation of a fixed wing attitude controller. + */ +#include <fixedwing_att_control_att.h> + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> + +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <systemlib/param/param.h> +#include <systemlib/pid/pid.h> +#include <systemlib/geo/geo.h> +#include <systemlib/systemlib.h> + + + + +struct fw_att_control_params { + float roll_p; + float rollrate_lim; + float pitch_p; + float pitchrate_lim; + float yawrate_lim; + float pitch_roll_compensation_p; +}; + +struct fw_pos_control_param_handles { + param_t roll_p; + param_t rollrate_lim; + param_t pitch_p; + param_t pitchrate_lim; + param_t yawrate_lim; + param_t pitch_roll_compensation_p; +}; + + + +/* Internal Prototypes */ +static int parameters_init(struct fw_pos_control_param_handles *h); +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p); + +static int parameters_init(struct fw_pos_control_param_handles *h) +{ + /* PID parameters */ + h->roll_p = param_find("FW_ROLL_P"); + h->rollrate_lim = param_find("FW_ROLLR_LIM"); + h->pitch_p = param_find("FW_PITCH_P"); + h->pitchrate_lim = param_find("FW_PITCHR_LIM"); + h->yawrate_lim = param_find("FW_YAWR_LIM"); + h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP"); + + return OK; +} + +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p) +{ + param_get(h->roll_p, &(p->roll_p)); + param_get(h->rollrate_lim, &(p->rollrate_lim)); + param_get(h->pitch_p, &(p->pitch_p)); + param_get(h->pitchrate_lim, &(p->pitchrate_lim)); + param_get(h->yawrate_lim, &(p->yawrate_lim)); + param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p)); + + return OK; +} + +int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp) +{ + static int counter = 0; + static bool initialized = false; + + static struct fw_att_control_params p; + static struct fw_pos_control_param_handles h; + + static PID_t roll_controller; + static PID_t pitch_controller; + + + if(!initialized) + { + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + initialized = true; + } + + /* load new parameters with lower rate */ + if (counter % 100 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + } + + /* Roll (P) */ + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0); + + /* Pitch (P) */ + float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan; + rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); + + /* Yaw (from coordinated turn constraint or lateral force) */ + //TODO + + counter++; + + return 0; +} + + + diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/apps/fixedwing_att_control/fixedwing_att_control_att.h new file mode 100644 index 000000000..ca7c14b43 --- /dev/null +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * + * 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 Fixed Wing Attitude Control */ + +#ifndef FIXEDWING_ATT_CONTROL_ATT_H_ +#define FIXEDWING_ATT_CONTROL_ATT_H_ + +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> + +int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp); + +#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c new file mode 100644 index 000000000..ad0f201e1 --- /dev/null +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -0,0 +1,277 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Doug Weibel <douglas.weibel@colorado.edu> + * + * 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 fixedwing_att_control.c + * Implementation of a fixed wing attitude controller. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/debug_key_value.h> +#include <systemlib/param/param.h> +#include <systemlib/pid/pid.h> +#include <systemlib/geo/geo.h> +#include <systemlib/systemlib.h> + +#include <fixedwing_att_control_rate.h> +#include <fixedwing_att_control_att.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* Prototypes */ +/** + * Deamon management function. + */ +__EXPORT int fixedwing_att_control_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int fixedwing_att_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/* Variables */ +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 */ + +/* Main Thread */ +int fixedwing_att_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; + } + } + + /* welcome user */ + printf("[fixedwing att_control] started\n"); + + /* declare and safely initialize all structs */ + 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_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); +// static struct debug_key_value_s debug_output; +// memset(&debug_output, 0, sizeof(debug_output)); + + /* output structs */ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); +// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); +// debug_output.key[0] = '1'; + + + /* subscribe */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + + /* Setup of loop */ + float gyro[3] = {0.0f, 0.0f, 0.0f}; + struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + + while(!thread_should_exit) + { + /* wait for a sensor update, check for exit condition every 500 ms */ + poll(&fds, 1, 500); + + /*Get Local Copies */ + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + /* Control */ + + /* Attitude Control */ + fixedwing_att_control_attitude(&att_sp, + &att, + &rates_sp); + + /* Attitude Rate Control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + //REMOVEME XXX + actuators.control[3] = 0.7f; + + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// debug_output.value = rates_sp.pitch; +// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); + } + + printf("[fixedwing_att_control] exiting, stopping all motors.\n"); + thread_running = false; + + /* kill all outputs */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + + + close(att_sub); + close(actuator_pub); + + fflush(stdout); + exit(0); + + return 0; + +} + +/* Startup Functions */ + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n"); + exit(1); +} + +/** + * 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 fixedwing_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("fixedwing_att_control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("fixedwing_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4096, + fixedwing_att_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)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) { + printf("\tfixedwing_att_control is running\n"); + } else { + printf("\tfixedwing_att_control not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + + + diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c new file mode 100644 index 000000000..567f03784 --- /dev/null +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 fixedwing_att_control_rate.c + * Implementation of a fixed wing attitude controller. + */ +#include <fixedwing_att_control_rate.h> + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> + +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <systemlib/param/param.h> +#include <systemlib/pid/pid.h> +#include <systemlib/geo/geo.h> +#include <systemlib/systemlib.h> + + + + + +struct fw_rate_control_params { + float rollrate_p; + float rollrate_i; + float rollrate_awu; + float pitchrate_p; + float pitchrate_i; + float pitchrate_awu; + float yawrate_p; + float yawrate_i; + float yawrate_awu; + +}; + +struct fw_rate_control_param_handles { + param_t rollrate_p; + param_t rollrate_i; + param_t rollrate_awu; + param_t pitchrate_p; + param_t pitchrate_i; + param_t pitchrate_awu; + param_t yawrate_p; + param_t yawrate_i; + param_t yawrate_awu; + +}; + + + +/* Internal Prototypes */ +static int parameters_init(struct fw_rate_control_param_handles *h); +static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p); + +static int parameters_init(struct fw_rate_control_param_handles *h) +{ + /* PID parameters */ + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); + + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_awu = param_find("FW_PITCHR_AWU"); + + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + + return OK; +} + +static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p) +{ + param_get(h->rollrate_p, &(p->rollrate_p)); + param_get(h->rollrate_i, &(p->rollrate_i)); + param_get(h->rollrate_awu, &(p->rollrate_awu)); + param_get(h->pitchrate_p, &(p->pitchrate_p)); + param_get(h->pitchrate_i, &(p->pitchrate_i)); + param_get(h->pitchrate_awu, &(p->pitchrate_awu)); + param_get(h->yawrate_p, &(p->yawrate_p)); + param_get(h->yawrate_i, &(p->yawrate_i)); + param_get(h->yawrate_awu, &(p->yawrate_awu)); + + + return OK; +} + +int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], + struct actuator_controls_s *actuators) +{ + static int counter = 0; + static bool initialized = false; + + static struct fw_rate_control_params p; + static struct fw_rate_control_param_handles h; + + static PID_t roll_rate_controller; + static PID_t pitch_rate_controller; + static PID_t yaw_rate_controller; + + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + if(!initialized) + { + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + initialized = true; + } + + /* load new parameters with lower rate */ + if (counter % 100 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + } + + + /* Roll Rate (PI) */ + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); + + + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... + actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now + + counter++; + + return 0; +} + + + diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.h b/apps/fixedwing_att_control/fixedwing_att_control_rate.h new file mode 100644 index 000000000..d394c3dac --- /dev/null +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * + * 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 Fixed Wing Attitude Rate Control */ + +#ifndef FIXEDWING_ATT_CONTROL_RATE_H_ +#define FIXEDWING_ATT_CONTROL_RATE_H_ + +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], + struct actuator_controls_s *actuators); + +#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ diff --git a/apps/fixedwing_pos_control/Makefile b/apps/fixedwing_pos_control/Makefile new file mode 100644 index 000000000..bce391f49 --- /dev/null +++ b/apps/fixedwing_pos_control/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Fixedwing Control application +# + +APPNAME = fixedwing_pos_control +PRIORITY = SCHED_PRIORITY_MAX - 30 +STACKSIZE = 2048 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk + diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h new file mode 100644 index 000000000..f631c90a1 --- /dev/null +++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Doug Weibel <douglas.weibel@colorado.edu> + * @author Lorenz Meier <lm@inf.ethz.ch> + @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 fixedwing_pos_control.h + * Position control for fixed wing airframes. + */ + +#ifndef FIXEDWING_POS_CONTROL_H_ +#define FIXEDWING_POS_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +#endif + + +struct planned_path_segments_s { + bool segment_type; + double start_lat; // Start of line or center of arc + double start_lon; + double end_lat; + double end_lon; + float radius; // Radius of arc + float arc_start_bearing; // Bearing from center to start of arc + float arc_sweep; // Angle (radians) swept out by arc around center. + // Positive for clockwise, negative for counter-clockwise +}; + + +float _wrap180(float bearing); +float _wrap360(float bearing); +float _wrapPI(float bearing); +float _wrap2PI(float bearing); + +/* FIXEDWING_CONTROL_H_ */ + diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c new file mode 100644 index 000000000..5f5b70090 --- /dev/null +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -0,0 +1,447 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Doug Weibel <douglas.weibel@colorado.edu> + * + * 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 fixedwing_pos_control.c + * Implementation of a fixed wing attitude controller. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <systemlib/param/param.h> +#include <systemlib/pid/pid.h> +#include <systemlib/geo/geo.h> +#include <systemlib/systemlib.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ +PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track +PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians + + +struct fw_pos_control_params { + float heading_p; + float xtrack_p; + float altitude_p; + float roll_lim; + float pitch_lim; +}; + +struct fw_pos_control_param_handles { + param_t heading_p; + param_t xtrack_p; + param_t altitude_p; + param_t roll_lim; + param_t pitch_lim; + +}; + + +/* Prototypes */ +/* Internal Prototypes */ +static int parameters_init(struct fw_pos_control_param_handles *h); +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); + +/** + * Deamon management function. + */ +__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int fixedwing_pos_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/* Variables */ +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 */ + + +/** + * Parameter management + */ +static int parameters_init(struct fw_pos_control_param_handles *h) +{ + /* PID parameters */ + h->heading_p = param_find("FW_HEADING_P"); + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); + + + return OK; +} + +static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) +{ + param_get(h->heading_p, &(p->heading_p)); + param_get(h->xtrack_p, &(p->xtrack_p)); + param_get(h->altitude_p, &(p->altitude_p)); + param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->pitch_lim, &(p->pitch_lim)); + + return OK; +} + + +/* Main Thread */ +int fixedwing_pos_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; + } + } + + /* welcome user */ + printf("[fixedwing att_control] started\n"); + + /* declare and safely initialize all structs */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + struct vehicle_global_position_s start_pos; // Temporary variable, replace with + memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available + struct vehicle_global_position_setpoint_s global_setpoint; + memset(&global_setpoint, 0, sizeof(global_setpoint)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + crosstrack_error_s xtrack_err; + memset(&xtrack_err, 0, sizeof(xtrack_err)); + + /* output structs */ + struct vehicle_attitude_setpoint_s attitude_setpoint; + memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); + + /* publish attitude setpoint */ + attitude_setpoint.roll_tait_bryan = 0.0f; + attitude_setpoint.pitch_tait_bryan = 0.0f; + attitude_setpoint.yaw_tait_bryan = 0.0f; + orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); + + /* subscribe */ + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* Setup of loop */ + struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + bool global_sp_updated_set_once = false; + + float psi_track = 0.0f; + + while(!thread_should_exit) + { + /* wait for a sensor update, check for exit condition every 500 ms */ + poll(&fds, 1, 500); + + static int counter = 0; + static bool initialized = false; + + static struct fw_pos_control_params p; + static struct fw_pos_control_param_handles h; + + PID_t heading_controller; + PID_t altitude_controller; + + if(!initialized) + { + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); + initialized = true; + } + + /* load new parameters with lower rate */ + if (counter % 100 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + + } + + /* Check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + + /* Load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + if(pos_updated) + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + if (global_sp_updated) + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + + if(global_sp_updated) { + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + printf("psi_track: %0.4f\n", psi_track); + } + + /* Control */ + + + /* Simple Horizontal Control */ + if(global_sp_updated_set_once) + { + if (counter % 100 == 0) + printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); + + /* calculate bearing error */ +// float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, +// global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + xtrack_err = get_distance_to_line((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + if(!(xtrack_err.error || xtrack_err.past_end)) { + + float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + + if(delta_psi_c > 60.0f*M_DEG_TO_RAD) + delta_psi_c = 60.0f*M_DEG_TO_RAD; + + if(delta_psi_c < -60.0f*M_DEG_TO_RAD) + delta_psi_c = -60.0f*M_DEG_TO_RAD; + + float psi_c = psi_track + delta_psi_c; + + + float psi_e = psi_c - att.yaw; + + + /* shift error to prevent wrapping issues */ + psi_e = _wrapPI(psi_e); + + /* calculate roll setpoint, do this artificially around zero */ + attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + + if (counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } + else { + if (counter % 100 == 0) + printf("error: %d, past_end %d\n", xtrack_err.error, xtrack_err.past_end); + } + + } + + /* Very simple Altitude Control */ + if(global_sp_updated_set_once && pos_updated) + { + + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + + } + /*Publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + counter++; + } + + printf("[fixedwing_pos_control] exiting.\n"); + thread_running = false; + + + close(attitude_setpoint_pub); + + fflush(stdout); + exit(0); + + return 0; + +} + +/* Startup Functions */ + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n"); + exit(1); +} + +/** + * 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 fixedwing_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("fixedwing_pos_control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("fixedwing_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4096, + fixedwing_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)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) { + printf("\tfixedwing_pos_control is running\n"); + } else { + printf("\tfixedwing_pos_control not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + + +float _wrapPI(float bearing) +{ + + while (bearing > M_PI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing <= -M_PI_F) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +float _wrap2PI(float bearing) +{ + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +float _wrap180(float bearing) +{ + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + +float _wrap360(float bearing) +{ + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + + + + diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7d8232b3a..9b2cfcbba 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -589,16 +589,15 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 0dff743bc..39e574c04 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg) rc_hil.timestamp = hrt_absolute_time(); rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; rc_hil.chan[0].scaled = man.x / 1000.0f; rc_hil.chan[1].scaled = man.y / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 4b66ee438..a6ae94495 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; +struct rc_input_values rc_raw; struct actuator_armed_s armed; struct mavlink_subscriptions mavlink_subs; @@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l); static void l_vehicle_gps_position(struct listener *l); static void l_vehicle_status(struct listener *l); static void l_rc_channels(struct listener *l); +static void l_input_rc(struct listener *l); static void l_global_position(struct listener *l); static void l_local_position(struct listener *l); static void l_global_position_setpoint(struct listener *l); @@ -116,6 +118,7 @@ struct listener listeners[] = { {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, {l_vehicle_status, &status_sub, 0}, {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, @@ -274,21 +277,29 @@ l_rc_channels(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc.timestamp / 1000, + rc_raw.timestamp / 1000, 0, - rc.chan[0].raw, - rc.chan[1].raw, - rc.chan[2].raw, - rc.chan[3].raw, - rc.chan[4].raw, - rc.chan[5].raw, - rc.chan[6].raw, - rc.chan[7].raw, - rc.rssi); + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -474,7 +485,6 @@ l_vehicle_attitude_controls(struct listener *l) /* Only send in HIL mode */ if (mavlink_hil_enabled) { - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; @@ -584,6 +594,10 @@ uorb_receive_start(void) rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index f041e7c4c..6860702d2 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -57,6 +57,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> +#include <drivers/drv_rc_input.h> struct mavlink_subscriptions { int sensor_sub; @@ -75,6 +76,7 @@ struct mavlink_subscriptions { int spl_sub; int spg_sub; int debug_key_value; + int input_rc_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 8fdf7d1c8..1edff25b1 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -116,18 +116,32 @@ int frame_rx; int frame_bad; static void -comms_handle_frame(void *arg, const void *buffer, size_t length) +comms_handle_config(const void *buffer, size_t length) { - struct px4io_command *cmd = (struct px4io_command *)buffer; + const struct px4io_config *cfg = (struct px4io_config *)buffer; - /* make sure it's what we are expecting */ - if ((length != sizeof(struct px4io_command)) || - (cmd->f2i_magic != F2I_MAGIC)) { - frame_bad++; + if (length != sizeof(*cfg)) { + frame_bad++; return; } + frame_rx++; + mixer_set_serial_mode(cfg->serial_rx_mode); + +} + +static void +comms_handle_command(const void *buffer, size_t length) +{ + const struct px4io_command *cmd = (struct px4io_command *)buffer; + + if (length != sizeof(*cmd)) { + frame_bad++; + return; + } + + frame_rx++; irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -146,6 +160,29 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; -out: irqrestore(flags); } + + +static void +comms_handle_frame(void *arg, const void *buffer, size_t length) +{ + const uint16_t *type = (const uint16_t *)buffer; + + + /* make sure it's what we are expecting */ + if (length > 2) { + switch (*type) { + case F2I_MAGIC: + comms_handle_command(buffer, length); + break; + case F2I_CONFIG_MAGIC: + comms_handle_config(buffer, length); + break; + default: + break; + } + } + frame_bad++; +} + diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 94d10ef57..471965fd7 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -40,10 +40,13 @@ #include <sys/types.h> #include <stdbool.h> - +#include <string.h> #include <assert.h> #include <errno.h> #include <fcntl.h> +#include <termios.h> +#include <unistd.h> +#include <fcntl.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> @@ -65,6 +68,11 @@ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 /* + * Serial port fd for serial RX protocols + */ +static int rx_port = -1; + +/* * HRT periodic call used to check for control input data. */ static struct hrt_call mixer_input_call; @@ -96,14 +104,49 @@ struct mixer { } mixers[IO_SERVO_COUNT]; int -mixer_init(const char *mq_name) +mixer_init(void) { + /* open the serial port */ + rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); return 0; } +void +mixer_set_serial_mode(uint8_t serial_mode) +{ + + if (serial_mode == system_state.serial_rx_mode) + return; + + struct termios t; + tcgetattr(rx_port, &t); + + switch (serial_mode) { + case RX_MODE_PPM_ONLY: + break; + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + /* 115200, no parity, one stop bit */ + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + break; + case RX_MODE_FUTABA_SBUS: + /* 100000, even parity, two stop bits */ + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + break; + default: + return; + } + + tcsetattr(rx_port, TCSANOW, &t); + system_state.serial_rx_mode = serial_mode; +} + static void mixer_tick(void *arg) { @@ -181,22 +224,145 @@ mixer_tick(void *arg) } static void -mixer_get_rc_input(void) +mixer_update(int mixer, uint16_t *inputs, int input_count) { + /* simple passthrough for now */ + if (mixer < input_count) { + mixers[mixer].current_value = inputs[mixer]; + } else { + mixers[mixer].current_value = 0; + } +} + +static bool +mixer_get_spektrum_input(void) +{ + static uint8_t buf[16]; + static unsigned count; + + /* always read as much data as we can into the buffer */ + if (count >= sizeof(buf)) + count = 0; + ssize_t result = read(rx_port, buf, sizeof(buf) - count); + /* no data or an error */ + if (result <= 0) + return false; + count += result; + + /* if there are more than two bytes in the buffer, check for sync */ + if (count >= 2) { + if ((buf[0] != 0x3) || (buf[1] != 0x1)) { + /* not in sync; look for a possible sync marker */ + for (unsigned i = 1; i < count; i++) { + if (buf[i] == 0x3) { + /* could be a frame marker; move buffer bytes */ + count -= i; + memmove(buf, buf + i, count); + break; + } + } + } + } + if (count < sizeof(buf)) + return false; + + /* we got a frame; decode it */ + const uint16_t *channels = (const uint16_t *)&buf[2]; + /* - * XXX currently only supporting PPM + * Channel assignment for DX6i vs. DX7 is different. + * + * DX7 etc. is: + * + * 0: Aileron + * 1: Flaps + * 2: Gear + * 3: Elevator + * 4: Aux2 + * 5: Throttle + * 6: Rudder + * + * DX6i is: * - * XXX check timestamp to ensure current + * 0: Aileron + * 1: Flaps + * 2: Elevator + * 3: Rudder + * 4: Throttle + * 5: Gear + * 6: <notused> + * + * We convert these to our standard Futaba-style assignment: + * + * 0: Throttle (Throttle) + * 1: Roll (Aileron) + * 2: Pitch (Elevator) + * 3: Yaw (Rudder) + * 4: Override (Flaps) + * 5: FUNC_0 (Gear) + * 6: FUNC_1 (Aux2) */ - if (ppm_decoded_channels > 0) { - mixer_input_drops = 0; - system_state.fmu_report_due = true; + if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) { + system_state.rc_channel_data[0] = channels[5]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[3]; /* Pitch */ + system_state.rc_channel_data[3] = channels[6]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */ + system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */ + system_state.rc_channels = 7; + } else { + system_state.rc_channel_data[0] = channels[4]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[2]; /* Pitch */ + system_state.rc_channel_data[3] = channels[3]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */ + system_state.rc_channels = 6; + } + count = 0; + return true; +} + +static bool +mixer_get_sbus_input(void) +{ + /* XXX not implemented yet */ + return false; +} + +static void +mixer_get_rc_input(void) +{ + bool got_input = false; + + switch (system_state.serial_rx_mode) { + case RX_MODE_PPM_ONLY: + if (ppm_decoded_channels > 0) { + /* copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + got_input = true; + } + break; + + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + got_input = mixer_get_spektrum_input(); + break; - /* copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; + case RX_MODE_FUTABA_SBUS: + got_input = mixer_get_sbus_input(); + break; + default: + break; + } + + if (got_input) { + mixer_input_drops = 0; + system_state.fmu_report_due = true; } else { /* * No data; count the 'frame drops' and once we hit the limit @@ -213,14 +379,3 @@ mixer_get_rc_input(void) } } } - -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) -{ - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c4a63d886..7467f1adc 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,6 +62,18 @@ struct px4io_command { bool arm_ok; }; +/* config message from FMU to IO */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + uint8_t serial_rx_mode; +#define RX_MODE_PPM_ONLY 0 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 +#define RX_MODE_FUTABA_SBUS 3 +}; + /* report from IO to FMU */ struct px4io_report { uint16_t i2f_magic; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 90057c790..7039e5d58 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -59,8 +59,6 @@ int gpio_fd; static const char cursor[] = {'|', '/', '-', '\\'}; -static const char *rc_input_mq_name = "rc_input"; - static struct hrt_call timer_tick_call; volatile int timers[TIMER_NUM_TIMERS]; static void timer_tick(void *arg); @@ -74,7 +72,11 @@ int user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); - /* configure the PWM outputs */ + /* init the FMU link */ + comms_init(); + + /* configure the first 8 PWM outputs (i.e. all of them) */ + /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ up_pwm_servo_init(0xff); /* print some startup info */ @@ -94,16 +96,13 @@ int user_start(int argc, char *argv[]) POWER_SERVO(true); /* start the mixer */ - mixer_init(rc_input_mq_name); + mixer_init(); /* start the safety switch handler */ safety_init(); - /* init the FMU link */ - comms_init(); - /* set up some timers for the main loop */ - timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */ + timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */ timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ /* @@ -115,21 +114,21 @@ int user_start(int argc, char *argv[]) comms_check(); /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_AMBER] == 0) { - timers[TIMER_BLINK_AMBER] = 250; - LED_AMBER(heartbeat = !heartbeat); + if (timers[TIMER_BLINK_BLUE] == 0) { + timers[TIMER_BLINK_BLUE] = 250; + LED_BLUE(heartbeat = !heartbeat); } /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 125; + if (timers[TIMER_BLINK_AMBER] == 0) { + timers[TIMER_BLINK_AMBER] = 125; failsafe = !failsafe; } } else { failsafe = false; } - LED_BLUE(failsafe); + LED_AMBER(failsafe); /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index bbbe91865..274b27ff3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -53,6 +53,16 @@ #define IO_SERVO_COUNT 8 /* + * Debug logging + */ + +#if 1 +# define debug(fmt, ...) lib_lowprintf(fmt "\n", ##args) +#else +# define debug(fmt, ...) do {} while(0) +#endif + +/* * System state structure. */ struct sys_state_s @@ -91,6 +101,12 @@ struct sys_state_s * If true, new control data from the FMU has been received. */ bool fmu_data_received; + + /* + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; }; extern struct sys_state_s system_state; @@ -113,8 +129,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * GPIO handling. */ -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) @@ -130,7 +146,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * Mixer */ -extern int mixer_init(const char *mq_name); +extern int mixer_init(void); +extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 453502b05..3e9f35eaf 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -58,11 +58,13 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_rc_input.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> + #include <systemlib/ppm_decode.h> #include <uORB/uORB.h> @@ -143,6 +145,7 @@ private: int _gyro_sub; /**< raw gyro data subscription */ int _accel_sub; /**< raw accel data subscription */ int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -162,6 +165,7 @@ private: float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; float ex[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; float mag_offset[3]; @@ -331,6 +335,7 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _rc_sub(-1), _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -464,14 +469,13 @@ Sensors::parameters_update() warnx("Failed getting exponential gain for chan %d", i); } - _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) { - _rc.chan[i].scaling_factor = 0; + if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + _parameters.scaling_factor[i] = 0; } - _rc.chan[i].mid = _parameters.trim[i]; } /* update RC function mappings */ @@ -856,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - struct manual_control_setpoint_s manual_control; - - /* check to see whether a new frame has been decoded */ - if (_ppm_last_valid == ppm_last_valid_decode) - return; - /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 4) - return; - - unsigned channel_limit = ppm_decoded_channels; - if (channel_limit > _rc_max_chan_count) - channel_limit = _rc_max_chan_count; - - /* we are accepting this decode */ - _ppm_last_valid = ppm_last_valid_decode; - - /* Read out values from HRT */ - for (unsigned int i = 0; i < channel_limit; i++) { - _rc.chan[i].raw = ppm_buffer[i]; - - /* scale around the mid point differently for lower and upper range */ - if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); - } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - - } else { - /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + /* fake low-level driver, directly pulling from driver variables */ + static orb_advert_t rc_input_pub = -1; + struct rc_input_values raw; + + raw.timestamp = ppm_last_valid_decode; + + if (ppm_decoded_channels > 1) { + + for (int i = 0; i < ppm_decoded_channels; i++) { + raw.values[i] = ppm_buffer[i]; } - /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } + raw.channel_count = ppm_decoded_channels; + + /* publish to object request broker */ + if (rc_input_pub <= 0) { + rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); } else { - _rc.chan[i].scaled *= _parameters.rev[i]; + orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } + } - /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) - _rc.chan[i].scaled = 0.0f; - //_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; - } + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - _rc.chan_count = ppm_decoded_channels; - _rc.timestamp = ppm_last_valid_decode; + if (rc_updated) { + struct rc_input_values rc_input; - manual_control.timestamp = ppm_last_valid_decode; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } + struct manual_control_setpoint_s manual_control; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + /* require at least two chanels to consider the signal valid */ + if (rc_input.channel_count < 2) + return; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; - /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + /* Read out values from raw message */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* scale around the mid point differently for lower and upper range */ + if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ + _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } + + /* reverse channel if required */ + if (i == _rc.function[THROTTLE]) { + if ((int)_parameters.rev[i] == -1) { + _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; + } + } else { + _rc.chan[i].scaled *= _parameters.rev[i]; + } + + /* handle any parameter-induced blowups */ + if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + } + + _rc.chan_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } } #endif @@ -979,6 +1010,7 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 2c487cf3b..fef6ef2b3 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,14 +50,6 @@ * @{ */ -enum RC_CHANNELS_STATUS -{ - UNKNOWN = 0, - KNOWN = 1, - SIGNAL = 2, - TIMEOUT = 3 -}; - /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -85,12 +77,7 @@ struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { - uint16_t mid; /**< midpoint (0). */ - float scaling_factor; /**< scaling factor from raw counts to -1..+1 */ - uint16_t raw; /**< current raw value */ float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - uint16_t override; - enum RC_CHANNELS_STATUS status; /**< status of the channel */ } chan[RC_CHANNELS_FUNCTION_MAX]; uint8_t chan_count; /**< maximum number of valid channels */ @@ -98,6 +85,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; uint8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 62367cf77..8663846fc 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,9 +56,9 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - //float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - //float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - //float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ //float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ float roll_body; /**< body angle in NED frame */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5b2546911..c696407ea 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -75,7 +75,9 @@ CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += fixedwing_control +#CONFIGURED_APPS += fixedwing_control +CONFIGURED_APPS += fixedwing_att_control +CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 64b521e21..e90e7ce62 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n # CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity # CONFIG_USARTn_2STOP - Two stop bits # +CONFIG_SERIAL_TERMIOS=y + CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n @@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=32 -CONFIG_USART1_RXBUFSIZE=32 +CONFIG_USART1_RXBUFSIZE=64 CONFIG_USART2_RXBUFSIZE=128 CONFIG_USART3_RXBUFSIZE=32 -CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 |
