[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5335] added telemetry and datalink to test passthro
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [5335] added telemetry and datalink to test passthrough |
|
Date: |
Thu, 12 Aug 2010 00:50:32 +0000 |
Revision: 5335
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5335
Author: poine
Date: 2010-08-12 00:50:31 +0000 (Thu, 12 Aug 2010)
Log Message:
-----------
added telemetry and datalink to test passthrough
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/booz2_a8.xml
paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile
paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/fms/fms_gs_com.c
paparazzi3/trunk/sw/airborne/fms/fms_gs_com.h
paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough_telemetry.h
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a8.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a8.xml 2010-08-12 00:49:43 UTC
(rev 5334)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a8.xml 2010-08-12 00:50:31 UTC
(rev 5335)
@@ -89,6 +89,42 @@
-->
+<!-- booz2 -->
+
+ <section name="IMU" prefix="IMU_">
+
+ <define name="GYRO_P_NEUTRAL" value="31948"/>
+ <define name="GYRO_Q_NEUTRAL" value="31834"/>
+ <define name="GYRO_R_NEUTRAL" value="32687"/>
+
+ <define name="GYRO_P_SENS" value=" 1.101357422" integer="16"/>
+ <define name="GYRO_Q_SENS" value=" 1.122670898" integer="16"/>
+ <define name="GYRO_R_SENS" value=" 1.104890137" integer="16"/>
+
+
+ <define name="ACCEL_X_SENS" value=" 2.58273701242" integer="16"/>
+ <define name="ACCEL_Y_SENS" value=" 2.54076215332" integer="16"/>
+ <define name="ACCEL_Z_SENS" value=" 2.57633620646" integer="16"/>
+
+ <define name="ACCEL_X_NEUTRAL" value="32857"/>
+ <define name="ACCEL_Y_NEUTRAL" value="32429"/>
+ <define name="ACCEL_Z_NEUTRAL" value="32593"/>
+
+ <define name="MAG_X_SENS" value=" 5.32718104135" integer="16"/>
+ <define name="MAG_Y_SENS" value=" 4.87857821202" integer="16"/>
+ <define name="MAG_Z_SENS" value=" 3.11986612709" integer="16"/>
+
+ <define name="MAG_X_NEUTRAL" value="-43"/>
+ <define name="MAG_Y_NEUTRAL" value=" 49"/>
+ <define name="MAG_Z_NEUTRAL" value="-66"/>
+
+ <define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0.)"/>
+ <define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0.)"/>
+ <define name="BODY_TO_IMU_PSI" value="RadOfDeg( 45.)"/>
+
+ </section>
+
+<!-- aspirin
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="-1"/>
@@ -143,9 +179,9 @@
</section>
+-->
-
<section name="BAT">
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
@@ -160,7 +196,8 @@
USER =
#HOST = 10.31.4.22
#HOST = overo
-HOST = beth
+HOST = auto7
+#HOST = 192.168.2.230
TARGET_DIR = ~
SRC_FMS=fms
@@ -171,6 +208,8 @@
BOARD_CFG=\"boards/lisa_0.99.h\"
FLASH_MODE = JTAG
+PERIODIC_FREQ = 512
+
include $(PAPARAZZI_SRC)/conf/autopilot/lisa_test_progs.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/lisa_passthrough.makefile
Modified: paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile 2010-08-12
00:49:43 UTC (rev 5334)
+++ paparazzi3/trunk/conf/autopilot/lisa_passthrough.makefile 2010-08-12
00:50:31 UTC (rev 5335)
@@ -66,5 +66,6 @@
# PWM actuator
pt.CFLAGS += -DSERVO_HZ=200
+#pt.CFLAGS += -DSERVO_HZ=50
pt.srcs += $(SRC_BOOZ)/actuators/booz_actuators_pwm.c
pt.srcs += $(SRC_BOOZ_ARCH)/actuators/booz_actuators_pwm_hw.c
Modified: paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile 2010-08-12
00:49:43 UTC (rev 5334)
+++ paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile 2010-08-12
00:50:31 UTC (rev 5335)
@@ -42,13 +42,6 @@
-# test passthrough spi link between overo and stm32
-overo_test_passthrough.ARCHDIR = omap
-overo_test_passthrough.CFLAGS += -I$(ACINCLUDE) -I.
-I$(PAPARAZZI_HOME)/var/include
-overo_test_passthrough.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown
-overo_test_passthrough.srcs = $(SRC_FMS)/overo_test_passthrough.c
-overo_test_passthrough.srcs += $(SRC_FMS)/fms_spi_link.c
-
# test network based telemetry on overo
overo_test_telemetry.ARCHDIR = omap
overo_test_telemetry.CFLAGS += -I$(ACINCLUDE) -I.
-I$(PAPARAZZI_HOME)/var/include
@@ -94,7 +87,27 @@
overo_test_periodic.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
overo_test_periodic.srcs += $(SRC_FMS)/fms_spi_link.c
+# test passthrough , aka using stm32 as io processor
+# this demonstrates
+# -link with io processor
+# -periodic event
+# -telemetry and datalink
+#
+overo_test_passthrough.ARCHDIR = omap
+overo_test_passthrough.LDFLAGS += -levent -lm
+overo_test_passthrough.CFLAGS += -I$(ACINCLUDE) -I.
-I$(PAPARAZZI_HOME)/var/include
+overo_test_passthrough.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown
+overo_test_passthrough.srcs = $(SRC_FMS)/overo_test_passthrough.c
+overo_test_passthrough.CFLAGS += -DFMS_PERIODIC_FREQ=512
+overo_test_passthrough.srcs += $(SRC_FMS)/fms_periodic.c
+overo_test_passthrough.srcs += $(SRC_FMS)/fms_spi_link.c
+overo_test_passthrough.srcs += $(SRC_FMS)/fms_gs_com.c
+overo_test_passthrough.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
+overo_test_passthrough.srcs += $(SRC_FMS)/udp_transport2.c downlink.c
+overo_test_passthrough.srcs += $(SRC_FMS)/fms_network.c
+
+
################################################################################
#
#
Added: paparazzi3/trunk/sw/airborne/fms/fms_gs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_gs_com.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_gs_com.c 2010-08-12 00:50:31 UTC
(rev 5335)
@@ -0,0 +1,83 @@
+#include "fms/fms_gs_com.h"
+
+#include <unistd.h>
+
+#include "udp_transport2.h"
+
+/* generated */
+#include "periodic.h"
+/* holds the definitions of PERIODIC_SEND_XXX */
+#include "overo_test_passthrough_telemetry.h"
+/* holds the definitions of DOWNLINK_SEND_XXX */
+#include "messages2.h"
+#include "dl_protocol.h"
+#include "settings.h"
+
+struct FmsGsCom fms_gs_com;
+uint8_t telemetry_mode_Main_DefaultChannel;
+
+
+static void on_datalink_event(int fd, short event __attribute__((unused)),
void *arg);
+static void on_datalink_message(void);
+
+uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port,
+ uint16_t datalink_port, uint8_t broadcast) {
+
+ fms_gs_com.network = network_new(gs_host, gs_port, datalink_port, broadcast);
+ fms_gs_com.udp_transport = udp_transport_new(fms_gs_com.network);
+ event_set(&fms_gs_com.datalink_event, fms_gs_com.network->socket_in, EV_READ
| EV_PERSIST,
+ on_datalink_event, fms_gs_com.udp_transport);
+ event_add(&fms_gs_com.datalink_event, NULL);
+
+ return 0;
+}
+
+void fms_gs_com_periodic(void) {
+
+ PeriodicSendMain(fms_gs_com.udp_transport);
+
+ RunOnceEvery(10,
{fms_gs_com.udp_transport->Periodic(fms_gs_com.udp_transport->impl);});
+
+}
+
+
+static void on_datalink_event(int fd, short event __attribute__((unused)),
void *arg) {
+ char buf[512];
+ int bytes_read = read(fd, buf, 512);
+ uint16_t i = 0;
+ struct udp_transport *tp = fms_gs_com.udp_transport->impl;
+ while (i<bytes_read) {
+ parse_udp_dl(tp, buf[i]);
+ if (tp->udp_dl_msg_received) {
+ on_datalink_message();
+ tp->udp_dl_msg_received = FALSE;
+ }
+ i++;
+ }
+
+}
+
+static void on_datalink_message(void) {
+ struct udp_transport *tp = fms_gs_com.udp_transport->impl;
+ uint8_t msg_id = tp->udp_dl_payload[1];
+
+ switch (msg_id) {
+ case DL_PING:
+ DOWNLINK_SEND_PONG(fms_gs_com.udp_transport);
+ break;
+ case DL_SETTING : {
+ uint8_t i = DL_SETTING_index(tp->udp_dl_payload);
+ float var = DL_SETTING_value(tp->udp_dl_payload);
+ DlSetting(i, var);
+ printf("datalink : %d %f\n",i,var);
+ DOWNLINK_SEND_DL_VALUE(fms_gs_com.udp_transport, &i, &var);
+ }
+ break;
+
+ default :
+ printf("did nothing\n");
+ break;
+ }
+
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/fms_gs_com.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_gs_com.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_gs_com.h 2010-08-12 00:50:31 UTC
(rev 5335)
@@ -0,0 +1,34 @@
+/*
+ * This module handles communications with ground segment
+ *
+ * for now one channel of telemetry/datalink via wifi
+ *
+ */
+
+#ifndef FMS_GS_COM_H
+#define FMS_GS_COM_H
+
+#include <event.h>
+#include "fms_network.h"
+#include "downlink_transport.h"
+
+#include "std.h"
+
+struct FmsGsCom {
+
+ struct FmsNetwork* network;
+ struct DownlinkTransport *udp_transport;
+ struct event datalink_event;
+
+};
+
+
+extern struct FmsGsCom fms_gs_com;
+/* remove me */
+extern uint8_t telemetry_mode_Main_DefaultChannel;
+
+extern uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port,
+ uint16_t datalink_port, uint8_t broadcast);
+extern void fms_gs_com_periodic(void);
+
+#endif /* FMS_GS_COM_H */
Modified: paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c 2010-08-12
00:49:43 UTC (rev 5334)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c 2010-08-12
00:50:31 UTC (rev 5335)
@@ -28,44 +28,69 @@
#include <stdlib.h>
#include <string.h>
+#include <event.h>
+
+#include "std.h"
#include "fms_debug.h"
+#include "fms_periodic.h"
+
+/* stuff for io processor link */
#include "fms_spi_link.h"
#include "fms_autopilot_msg.h"
-static struct AutopilotMessageCRCFrame msg_in;
-static struct AutopilotMessageCRCFrame msg_out;
+/* stuff for telemetry/datalink */
+#include "fms_gs_com.h"
+
+/* let's store those data somewhere */
+#include "booz/booz_imu.h"
+
+struct BoozImuFloat imu;
+
+static void main_periodic(int my_sig_num);
+
+
+
+
+
static void send_message(void);
static void print_up_msg(struct AutopilotMessageCRCFrame * msg);
static void print_down_msg(struct AutopilotMessageCRCFrame * msg);
int main(int argc, char *argv[]) {
- uint32_t us_delay;
-
+ /* Initalize our SPI link to IO processor */
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return -1;
}
- if(argc > 1) {
- us_delay = atoi(argv[1]);
+ /* Initalize the event library */
+ event_init();
+
+ /* Initalize our \xF4 so accurate periodic timer */
+ if (fms_periodic_init(main_periodic)) {
+ TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+ return -1;
}
- else {
- us_delay = 1953;
- }
+
+ /* Initialize our communications with ground segment */
+ fms_gs_com_init("10.31.4.7", 4242, 4243, FALSE);
- printf("Delay: %dus\n", us_delay);
+ /* Enter our mainloop */
+ event_dispatch();
+
+ printf("leaving... goodbye!\n");
+ return 0;
- printf("AutopilotMessage size: %d\n", sizeof(union
AutopilotMessage));
- printf("AutopilotMessageCRCFrame size: %d\n", sizeof(struct
AutopilotMessageCRCFrame));
+}
+
+static void main_periodic(int my_sig_num) {
+
+ send_message();
- while (1) {
- send_message();
- usleep(us_delay);
- }
-
- return 0;
+ fms_gs_com_periodic();
+
}
@@ -73,23 +98,37 @@
static void send_message() {
static uint32_t foo = 0;
+ struct AutopilotMessageCRCFrame msg_in;
+ struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
-
+
+ uint16_t val = 1500 + 500*sin(foo*0.001);
+ msg_out.payload.msg_down.pwm_outputs_usecs[0] = val;
+ msg_out.payload.msg_down.pwm_outputs_usecs[1] = val;
+ msg_out.payload.msg_down.pwm_outputs_usecs[2] = val;
+
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in,
&crc_valid);
- if (!(foo % 100)) {
- printf("msg %d, CRC errors: %d\n", spi_link.msg_cnt, spi_link.crc_err_cnt);
+
+ struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up;
+ RATES_FLOAT_OF_BFP(imu.gyro, in->gyro);
+ ACCELS_FLOAT_OF_BFP(imu.accel, in->accel);
+
+ if (!(foo % 200)) {
+ // printf("msg %d, CRC errors: %d\n", spi_link.msg_cnt,
spi_link.crc_err_cnt);
// print_up_msg(&msg_in);
// print_down_msg(&msg_out);
- printf("0x%08x -> gx%+02f gy%+02f gz%+02f ax%+02f ay%+02f az%+02f rs%02x |
CRC errors: %d \n",
+ printf("%08d -> gx%+02.1f gy%+02.1f gz%+02.1f ax%+02.1f ay%+02.1f
az%+02.1f rs%02x stm_msg %08d stm_err %d | CRC errors: %d\n",
foo,
- DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.p)),
- DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.q)),
- DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.r)),
- ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.x),
- ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.y),
- ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.z),
- msg_in.payload.msg_up.rc_status,
+ DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.p)),
+ DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.q)),
+ DegOfRad(RATE_FLOAT_OF_BFP(in->gyro.r)),
+ ACCEL_FLOAT_OF_BFP(in->accel.x),
+ ACCEL_FLOAT_OF_BFP(in->accel.y),
+ ACCEL_FLOAT_OF_BFP(in->accel.z),
+ in->rc_status,
+ in->stm_msg_cnt,
+ in->stm_crc_err_cnt,
spi_link.crc_err_cnt);
}
foo++;
Added: paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough_telemetry.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough_telemetry.h
2010-08-12 00:50:31 UTC (rev 5335)
@@ -0,0 +1,13 @@
+#ifndef OVERO_TEST_PASSTHROUGH_TELEMETRY_H
+#define OVERO_TEST_PASSTHROUGH_TELEMETRY_H
+
+#define PERIODIC_SEND_ALIVE(_transport) DOWNLINK_SEND_ALIVE(_transport, 16,
MD5SUM)
+
+#include "booz/booz_imu.h"
+extern struct BoozImuFloat imu;
+#define PERIODIC_SEND_IMU_GYRO(_transport) \
+ DOWNLINK_SEND_IMU_GYRO(_transport, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r)
+
+
+
+#endif /* OVERO_TEST_PASSTHROUGH_TELEMETRY_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5335] added telemetry and datalink to test passthrough,
antoine drouin <=