Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 09:36:19 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[nps] datalink via ivy handling

Browse Source
This commit is contained in:
Felix Ruess
2013-08-08 15:09:36 +02:00
parent 1827ed12a9
commit 29bdf40d9a
7 changed files with 85 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
+1
View File
@@ -56,6 +56,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_radio_control_joystick.c \
$(NPSDIR)/nps_radio_control_spektrum.c \
$(NPSDIR)/nps_autopilot_fixedwing.c \
$(NPSDIR)/nps_ivy_common.c \
$(NPSDIR)/nps_ivy_fixedwing.c \
$(NPSDIR)/nps_flightgear.c \
conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+7 -6
View File
@@ -38,7 +38,7 @@ else
endif
nps.srcs += $(NPSDIR)/nps_main.c \
nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_fdm_jsbsim.c \
$(NPSDIR)/nps_random.c \
$(NPSDIR)/nps_sensors.c \
@@ -51,17 +51,18 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_radio_control.c \
$(NPSDIR)/nps_radio_control_joystick.c \
$(NPSDIR)/nps_radio_control_spektrum.c \
$(NPSDIR)/nps_autopilot_rotorcraft.c \
$(NPSDIR)/nps_ivy.c \
$(NPSDIR)/nps_autopilot_rotorcraft.c \
$(NPSDIR)/nps_ivy_common.c \
$(NPSDIR)/nps_ivy_rotorcraft.c \
$(NPSDIR)/nps_flightgear.c \
nps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT
nps.srcs += firmwares/rotorcraft/main.c
nps.srcs += mcu.c
nps.srcs += $(SRC_ARCH)/mcu_arch.c
nps.srcs += firmwares/rotorcraft/main.c
nps.srcs += mcu.c
nps.srcs += $(SRC_ARCH)/mcu_arch.c
nps.srcs += mcu_periph/i2c.c
nps.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
sw/simulator/nps/nps_autopilot_fixedwing.c
+2 -1
View File
@@ -85,11 +85,12 @@ void nps_autopilot_run_systime_step( void ) {
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
#ifdef RADIO_CONTROL_TYPE_PPM
if (nps_radio_control_available(time)) {
radio_control_feed();
Fbw(event_task);
Ap(event_task);
}
#endif
if (nps_sensors_gyro_available()) {
imu_feed_gyro_accel();
sw/simulator/nps/nps_ivy.h
+1
View File
@@ -1,6 +1,7 @@
#ifndef NPS_IVY
#define NPS_IVY
extern void nps_ivy_common_init(char* ivy_bus);
extern void nps_ivy_init(char* ivy_bus);
extern void nps_ivy_display(void);
sw/simulator/nps/nps_ivy.c → sw/simulator/nps/nps_ivy_common.c
+1 -26
View File
@@ -10,7 +10,6 @@
#include "nps_fdm.h"
#include "nps_sensors.h"
#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
#ifdef RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h"
@@ -36,10 +35,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
@@ -50,7 +45,7 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
#endif
void nps_ivy_init(char* ivy_bus) {
void nps_ivy_common_init(char* ivy_bus) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
@@ -58,7 +53,6 @@ void nps_ivy_init(char* ivy_bus) {
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) GET_DL_SETTING (\\S*) (\\S*)");
IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
#ifdef RADIO_CONTROL_TYPE_DATALINK
IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)");
@@ -117,25 +111,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
printf("goto block %d\n", block);
}
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
uint8_t wp_id = atoi(argv[1]);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
}
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
sw/simulator/nps/nps_ivy_fixedwing.c
+15 -136
View File
@@ -2,114 +2,45 @@
#include <stdlib.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include "generated/airframe.h"
#include "math/pprz_algebra_double.h"
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
#include "subsystems/ins.h"
#include "subsystems/navigation/common_nav.h"
#include NPS_SENSORS_PARAMS
/* Datalink Ivy functions */
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
/* fixedwing specific Datalink Ivy functions */
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
void nps_ivy_init(char* ivy_bus) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
IvyBindMsg(on_DL_PING, NULL, "^(\\S*) DL_PING");
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) GET_DL_SETTING (\\S*) (\\S*)");
IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
/* init ivy and bind some messages common to fw and rotorcraft */
nps_ivy_common_init(ivy_bus);
#ifdef __APPLE__
const char* default_ivy_bus = "224.255.255.255";
#else
const char* default_ivy_bus = "127.255.255.255";
#endif
if (ivy_bus == NULL) {
IvyStart(default_ivy_bus);
} else {
IvyStart(ivy_bus);
}
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
}
//TODO use datalink parsing from booz or fw instead of doing it here explicitly
//FIXME currently parsed correctly for booz only
//TODO use datalink parsing from fixedwing instead of doing it here explicitly
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
uint8_t index = atoi(argv[2]);
float value = atof(argv[3]);
DlSetting(index, value);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
printf("setting %d %f\n", index, value);
}
static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
uint8_t index = atoi(argv[2]);
float value = settings_get_value(index);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
printf("get setting %d %f\n", index, value);
}
#define MOfCm(_x) (((float)(_x))/100.)
static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[] __attribute__ ((unused))) {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
int block = atoi(argv[1]);
nav_goto_block(block);
printf("goto block %d\n", block);
}
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
float a = atoi(argv[5]) * 0.1;
float a = MOfCm(atoi(argv[5]));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
lla.lat = RadOfDeg((float)(atoi(argv[3]) / 1e7));
lla.lon = RadOfDeg((float)(atoi(argv[4]) / 1e7));
//printf("move wp id=%d lat=%f lon=%f alt=%f\n", wp_id, lla.lat, lla.lon, a);
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
@@ -120,58 +51,6 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
utm.east = waypoints[wp_id].x + nav_utm_east0;
utm.north = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
printf("move wp id=%d east=%f north=%f alt=%f\n", wp_id, utm.east, utm.north, a);
printf("move wp id=%d east=%f north=%f zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a);
}
}
void nps_ivy_display(void) {
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm.body_ecef_rotvel.p),
DegOfRad(fdm.body_ecef_rotvel.q),
DegOfRad(fdm.body_ecef_rotvel.r),
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.psi));
IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.lla_pos_pprz.lat),
(fdm.lla_pos_geod.lat),
(fdm.lla_pos_geoc.lat),
(fdm.lla_pos_pprz.lon),
(fdm.lla_pos_geod.lon),
(fdm.lla_pos_pprz.alt),
(fdm.lla_pos_geod.alt),
(fdm.agl),
(fdm.hmsl));
IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.ltpprz_ecef_accel.x),
(fdm.ltpprz_ecef_accel.y),
(fdm.ltpprz_ecef_accel.z),
(fdm.ltpprz_ecef_vel.x),
(fdm.ltpprz_ecef_vel.y),
(fdm.ltpprz_ecef_vel.z),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.z));
IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
AC_ID,
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z));
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h);
IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
AC_ID,
((sensors.accel.value.x - sensors.accel.neutral.x)/NPS_ACCEL_SENSITIVITY_XX),
((sensors.accel.value.y - sensors.accel.neutral.y)/NPS_ACCEL_SENSITIVITY_YY),
((sensors.accel.value.z - sensors.accel.neutral.z)/NPS_ACCEL_SENSITIVITY_ZZ),
h_body.x,
h_body.y,
h_body.z);
}
sw/simulator/nps/nps_ivy_rotorcraft.c
+58
View File
@@ -0,0 +1,58 @@
#include "nps_ivy.h"
#include <stdlib.h>
#include <Ivy/ivy.h>
#include "generated/airframe.h"
#include "math/pprz_algebra_double.h"
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
#ifdef RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h"
#endif
#include NPS_SENSORS_PARAMS
/* rotorcraft specificDatalink Ivy functions */
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
void nps_ivy_init(char* ivy_bus) {
/* init ivy and bind some messages common to fw and rotorcraft */
nps_ivy_common_init(ivy_bus);
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
}
//TODO use datalink parsing from rotorcraft instead of doing it here explicitly
#include "generated/settings.h"
#include "dl_protocol.h"
#include "subsystems/datalink/downlink.h"
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
}
}