mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
added telemetry and datalink to test passthrough
This commit is contained in:
Antoine Drouin
@@ -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,7 +179,7 @@
|
||||
|
||||
</section>
|
||||
|
||||
|
||||
-->
|
||||
|
||||
|
||||
<section name="BAT">
|
||||
@@ -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 @@ ARCHI=stm32
|
||||
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
|
||||
|
||||
@@ -66,5 +66,6 @@ pt.CFLAGS += -DRADIO_CONTROL_LINK=Uart3
|
||||
|
||||
# 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
|
||||
|
||||
@@ -42,13 +42,6 @@ overo_test_spi_link.srcs += $(SRC_FMS)/fms_spi_link.c
|
||||
|
||||
|
||||
|
||||
# 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,6 +87,26 @@ overo_test_periodic.LDFLAGS += -levent
|
||||
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
|
||||
|
||||
|
||||
|
||||
################################################################################
|
||||
#
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
@@ -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]);
|
||||
}
|
||||
else {
|
||||
us_delay = 1953;
|
||||
}
|
||||
|
||||
printf("Delay: %dus\n", us_delay);
|
||||
|
||||
|
||||
printf("AutopilotMessage size: %d\n", sizeof(union AutopilotMessage));
|
||||
printf("AutopilotMessageCRCFrame size: %d\n", sizeof(struct AutopilotMessageCRCFrame));
|
||||
/* Initalize the event library */
|
||||
event_init();
|
||||
|
||||
while (1) {
|
||||
send_message();
|
||||
usleep(us_delay);
|
||||
/* Initalize our ô so accurate periodic timer */
|
||||
if (fms_periodic_init(main_periodic)) {
|
||||
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Initialize our communications with ground segment */
|
||||
fms_gs_com_init("10.31.4.7", 4242, 4243, FALSE);
|
||||
|
||||
/* Enter our mainloop */
|
||||
event_dispatch();
|
||||
|
||||
printf("leaving... goodbye!\n");
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
static void main_periodic(int my_sig_num) {
|
||||
|
||||
send_message();
|
||||
|
||||
fms_gs_com_periodic();
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -73,23 +98,37 @@ int main(int argc, char *argv[]) {
|
||||
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++;
|
||||
|
||||
@@ -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 */
|
||||
Reference in New Issue
Block a user