[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5031] Add booz_radio_control_joby and enable it for
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [5031] Add booz_radio_control_joby and enable it for lisa_stm_passthrough Joby RC over uart with 9channel transmitter |
|
Date: |
Fri, 09 Jul 2010 02:05:48 +0000 |
Revision: 5031
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5031
Author: aibara
Date: 2010-07-09 02:05:48 +0000 (Fri, 09 Jul 2010)
Log Message:
-----------
Add booz_radio_control_joby and enable it for lisa_stm_passthrough Joby RC over
uart with 9channel transmitter
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile
paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.c
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.h
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby_9ch.h
Modified: paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile 2010-07-08
19:02:31 UTC (rev 5030)
+++ paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile 2010-07-09
02:05:48 UTC (rev 5031)
@@ -47,10 +47,10 @@
# Radio control
pt.CFLAGS += -DUSE_RADIO_CONTROL
-pt.CFLAGS +=
-DRADIO_CONTROL_TYPE_H=\"radio_control/booz_radio_control_spektrum.h\"
-pt.CFLAGS +=
-DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"radio_control/booz_radio_control_spektrum_dx7se.h\"
+pt.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/booz_radio_control_joby.h\"
+pt.CFLAGS +=
-DRADIO_CONTROL_JOBY_MODEL_H=\"radio_control/booz_radio_control_joby_9ch.h\"
pt.srcs += $(SRC_BOOZ)/booz_radio_control.c \
- $(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c
+ $(SRC_BOOZ)/radio_control/booz_radio_control_joby.c
pt.CFLAGS += -DRADIO_CONTROL_LED=6
pt.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200
pt.CFLAGS += -DRADIO_CONTROL_LINK=Uart3
Added: paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.c
2010-07-09 02:05:48 UTC (rev 5031)
@@ -0,0 +1,93 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "stdio.h"
+#include "booz_radio_control.h"
+
+static struct rc_joby_parser_state parser;
+
+static void handle_channel(void (* callback)(void))
+{
+ if (parser.parser_normal_buf == RC_JOBY_MAGIC_START) {
+ // got start channel, look for channel 0 next
+ parser.current_channel = 0;
+ } else if (parser.current_channel == -1) {
+ // looking for start channel byte but didn't get it, reset
+ parser.current_byte = READING_HIGH_BYTE;
+ parser.current_inverted = READING_NORMAL;
+ } else {
+ // valid channel, store and look for next
+ radio_control.values[parser.current_channel] = parser.parser_normal_buf;
+ parser.current_channel++;
+ if (parser.current_channel == RADIO_CONTROL_NB_CHANNEL) {
+ // all channels read, reset parser and handle message
+ parser.current_channel = -1;
+ radio_control.frame_cpt++;
+ radio_control.status = RADIO_CONTROL_OK;
+ radio_control.time_since_last_frame = 0;
+ if (callback != NULL)
+ callback();
+ }
+ }
+}
+
+static void handle_tuple(void (* callback)(void))
+{
+ if (parser.current_inverted == READING_NORMAL) {
+ parser.parser_normal_buf = ((parser.high_byte_buf << 8) |
parser.low_byte_buf);
+ parser.current_inverted = READING_INVERTED;
+ } else if (parser.current_inverted == READING_INVERTED) {
+ parser.parser_inverted_buf = ((parser.high_byte_buf << 8) |
parser.low_byte_buf);
+ parser.current_inverted = READING_NORMAL;
+ if (parser.parser_normal_buf == ~parser.parser_inverted_buf) {
+ handle_channel(callback);
+ } else {
+ // normal didn't match inverted, error, reset
+ parser.current_inverted = READING_NORMAL;
+ parser.current_byte = READING_HIGH_BYTE;
+ parser.current_channel = -1;
+ parser.error_counter++;
+ }
+ }
+}
+
+void rc_joby_parse(int8_t c, void (* callback)(void))
+{
+ if (parser.current_byte == READING_HIGH_BYTE) {
+ parser.high_byte_buf = c;
+ if (parser.current_channel >= 0 || parser.high_byte_buf ==
(RC_JOBY_MAGIC_START >> 8) || parser.current_inverted == READING_INVERTED) {
+ // only advance parser state to low byte if we're not looking for a
sync byte which we didn't find
+ parser.current_byte = READING_LOW_BYTE;
+ }
+ } else { // READING_LOW_BYTE
+ parser.low_byte_buf = c;
+ parser.current_byte = READING_HIGH_BYTE;
+ handle_tuple(callback);
+ }
+}
+
+void radio_control_impl_init(void) {
+ parser.current_byte = READING_HIGH_BYTE;
+ parser.current_inverted = READING_NORMAL;
+ parser.current_channel = -1;
+}
Added: paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby.h
2010-07-09 02:05:48 UTC (rev 5031)
@@ -0,0 +1,75 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2009 Pascal Brisset <address@hidden>,
+ * Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BOOZ_RADIO_CONTROL_JOBY_H
+#define BOOZ_RADIO_CONTROL_JOBY_H
+
+#include "std.h"
+#include "uart.h"
+
+#define RC_JOBY_MAGIC_START 13999
+
+#include RADIO_CONTROL_JOBY_MODEL_H
+
+typedef enum {
+ READING_LOW_BYTE = 0,
+ READING_HIGH_BYTE
+} parser_byte_t;
+
+typedef enum {
+ READING_NORMAL = 0,
+ READING_INVERTED
+} parser_inverted_t;
+
+struct rc_joby_parser_state
+{
+ parser_byte_t current_byte;
+ parser_inverted_t current_inverted;
+ int current_channel;
+
+ int16_t parser_inverted_buf;
+ int16_t parser_normal_buf;
+ uint8_t high_byte_buf;
+ uint8_t low_byte_buf;
+
+ uint32_t error_counter;
+};
+
+void rc_joby_parse(int8_t c, void (* callback)(void));
+
+#define __RcLink(dev, _x) dev##_x
+#define _RcLink(dev, _x) __RcLink(dev, _x)
+#define RcLink(_x) _RcLink(RADIO_CONTROL_LINK, _x)
+
+#define RcLinkChAvailable() RcLink(ChAvailable())
+#define RcLinkGetCh() RcLink(Getch())
+
+#define RadioControlEvent(_received_frame_handler) { \
+ while (RcLinkChAvailable()) { \
+ rc_joby_parse(RcLinkGetCh(), _received_frame_handler); \
+ } \
+ }
+
+
+#endif /* BOOZ_RADIO_CONTROL_JOBY_H */
Added:
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby_9ch.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby_9ch.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_joby_9ch.h
2010-07-09 02:05:48 UTC (rev 5031)
@@ -0,0 +1,57 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BOOZ_RADIO_CONTROL_JOBY_9CH_H
+#define BOOZ_RADIO_CONTROL_JOBY_9CH_H
+
+#define RADIO_CONTROL_NB_CHANNEL 9
+
+#define RADIO_CONTROL_THROTTLE 0
+#define RADIO_CONTROL_YAW 1
+#define RADIO_CONTROL_PITCH 2
+#define RADIO_CONTROL_ROLL 3
+#define RADIO_CONTROL_GEAR 4
+#define RADIO_CONTROL_MODE 5
+#define RADIO_CONTROL_KILL 6
+#define RADIO_CONTROL_AUX3 7
+#define RADIO_CONTROL_AUX4 8
+
+#define RC_JOBY_SYNC_2 0x12
+
+#define RC_JOBY_THROWS { MAX_PPRZ/MAX_SPK, \
+ MAX_PPRZ/MAX_SPK, \
+ -MAX_PPRZ/MAX_SPK, \
+ MAX_PPRZ/MAX_SPK, \
+ MAX_PPRZ/MAX_SPK, \
+ -MAX_PPRZ/MAX_SPK, \
+ MAX_PPRZ/MAX_SPK }
+
+/*
+ aileron 1
+ elevator 2
+ rudder 3
+ gear 4
+ throttle 5
+*/
+
+#endif /* BOOZ_RADIO_CONTROL_JOBY_9CH_H */
Modified: paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h 2010-07-08
19:02:31 UTC (rev 5030)
+++ paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h 2010-07-09
02:05:48 UTC (rev 5031)
@@ -83,6 +83,10 @@
int16_t rc_yaw;
int16_t rc_thrust;
int16_t rc_mode;
+ int16_t rc_kill;
+ int16_t rc_gear;
+ int16_t rc_aux3;
+ int16_t rc_aux4;
uint8_t rc_status;
};
Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
2010-07-08 19:02:31 UTC (rev 5030)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
2010-07-09 02:05:48 UTC (rev 5031)
@@ -27,6 +27,7 @@
#include "booz/booz2_commands.h"
#include "booz/booz_actuators.h"
#include "booz/booz_imu.h"
+#include "booz_radio_control.h"
#include "lisa/lisa_overo_link.h"
static inline void main_init(void);
@@ -58,6 +59,7 @@
sys_time_init();
actuators_init();
booz_imu_init();
+ radio_control_init();
overo_link_init();
}
@@ -66,13 +68,14 @@
booz_imu_periodic();
actuators_set(FALSE);
OveroLinkPeriodic(main_on_overo_link_lost);
- RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel,
16, MD5SUM);});
+ RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel,
16, MD5SUM);radio_control_periodic();});
}
static inline void main_event(void) {
BoozImuEvent(on_gyro_accel_event, on_mag_event);
OveroLinkEvent(main_on_overo_msg_received);
+ RadioControlEvent(NULL);
}
static inline void main_on_overo_msg_received(void) {
@@ -90,7 +93,16 @@
msg_out->mag.y = booz_imu.mag.y;
msg_out->mag.z = booz_imu.mag.z;
- msg_out->rc_status++;
+ msg_out->rc_pitch = radio_control.values[RADIO_CONTROL_PITCH];
+ msg_out->rc_roll = radio_control.values[RADIO_CONTROL_ROLL];
+ msg_out->rc_yaw = radio_control.values[RADIO_CONTROL_YAW];
+ msg_out->rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE];
+ msg_out->rc_mode = radio_control.values[RADIO_CONTROL_MODE];
+ msg_out->rc_kill = radio_control.values[RADIO_CONTROL_KILL];
+ msg_out->rc_gear = radio_control.values[RADIO_CONTROL_GEAR];
+ msg_out->rc_aux3 = radio_control.values[RADIO_CONTROL_AUX3];
+ msg_out->rc_aux4 = radio_control.values[RADIO_CONTROL_AUX4];
+ msg_out->rc_status = radio_control.status;
}
static inline void main_on_overo_link_lost(void) {
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5031] Add booz_radio_control_joby and enable it for lisa_stm_passthrough Joby RC over uart with 9channel transmitter,
Allen Ibara <=