From 87618bb9f28dcbb26a7b0c5f50b62edd53949699 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 3 Nov 2012 19:44:06 -0700 Subject: [PATCH 01/12] Added mixer files for hex and octo --- ROMFS/Makefile | 4 ++++ ROMFS/mixers/FMU_hex_+.mix | 7 +++++++ ROMFS/mixers/FMU_hex_x.mix | 7 +++++++ ROMFS/mixers/FMU_octo_+.mix | 7 +++++++ ROMFS/mixers/FMU_octo_x.mix | 7 +++++++ 5 files changed, 32 insertions(+) create mode 100644 ROMFS/mixers/FMU_hex_+.mix create mode 100644 ROMFS/mixers/FMU_hex_x.mix create mode 100644 ROMFS/mixers/FMU_octo_+.mix create mode 100644 ROMFS/mixers/FMU_octo_x.mix diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 56ae63d271..ec4221b93a 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -29,6 +29,10 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \ $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \ + $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \ + $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \ + $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \ + $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \ $(SRCROOT)/logging/logconv.m~logging/logconv.m # diff --git a/ROMFS/mixers/FMU_hex_+.mix b/ROMFS/mixers/FMU_hex_+.mix new file mode 100644 index 0000000000..b5e38ce9ef --- /dev/null +++ b/ROMFS/mixers/FMU_hex_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the + configuration. All controls +are mixed 100%. + +R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_hex_x.mix b/ROMFS/mixers/FMU_hex_x.mix new file mode 100644 index 0000000000..8e8d122adc --- /dev/null +++ b/ROMFS/mixers/FMU_hex_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the X configuration. All controls +are mixed 100%. + +R: 6x 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_octo_+.mix b/ROMFS/mixers/FMU_octo_+.mix new file mode 100644 index 0000000000..2cb70e814c --- /dev/null +++ b/ROMFS/mixers/FMU_octo_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the + configuration. All controls +are mixed 100%. + +R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_octo_x.mix b/ROMFS/mixers/FMU_octo_x.mix new file mode 100644 index 0000000000..edc71f0139 --- /dev/null +++ b/ROMFS/mixers/FMU_octo_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the X configuration. All controls +are mixed 100%. + +R: 8x 10000 10000 10000 0 From e0e96c005aab8b35f68ea3def75f5e762a115d32 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2012 18:57:42 -0800 Subject: [PATCH 02/12] Corrected two small bugs considering arming and disarming --- apps/drivers/px4io/px4io.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 4687df2aa0..0ec1b9922f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -37,7 +37,6 @@ * * PX4IO is connected via serial (or possibly some other interface at a later * point). - */ #include @@ -384,7 +383,7 @@ PX4IO::task_main() if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); + orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } } @@ -538,7 +537,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_DISARM: /* fake a disarmed transition */ - _armed.armed = true; + _armed.armed = false; _send_needed = true; break; From 08d6ade585200e2c6bed73d02ebfe990e6929446 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2012 19:01:04 -0800 Subject: [PATCH 03/12] Some failsafe hack when RC signal is lost --- apps/multirotor_att_control/multirotor_att_control_main.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 29463d7447..bfa5e1b1f6 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -226,6 +226,14 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } att_sp.thrust = manual.throttle; + + if(state.rc_signal_lost) { + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.4f; + } + att_sp.timestamp = hrt_absolute_time(); } /* STEP 2: publish the result to the vehicle actuators */ From 7da799d154f6f3c5f0c1a92cbad4288f83c90650 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2012 19:02:01 -0800 Subject: [PATCH 04/12] Scale control signals with thrust for low thrust --- apps/systemlib/mixer/mixer_multirotor.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 0e714ed50a..a3577245da 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space) float max = 0.0f; float fixup_scale; + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float output_factor = 0.0f; + float startpoint_full_control = 0.20f; + float endpoint_full_control = 0.80f; + + if (thrust <= min_thrust) { + output_factor = 0.0f; + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust/max_thrust)/startpoint_full_control; + } else if (thrust >= startpoint_full_control && thrust < endpoint_full_control) { + output_factor = max_thrust; + } else if (thrust >= endpoint_full_control) { + output_factor = max_thrust/(endpoint_full_control-max_thrust); + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + + /* perform initial mix pass yielding un-bounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { float tmp = roll * _rotors[i].roll_scale + From 74f76006029ad6b209c1a409e8e35044b1c53bbb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 6 Nov 2012 19:03:08 -0800 Subject: [PATCH 05/12] Again some hacking to make arming/disarming work for now --- apps/px4io/comms.c | 10 ++++++++-- apps/px4io/mixer.c | 14 +++++++------- apps/px4io/px4io.c | 3 ++- apps/px4io/px4io.h | 2 +- apps/px4io/safety.c | 4 ++-- 5 files changed, 20 insertions(+), 13 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 1edff25b17..0a0453b550 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -148,13 +148,19 @@ comms_handle_command(const void *buffer, size_t length) for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) system_state.fmu_channel_data[i] = cmd->servo_command[i]; + /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + if(system_state.arm_ok && !cmd->arm_ok) { + system_state.armed = false; + } + system_state.arm_ok = cmd->arm_ok; system_state.mixer_use_fmu = true; system_state.fmu_data_received = true; + /* handle changes signalled by FMU */ - if (!system_state.arm_ok && system_state.armed) - system_state.armed = false; +// if (!system_state.arm_ok && system_state.armed) +// system_state.armed = false; /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 471965fd7c..28edb2357e 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -93,7 +93,7 @@ static void mixer_get_rc_input(void); static void mixer_update(int mixer, uint16_t *inputs, int input_count); /* current servo arm/disarm state */ -bool mixer_servos_armed; +bool mixer_servos_armed = false; /* * Each mixer consumes a set of inputs and produces a single output. @@ -182,10 +182,10 @@ mixer_tick(void *arg) system_state.fmu_data_received = false; } - } else if (system_state.rc_channels > 0) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; +// } else if (system_state.rc_channels > 0) { +// /* we have control data from an R/C input */ +// control_count = system_state.rc_channels; +// control_values = &system_state.rc_channel_data[0]; } else { /* we have no control input */ @@ -202,7 +202,7 @@ mixer_tick(void *arg) /* * If we are armed, update the servo output. */ - if (system_state.armed) + if (system_state.armed && system_state.arm_ok) up_pwm_servo_set(i, mixers[i].current_value); } } @@ -210,7 +210,7 @@ mixer_tick(void *arg) /* * Decide whether the servos should be armed right now. */ - should_arm = system_state.armed && (control_count > 0); + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 7039e5d583..7240a80efd 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -133,8 +133,9 @@ int user_start(int argc, char *argv[]) /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r", + lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r", cursor[cycle++ & 3], + (system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"), (system_state.armed ? "ARMED" : "SAFE"), (system_state.rc_channels ? "RC OK" : "NO RC"), (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 274b27ff31..8eb0c53b21 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -68,7 +68,7 @@ struct sys_state_s { - bool armed; /* actually armed */ + bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ /* diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index f274326643..0ad636c0b3 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -76,9 +76,9 @@ safety_check_button(void *arg) /* * Debounce the safety button, change state if it has been held for long enough. * - * Ignore the button if FMU has not said it's OK to arm yet. */ - if (BUTTON_SAFETY && system_state.arm_ok) { + + if (BUTTON_SAFETY) { if (arm_counter < ARM_COUNTER_THRESHOLD) { arm_counter++; } else if (arm_counter == ARM_COUNTER_THRESHOLD) { From 5995240a078828a3de41aea6ad03aa0cb3952613 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 Nov 2012 10:50:52 -0800 Subject: [PATCH 06/12] Failsafe throttle when RC is lost is now a parameter --- .../multirotor_att_control/multirotor_att_control_main.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index bfa5e1b1f6..78ea630189 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -68,10 +68,13 @@ #include #include +#include #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" +PARAM_DEFINE_FLOAT(MC_RCLOSS_THROT, 0.0f); + __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -142,6 +145,9 @@ mc_thread_main(int argc, char *argv[]) bool flag_system_armed = false; bool man_yaw_zero_once = false; + param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THROT"); + float failsafe_throttle = 0.0f; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -228,10 +234,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.thrust = manual.throttle; if(state.rc_signal_lost) { + param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.4f; + att_sp.thrust = failsafe_throttle; } att_sp.timestamp = hrt_absolute_time(); From 25ed791b7016cbbcd2dfeac8b62461733bd5a26e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 Nov 2012 10:56:25 -0800 Subject: [PATCH 07/12] Forgot to comment --- apps/multirotor_att_control/multirotor_att_control_main.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 78ea630189..951840ab13 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -73,7 +73,7 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THROT, 0.0f); +PARAM_DEFINE_FLOAT(MC_RCLOSS_THROT, 0.0f); // This defines the throttle when the RC signal is lost. __EXPORT int multirotor_att_control_main(int argc, char *argv[]); @@ -145,6 +145,7 @@ mc_thread_main(int argc, char *argv[]) bool flag_system_armed = false; bool man_yaw_zero_once = false; + /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THROT"); float failsafe_throttle = 0.0f; @@ -233,7 +234,9 @@ mc_thread_main(int argc, char *argv[]) } att_sp.thrust = manual.throttle; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if(state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; From 38b20f2a1ad32ed3091c781713c01f209f95016a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 Nov 2012 14:46:28 -0800 Subject: [PATCH 08/12] Revert "Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs." This reverts commit ff3a014971f83f15f4884e584a2f58ee979f23ee. --- apps/drivers/px4io/px4io.cpp | 10 +- apps/px4io/comms.c | 119 +++------------------ apps/px4io/dsm.c | 184 -------------------------------- apps/px4io/mixer.c | 199 ++++++++++++++++++++++++++++++++--- apps/px4io/protocol.h | 9 +- apps/px4io/px4io.c | 2 +- apps/px4io/px4io.h | 9 +- apps/px4io/sbus.c | 58 ---------- 8 files changed, 213 insertions(+), 377 deletions(-) delete mode 100644 apps/px4io/dsm.c delete mode 100644 apps/px4io/sbus.c diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index cd8783673c..0ec1b9922f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -743,15 +743,15 @@ px4io_main(int argc, char *argv[]) return ret; } - if (!strcmp(argv[1], "rx_dsm_10bit")) { + if (!strcmp(argv[1], "rx_spektrum6")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_10BIT); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); } - if (!strcmp(argv[1], "rx_dsm_11bit")) { + if (!strcmp(argv[1], "rx_spektrum7")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_DSM_11BIT); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); } if (!strcmp(argv[1], "rx_sbus")) { if (g_dev == nullptr) @@ -763,5 +763,5 @@ px4io_main(int argc, char *argv[]) test(); - errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f1dac433fb..0a0453b550 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -44,7 +44,6 @@ #include #include #include -#include #include #include @@ -61,126 +60,26 @@ static int fmu_fd; static hx_stream_t stream; -static int rx_fd; + static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); -static struct pollfd pollfds[2]; -static int pollcount; - void comms_init(void) { - /* initialise the FMU interface */ fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); if (fmu_fd < 0) lib_lowprintf("COMMS: fmu open failed %d\n", errno); + stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); - pollfds[0].fd = fmu_fd; - pollfds[0].events = POLLIN; - pollcount = 1; - /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; - -} - -static void -serial_rx_init(unsigned serial_mode) -{ - if (serial_mode == system_state.serial_rx_mode) - return; - system_state.serial_rx_mode = serial_mode; - - if (serial_mode == RX_MODE_PPM_ONLY) { - if (rx_fd != -1) { - pollcount = 1; - close(rx_fd); - rx_fd = -1; - } - return; - } - - /* open the serial port used for DSM and S.bus communication */ - rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); - pollfds[1].fd = rx_fd; - pollfds[1].events = POLLIN; - pollcount = 2; - - struct termios t; - tcgetattr(rx_fd, &t); - - switch (serial_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: - - /* 115200, no parity, one stop bit */ - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - - dsm_init(serial_mode); - break; - - case RX_MODE_FUTABA_SBUS: - /* 100000, even parity, two stop bits */ - cfsetspeed(&t, 100000); - t.c_cflag |= (CSTOPB | PARENB); - - sbus_init(serial_mode); - break; - - default: - break; - } - - tcsetattr(rx_fd, TCSANOW, &t); } void comms_check(void) { - /* - * Check for serial data - */ - int ret = poll(pollfds, pollcount, 0); - - if (ret > 0) { - /* - * Pull bytes from FMU and feed them to the HX engine. - * Limit the number of bytes we actually process on any one iteration. - */ - if (pollfds[0].revents & POLLIN) { - char buf[32]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); - } - - /* - * Pull bytes from the serial RX port and feed them to the decoder - * if we care about serial RX data. - */ - if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { - switch (system_state.serial_rx_mode) { - case RX_MODE_DSM_10BIT: - case RX_MODE_DSM_11BIT: - dsm_input(rx_fd); - break; - - case RX_MODE_FUTABA_SBUS: - sbus_input(rx_fd); - break; - - default: - break; - } - } - } - - /* - * Decide if it's time to send an update to the FMU. - */ static hrt_abstime last_report_time; hrt_abstime now, delta; @@ -188,7 +87,7 @@ comms_check(void) now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && - (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { + (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; @@ -202,6 +101,15 @@ comms_check(void) /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } + + /* + * Check for bytes and feed them to the RX engine. + * Limit the number of bytes we actually process on any one iteration. + */ + char buf[32]; + ssize_t count = read(fmu_fd, buf, sizeof(buf)); + for (int i = 0; i < count; i++) + hx_stream_rx(stream, buf[i]); } int frame_rx; @@ -219,7 +127,8 @@ comms_handle_config(const void *buffer, size_t length) frame_rx++; - serial_rx_init(cfg->serial_rx_mode); + mixer_set_serial_mode(cfg->serial_rx_mode); + } static void diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c deleted file mode 100644 index 79b6301c7d..0000000000 --- a/apps/px4io/dsm.c +++ /dev/null @@ -1,184 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file dsm.c - * - * Serial protocol decoder for the Spektrum DSM* family of protocols. - * - * Decodes into the global PPM buffer and updates accordingly. - */ - -#include - -#include -#include -#include - -#include - -#include - -#include "px4io.h" -#include "protocol.h" - -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -static hrt_abstime last_frame_time; - -static uint8_t frame[DSM_FRAME_SIZE]; - -static unsigned partial_frame_count; -static bool insync; -static unsigned channel_shift; - -static void dsm_decode(void); - -void -dsm_init(unsigned mode) -{ - insync = false; - partial_frame_count = 0; - - if (mode == RX_MODE_DSM_10BIT) { - channel_shift = 10; - } else { - channel_shift = 11; - } - - last_frame_time = hrt_absolute_time(); -} - -void -dsm_input(int fd) -{ - uint8_t buf[DSM_FRAME_SIZE]; - ssize_t ret; - hrt_abstime now; - - /* - * The DSM* protocol doesn't provide any explicit framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 11ms; with 16 bytes at 115200bps - * frame transmission time is ~1.4ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, - * the first byte we read will be the first byte of a frame. - */ - now = hrt_absolute_time(); - if ((now - last_frame_time) > 5000) - partial_frame_count = 0; - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return; - - /* - * Add bytes to the current frame - */ - memcpy(&frame[partial_frame_count], buf, ret); - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return; - last_frame_time = now; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - dsm_decode(); - partial_frame_count = 0; -} - -static void -dsm_decode(void) -{ - uint16_t data_mask = (1 << channel_shift) - 1; - - /* - * The encoding of the first byte is uncertain, so we're going - * to ignore it for now. - * - * The second byte may tell us about the protocol, but it's not - * actually very interesting since what we really want to know - * is how the channel data is formatted, and there doesn't seem - * to be a reliable way to determine this from the protocol ID - * alone. - * - * Each channel is a 16-bit unsigned value containing either a 10- - * or 11-bit channel value and a 4-bit channel number, shifted - * either 10 or 11 bits. The MSB may also be set to indicate the - * second frame in variants of the protocol where more than - * seven channels are being transmitted. - */ - - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - - uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - - /* ignore pad channels */ - if (raw == 0xffff) - continue; - - unsigned channel = (raw >> channel_shift) & 0xf; - - /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) - continue; - - if (channel > ppm_decoded_channels) - ppm_decoded_channels = channel; - - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - unsigned data = raw & data_mask; - if (channel_shift == 11) - data /= 2; - ppm_buffer[channel] = 988 + data; - - } - ppm_last_valid_decode = hrt_absolute_time(); -} diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index fa4b3eb252..28edb2357e 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -32,9 +32,7 @@ ****************************************************************************/ /** - * @file mixer.c - * - * Control channel input/output mixer and failsafe. + * @file Control channel input/output mixer and failsafe. */ #include @@ -45,6 +43,8 @@ #include #include #include +#include +#include #include #include @@ -55,12 +55,23 @@ #include "px4io.h" +/* + * Count of periodic calls in which we have no data. + */ +static unsigned mixer_input_drops; +#define MIXER_INPUT_DROP_LIMIT 10 + /* * Count of periodic calls in which we have no FMU input. */ 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. */ @@ -95,7 +106,8 @@ struct mixer { int 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); @@ -103,6 +115,37 @@ mixer_init(void) 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) @@ -191,20 +234,148 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) } } +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]; + + /* + * 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: + * + * 0: Aileron + * 1: Flaps + * 2: Elevator + * 3: Rudder + * 4: Throttle + * 5: Gear + * 6: + * + * 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 (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; - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ - if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; - return; + 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; + + case RX_MODE_FUTABA_SBUS: + got_input = mixer_get_sbus_input(); + break; + + default: + break; } - /* otherwise, 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]; - system_state.fmu_report_due = true; + 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 + * assume that we have lost input. + */ + if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) { + mixer_input_drops++; + + /* if we hit the limit, stop pretending we have input and let the FMU know */ + if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) { + system_state.rc_channels = 0; + system_state.fmu_report_due = true; + } + } + } } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 660eed12b6..7467f1adcb 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,6 +41,11 @@ #pragma once +/* + * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL + * TREES ARE MERGED. + */ + #define PX4IO_OUTPUT_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 @@ -64,8 +69,8 @@ struct px4io_config { uint8_t serial_rx_mode; #define RX_MODE_PPM_ONLY 0 -#define RX_MODE_DSM_10BIT 1 -#define RX_MODE_DSM_11BIT 2 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 #define RX_MODE_FUTABA_SBUS 3 }; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index dec2cdde67..7240a80efd 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -72,7 +72,7 @@ int user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); - /* init the FMU and receiver links */ + /* init the FMU link */ comms_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index f50e29252a..8eb0c53b21 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -147,6 +147,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern int mixer_init(void); +extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. @@ -159,14 +160,6 @@ extern void safety_init(void); extern void comms_init(void); extern void comms_check(void); -/* - * Serial receiver decoders. - */ -extern void dsm_init(unsigned mode); -extern void dsm_input(int fd); -extern void sbus_init(unsigned mode); -extern void sbus_input(int fd); - /* * Assertion codes */ diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c deleted file mode 100644 index e363a0a78f..0000000000 --- a/apps/px4io/sbus.c +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file sbus.c - * - * Serial protocol decoder for the Futaba S.bus protocol. - */ - -#include - -#include -#include - -#include - -#include "px4io.h" -#include "protocol.h" - -void -sbus_init(unsigned mode) -{ -} - -void -sbus_input(int fd) -{ -} From 2b5f55183839d195ded5e753a583aeffc45c28e0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 Nov 2012 14:56:03 -0800 Subject: [PATCH 09/12] Revert "Revert "Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs."" This reverts commit 38b20f2a1ad32ed3091c781713c01f209f95016a. --- apps/drivers/px4io/px4io.cpp | 10 +- apps/px4io/comms.c | 119 ++++++++++++++++++--- apps/px4io/dsm.c | 184 ++++++++++++++++++++++++++++++++ apps/px4io/mixer.c | 199 +++-------------------------------- apps/px4io/protocol.h | 9 +- apps/px4io/px4io.c | 2 +- apps/px4io/px4io.h | 9 +- apps/px4io/sbus.c | 58 ++++++++++ 8 files changed, 377 insertions(+), 213 deletions(-) create mode 100644 apps/px4io/dsm.c create mode 100644 apps/px4io/sbus.c diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 0ec1b9922f..cd8783673c 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -743,15 +743,15 @@ px4io_main(int argc, char *argv[]) return ret; } - if (!strcmp(argv[1], "rx_spektrum6")) { + if (!strcmp(argv[1], "rx_dsm_10bit")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); + g_dev->set_rx_mode(RX_MODE_DSM_10BIT); } - if (!strcmp(argv[1], "rx_spektrum7")) { + if (!strcmp(argv[1], "rx_dsm_11bit")) { if (g_dev == nullptr) errx(1, "not started"); - g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); + g_dev->set_rx_mode(RX_MODE_DSM_11BIT); } if (!strcmp(argv[1], "rx_sbus")) { if (g_dev == nullptr) @@ -763,5 +763,5 @@ px4io_main(int argc, char *argv[]) test(); - errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'"); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 0a0453b550..f1dac433fb 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -60,26 +61,126 @@ static int fmu_fd; static hx_stream_t stream; - +static int rx_fd; static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); +static struct pollfd pollfds[2]; +static int pollcount; + void comms_init(void) { + /* initialise the FMU interface */ fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); if (fmu_fd < 0) lib_lowprintf("COMMS: fmu open failed %d\n", errno); - stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + pollfds[0].fd = fmu_fd; + pollfds[0].events = POLLIN; + pollcount = 1; + /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; + +} + +static void +serial_rx_init(unsigned serial_mode) +{ + if (serial_mode == system_state.serial_rx_mode) + return; + system_state.serial_rx_mode = serial_mode; + + if (serial_mode == RX_MODE_PPM_ONLY) { + if (rx_fd != -1) { + pollcount = 1; + close(rx_fd); + rx_fd = -1; + } + return; + } + + /* open the serial port used for DSM and S.bus communication */ + rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + pollfds[1].fd = rx_fd; + pollfds[1].events = POLLIN; + pollcount = 2; + + struct termios t; + tcgetattr(rx_fd, &t); + + switch (serial_mode) { + case RX_MODE_DSM_10BIT: + case RX_MODE_DSM_11BIT: + + /* 115200, no parity, one stop bit */ + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + + dsm_init(serial_mode); + break; + + case RX_MODE_FUTABA_SBUS: + /* 100000, even parity, two stop bits */ + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + + sbus_init(serial_mode); + break; + + default: + break; + } + + tcsetattr(rx_fd, TCSANOW, &t); } void comms_check(void) { + /* + * Check for serial data + */ + int ret = poll(pollfds, pollcount, 0); + + if (ret > 0) { + /* + * Pull bytes from FMU and feed them to the HX engine. + * Limit the number of bytes we actually process on any one iteration. + */ + if (pollfds[0].revents & POLLIN) { + char buf[32]; + ssize_t count = read(fmu_fd, buf, sizeof(buf)); + for (int i = 0; i < count; i++) + hx_stream_rx(stream, buf[i]); + } + + /* + * Pull bytes from the serial RX port and feed them to the decoder + * if we care about serial RX data. + */ + if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { + switch (system_state.serial_rx_mode) { + case RX_MODE_DSM_10BIT: + case RX_MODE_DSM_11BIT: + dsm_input(rx_fd); + break; + + case RX_MODE_FUTABA_SBUS: + sbus_input(rx_fd); + break; + + default: + break; + } + } + } + + /* + * Decide if it's time to send an update to the FMU. + */ static hrt_abstime last_report_time; hrt_abstime now, delta; @@ -87,7 +188,7 @@ comms_check(void) now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && - (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { + (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; @@ -101,15 +202,6 @@ comms_check(void) /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } - - /* - * Check for bytes and feed them to the RX engine. - * Limit the number of bytes we actually process on any one iteration. - */ - char buf[32]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); } int frame_rx; @@ -127,8 +219,7 @@ comms_handle_config(const void *buffer, size_t length) frame_rx++; - mixer_set_serial_mode(cfg->serial_rx_mode); - + serial_rx_init(cfg->serial_rx_mode); } static void diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c new file mode 100644 index 0000000000..79b6301c7d --- /dev/null +++ b/apps/px4io/dsm.c @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file dsm.c + * + * Serial protocol decoder for the Spektrum DSM* family of protocols. + * + * Decodes into the global PPM buffer and updates accordingly. + */ + +#include + +#include +#include +#include + +#include + +#include + +#include "px4io.h" +#include "protocol.h" + +#define DSM_FRAME_SIZE 16 +#define DSM_FRAME_CHANNELS 7 + +static hrt_abstime last_frame_time; + +static uint8_t frame[DSM_FRAME_SIZE]; + +static unsigned partial_frame_count; +static bool insync; +static unsigned channel_shift; + +static void dsm_decode(void); + +void +dsm_init(unsigned mode) +{ + insync = false; + partial_frame_count = 0; + + if (mode == RX_MODE_DSM_10BIT) { + channel_shift = 10; + } else { + channel_shift = 11; + } + + last_frame_time = hrt_absolute_time(); +} + +void +dsm_input(int fd) +{ + uint8_t buf[DSM_FRAME_SIZE]; + ssize_t ret; + hrt_abstime now; + + /* + * The DSM* protocol doesn't provide any explicit framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 11ms; with 16 bytes at 115200bps + * frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a frame. + */ + now = hrt_absolute_time(); + if ((now - last_frame_time) > 5000) + partial_frame_count = 0; + + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return; + + /* + * Add bytes to the current frame + */ + memcpy(&frame[partial_frame_count], buf, ret); + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < DSM_FRAME_SIZE) + return; + last_frame_time = now; + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + dsm_decode(); + partial_frame_count = 0; +} + +static void +dsm_decode(void) +{ + uint16_t data_mask = (1 << channel_shift) - 1; + + /* + * The encoding of the first byte is uncertain, so we're going + * to ignore it for now. + * + * The second byte may tell us about the protocol, but it's not + * actually very interesting since what we really want to know + * is how the channel data is formatted, and there doesn't seem + * to be a reliable way to determine this from the protocol ID + * alone. + * + * Each channel is a 16-bit unsigned value containing either a 10- + * or 11-bit channel value and a 4-bit channel number, shifted + * either 10 or 11 bits. The MSB may also be set to indicate the + * second frame in variants of the protocol where more than + * seven channels are being transmitted. + */ + + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + + uint8_t *dp = &frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + + /* ignore pad channels */ + if (raw == 0xffff) + continue; + + unsigned channel = (raw >> channel_shift) & 0xf; + + /* ignore channels out of range */ + if (channel >= PX4IO_INPUT_CHANNELS) + continue; + + if (channel > ppm_decoded_channels) + ppm_decoded_channels = channel; + + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ + unsigned data = raw & data_mask; + if (channel_shift == 11) + data /= 2; + ppm_buffer[channel] = 988 + data; + + } + ppm_last_valid_decode = hrt_absolute_time(); +} diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 28edb2357e..fa4b3eb252 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Control channel input/output mixer and failsafe. + * @file mixer.c + * + * Control channel input/output mixer and failsafe. */ #include @@ -43,8 +45,6 @@ #include #include #include -#include -#include #include #include @@ -55,23 +55,12 @@ #include "px4io.h" -/* - * Count of periodic calls in which we have no data. - */ -static unsigned mixer_input_drops; -#define MIXER_INPUT_DROP_LIMIT 10 - /* * Count of periodic calls in which we have no FMU input. */ 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. */ @@ -106,8 +95,7 @@ struct mixer { int 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); @@ -115,37 +103,6 @@ mixer_init(void) 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) @@ -234,148 +191,20 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) } } -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]; - - /* - * 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: - * - * 0: Aileron - * 1: Flaps - * 2: Elevator - * 3: Rudder - * 4: Throttle - * 5: Gear - * 6: - * - * 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 (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; - - case RX_MODE_FUTABA_SBUS: - got_input = mixer_get_sbus_input(); - break; - - default: - break; - } - - if (got_input) { - mixer_input_drops = 0; + /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ + if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { + system_state.rc_channels = 0; system_state.fmu_report_due = true; - } else { - /* - * No data; count the 'frame drops' and once we hit the limit - * assume that we have lost input. - */ - if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) { - mixer_input_drops++; - - /* if we hit the limit, stop pretending we have input and let the FMU know */ - if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; - } - } + return; } + + /* otherwise, 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]; + system_state.fmu_report_due = true; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 7467f1adcb..660eed12b6 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,11 +41,6 @@ #pragma once -/* - * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL - * TREES ARE MERGED. - */ - #define PX4IO_OUTPUT_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 @@ -69,8 +64,8 @@ struct px4io_config { 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_DSM_10BIT 1 +#define RX_MODE_DSM_11BIT 2 #define RX_MODE_FUTABA_SBUS 3 }; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 7240a80efd..dec2cdde67 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -72,7 +72,7 @@ int user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); - /* init the FMU link */ + /* init the FMU and receiver links */ comms_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 8eb0c53b21..f50e29252a 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -147,7 +147,6 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern int mixer_init(void); -extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. @@ -160,6 +159,14 @@ extern void safety_init(void); extern void comms_init(void); extern void comms_check(void); +/* + * Serial receiver decoders. + */ +extern void dsm_init(unsigned mode); +extern void dsm_input(int fd); +extern void sbus_init(unsigned mode); +extern void sbus_input(int fd); + /* * Assertion codes */ diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c new file mode 100644 index 0000000000..e363a0a78f --- /dev/null +++ b/apps/px4io/sbus.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file sbus.c + * + * Serial protocol decoder for the Futaba S.bus protocol. + */ + +#include + +#include +#include + +#include + +#include "px4io.h" +#include "protocol.h" + +void +sbus_init(unsigned mode) +{ +} + +void +sbus_input(int fd) +{ +} From 0b5da8b5998878064ed67be7992b7c413c443ee5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 Nov 2012 16:24:12 -0800 Subject: [PATCH 10/12] Got rid of the control limitation at high throttle --- apps/systemlib/mixer/mixer_multirotor.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index a3577245da..52d79e3693 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -161,20 +161,21 @@ MultirotorMixer::mix(float *outputs, unsigned space) float max = 0.0f; float fixup_scale; + /* use an output factor to prevent too strong control signals at low throttle */ float min_thrust = 0.05f; float max_thrust = 1.0f; - float output_factor = 0.0f; float startpoint_full_control = 0.20f; - float endpoint_full_control = 0.80f; + float output_factor; + /* keep roll, pitch and yaw control to 0 below min thrust */ if (thrust <= min_thrust) { output_factor = 0.0f; + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ } else if (thrust < startpoint_full_control && thrust > min_thrust) { - output_factor = (thrust/max_thrust)/startpoint_full_control; - } else if (thrust >= startpoint_full_control && thrust < endpoint_full_control) { + output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust); + /* and then stay at full control */ + } else { output_factor = max_thrust; - } else if (thrust >= endpoint_full_control) { - output_factor = max_thrust/(endpoint_full_control-max_thrust); } roll *= output_factor; @@ -182,7 +183,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) yaw *= output_factor; - /* perform initial mix pass yielding un-bounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { float tmp = roll * _rotors[i].roll_scale + From 6c3a340d6eb3c8cc450e27899309ee79dc1ee504 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Nov 2012 09:25:42 -0800 Subject: [PATCH 11/12] Made param name shorter, don't know if it was needed --- apps/multirotor_att_control/multirotor_att_control_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 951840ab13..6a83877416 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -73,7 +73,7 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THROT, 0.0f); // This defines the throttle when the RC signal is lost. +PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. __EXPORT int multirotor_att_control_main(int argc, char *argv[]); @@ -146,7 +146,7 @@ mc_thread_main(int argc, char *argv[]) bool man_yaw_zero_once = false; /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THROT"); + param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; while (!thread_should_exit) { From 52143d6a687ee5238a27d9afcba0a8d9703bac21 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Nov 2012 09:26:37 -0800 Subject: [PATCH 12/12] Reverting commenting, that happened by accident --- apps/px4io/mixer.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index fa4b3eb252..6cf499c391 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -139,10 +139,10 @@ mixer_tick(void *arg) system_state.fmu_data_received = false; } -// } else if (system_state.rc_channels > 0) { -// /* we have control data from an R/C input */ -// control_count = system_state.rc_channels; -// control_values = &system_state.rc_channel_data[0]; + } else if (system_state.rc_channels > 0) { + /* we have control data from an R/C input */ + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; } else { /* we have no control input */