Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Add booz_radio_control_joby and enable it for lisa_stm_passthrough Joby RC over uart with 9channel transmitter

Browse Source
This commit is contained in:
Allen Ibara
2010-07-09 02:05:48 +00:00
parent b51c3ad5f7
commit e925065109
6 changed files with 246 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/lisa_passthrough.makefile
+3 -3
View File
@@ -47,10 +47,10 @@ pt.srcs += $(SRC_BOOZ)/booz2_commands.c
# 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
sw/airborne/booz/radio_control/booz_radio_control_joby.c
+93
View File
@@ -0,0 +1,93 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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;
}
sw/airborne/booz/radio_control/booz_radio_control_joby.h
+75
View File
@@ -0,0 +1,75 @@
/*
* Paparazzi $Id$
*
* Copyright (C) 2009 Pascal Brisset <pascal.brisset@gmail.com>,
* Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/booz/radio_control/booz_radio_control_joby_9ch.h
+57
View File
@@ -0,0 +1,57 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/fms/fms_autopilot_msg.h
+4
View File
@@ -83,6 +83,10 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp
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;
};
sw/airborne/lisa/lisa_stm_passthrough_main.c
+14 -2
View File
@@ -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 @@ static inline void main_init(void) {
sys_time_init();
actuators_init();
booz_imu_init();
radio_control_init();
overo_link_init();
}
@@ -66,13 +68,14 @@ static inline void main_periodic(void) {
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 @@ static inline void main_on_overo_msg_received(void) {
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) {