mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 03:57:45 +08:00
[nps] use UDP instead of IVY telemetry
- get rid off all the datalink parsing "hacks" and use the actual parser from the firmware - this however means that you need to run link with the -udp option to use the NPS sim
This commit is contained in:
Felix Ruess
@@ -46,13 +46,10 @@ 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_ivy.c \
|
||||
$(NPSDIR)/nps_flightgear.c \
|
||||
|
||||
|
||||
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
|
||||
nps.srcs += subsystems/datalink/ivy_transport.c
|
||||
nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c
|
||||
include $(CFG_SHARED)/telemetry_transparent_udp.makefile
|
||||
nps.srcs += $(SRC_FIRMWARE)/datalink.c
|
||||
nps.srcs += $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c
|
||||
|
||||
@@ -61,14 +61,11 @@ 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_ivy.c \
|
||||
$(NPSDIR)/nps_flightgear.c \
|
||||
|
||||
nps.srcs += math/pprz_geodetic_wmm2015.c
|
||||
|
||||
nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
|
||||
nps.srcs += subsystems/datalink/ivy_transport.c
|
||||
nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c
|
||||
include $(CFG_SHARED)/telemetry_transparent_udp.makefile
|
||||
nps.srcs += $(SRC_FIRMWARE)/datalink.c
|
||||
nps.srcs += $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c
|
||||
|
||||
@@ -57,14 +57,12 @@ nps.srcs += $(NPSDIR)/nps_main.c \
|
||||
$(NPSDIR)/nps_radio_control_joystick.c \
|
||||
$(NPSDIR)/nps_radio_control_spektrum.c \
|
||||
$(NPSDIR)/nps_autopilot_rotorcraft.c \
|
||||
$(NPSDIR)/nps_ivy_common.c \
|
||||
$(NPSDIR)/nps_ivy_rotorcraft.c \
|
||||
$(NPSDIR)/nps_flightgear.c \
|
||||
$(NPSDIR)/nps_ivy_mission_commands.c
|
||||
$(NPSDIR)/nps_ivy.c \
|
||||
$(NPSDIR)/nps_flightgear.c
|
||||
|
||||
# for geo mag calculation
|
||||
nps.srcs += math/pprz_geodetic_wmm2015.c
|
||||
|
||||
include $(CFG_SHARED)/telemetry_ivy.makefile
|
||||
include $(CFG_SHARED)/telemetry_transparent_udp.makefile
|
||||
nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c
|
||||
nps.srcs += $(SRC_FIRMWARE)/datalink.c
|
||||
|
||||
@@ -92,7 +92,7 @@ void downlink_init(void)
|
||||
pprzlog_transport_init();
|
||||
#endif
|
||||
|
||||
#if SITL
|
||||
#if SITL && !USE_NPS
|
||||
ivy_transport_init();
|
||||
#endif
|
||||
|
||||
|
||||
@@ -36,7 +36,7 @@
|
||||
#include "messages.h"
|
||||
#include "generated/airframe.h" // AC_ID is required
|
||||
|
||||
#if defined SITL
|
||||
#if defined SITL && !USE_NPS
|
||||
/** Software In The Loop simulation uses IVY bus directly as the transport layer */
|
||||
#include "ivy_transport.h"
|
||||
|
||||
|
||||
@@ -0,0 +1,136 @@
|
||||
#include "nps_ivy.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_algebra_double.h"
|
||||
#include "nps_main.h"
|
||||
#include "nps_autopilot.h"
|
||||
#include "nps_fdm.h"
|
||||
#include "nps_sensors.h"
|
||||
#include "nps_atmosphere.h"
|
||||
|
||||
//#include "subsystems/navigation/common_flight_plan.h"
|
||||
|
||||
#if USE_GPS
|
||||
#include "subsystems/gps.h"
|
||||
#endif
|
||||
|
||||
#include NPS_SENSORS_PARAMS
|
||||
|
||||
/* Gaia Ivy functions */
|
||||
static void on_WORLD_ENV(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_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Parse WORLD_ENV message from gaia.
|
||||
*
|
||||
*/
|
||||
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
// wind speed in m/s
|
||||
struct FloatVect3 wind;
|
||||
wind.x = atof(argv[1]); //east
|
||||
wind.y = atof(argv[2]); //north
|
||||
wind.z = atof(argv[3]); //up
|
||||
|
||||
/* set wind speed in NED */
|
||||
nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
|
||||
|
||||
/* not used so far */
|
||||
//float ir_contrast = atof(argv[4]);
|
||||
|
||||
/* set new time factor */
|
||||
nps_set_time_factor(atof(argv[5]));
|
||||
|
||||
#if USE_GPS
|
||||
// directly set gps fix in subsystems/gps/gps_sim_nps.h
|
||||
gps_has_fix = atoi(argv[6]); // gps_availability
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
double_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);
|
||||
|
||||
IvySendMsg("%d NPS_WIND %f %f %f",
|
||||
AC_ID,
|
||||
fdm.wind.x,
|
||||
fdm.wind.y,
|
||||
fdm.wind.z);
|
||||
}
|
||||
@@ -1,12 +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);
|
||||
|
||||
#ifdef USE_MISSION_COMMANDS_IN_NPS
|
||||
extern void nps_ivy_mission_commands_init(void);
|
||||
#endif
|
||||
|
||||
#endif /* NPS_IVY */
|
||||
|
||||
@@ -1,288 +0,0 @@
|
||||
#include "nps_ivy.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_algebra_double.h"
|
||||
#include "nps_main.h"
|
||||
#include "nps_autopilot.h"
|
||||
#include "nps_fdm.h"
|
||||
#include "nps_sensors.h"
|
||||
#include "nps_atmosphere.h"
|
||||
#include "subsystems/ins.h"
|
||||
#include "subsystems/navigation/common_flight_plan.h"
|
||||
|
||||
#if USE_GPS
|
||||
#include "subsystems/gps.h"
|
||||
#endif
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
#include "subsystems/radio_control.h"
|
||||
#endif
|
||||
|
||||
#include NPS_SENSORS_PARAMS
|
||||
|
||||
/* Gaia Ivy functions */
|
||||
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
/* 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[]);
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
static void on_DL_RC_3CH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
static void on_DL_RC_4CH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
#endif
|
||||
|
||||
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);
|
||||
|
||||
IvyBindMsg(on_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
|
||||
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*)");
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_RC_4CH, NULL, "^(\\S*) RC_4CH (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
#endif
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Parse WORLD_ENV message from gaia.
|
||||
*
|
||||
*/
|
||||
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
// wind speed in m/s
|
||||
struct FloatVect3 wind;
|
||||
wind.x = atof(argv[1]); //east
|
||||
wind.y = atof(argv[2]); //north
|
||||
wind.z = atof(argv[3]); //up
|
||||
|
||||
/* set wind speed in NED */
|
||||
nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
|
||||
|
||||
/* not used so far */
|
||||
//float ir_contrast = atof(argv[4]);
|
||||
|
||||
/* set new time factor */
|
||||
nps_set_time_factor(atof(argv[5]));
|
||||
|
||||
#if USE_GPS
|
||||
// directly set gps fix in subsystems/gps/gps_sim_nps.h
|
||||
gps_has_fix = atoi(argv[6]); // gps_availability
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//TODO use datalink parsing from actual fixedwing or rotorcraft firmware,
|
||||
// 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[])
|
||||
{
|
||||
if (atoi(argv[1]) != AC_ID) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* HACK:
|
||||
* we actually don't want to allow changing settings if datalink is disabled,
|
||||
* but since we currently change this variable via settings we have to allow it
|
||||
*/
|
||||
//if (!autopilot.datalink_enabled)
|
||||
// return;
|
||||
|
||||
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[])
|
||||
{
|
||||
if (atoi(argv[1]) != AC_ID) {
|
||||
return;
|
||||
}
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
static void on_DL_PING(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[] __attribute__((unused)))
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
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[])
|
||||
{
|
||||
if (atoi(argv[2]) != AC_ID) {
|
||||
return;
|
||||
}
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
int block = atoi(argv[1]);
|
||||
nav_goto_block(block);
|
||||
printf("goto block %d\n", block);
|
||||
}
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
static void on_DL_RC_3CH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t throttle_mode = atoi(argv[2]);
|
||||
int8_t roll = atoi(argv[3]);
|
||||
int8_t pitch = atoi(argv[4]);
|
||||
parse_rc_3ch_datalink(throttle_mode, roll, pitch);
|
||||
//printf("rc_3ch: throttle_mode %d, roll %d, pitch %d\n", throttle_mode, roll, pitch);
|
||||
}
|
||||
|
||||
static void on_DL_RC_4CH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (atoi(argv[1]) != AC_ID) {
|
||||
return;
|
||||
}
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t mode = atoi(argv[2]);
|
||||
uint8_t throttle = atoi(argv[3]);
|
||||
int8_t roll = atoi(argv[4]);
|
||||
int8_t pitch = atoi(argv[5]);
|
||||
int8_t yaw = atoi(argv[6]);
|
||||
parse_rc_4ch_datalink(mode, throttle, roll, pitch, yaw);
|
||||
//printf("rc_4ch: mode %d, throttle %d, roll %d, pitch %d, yaw %d\n", mode, throttle, roll, pitch, yaw);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
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;
|
||||
double_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);
|
||||
|
||||
IvySendMsg("%d NPS_WIND %f %f %f",
|
||||
AC_ID,
|
||||
fdm.wind.x,
|
||||
fdm.wind.y,
|
||||
fdm.wind.z);
|
||||
}
|
||||
@@ -1,64 +0,0 @@
|
||||
#include "nps_ivy.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <Ivy/ivy.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_double.h"
|
||||
#include "subsystems/ins.h"
|
||||
#include "subsystems/navigation/common_nav.h"
|
||||
#include "nps_autopilot.h"
|
||||
|
||||
/* 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)
|
||||
{
|
||||
/* 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 fixedwing instead of doing it here explicitly
|
||||
|
||||
#include "generated/settings.h"
|
||||
#include "dl_protocol.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
#define MOfCm(_x) (((float)(_x))/100.)
|
||||
#define MOfMM(_x) (((float)(_x))/1000.)
|
||||
|
||||
void on_DL_MOVE_WP(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (atoi(argv[2]) == AC_ID) {
|
||||
uint8_t wp_id = atoi(argv[1]);
|
||||
float a = MOfMM(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);
|
||||
nav_move_waypoint(wp_id, utm.east, utm.north, a);
|
||||
|
||||
/* Waypoint range is limited. Computes the UTM pos back from the relative
|
||||
coordinates */
|
||||
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 zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a);
|
||||
}
|
||||
}
|
||||
@@ -1,321 +0,0 @@
|
||||
#if USE_MISSION_COMMANDS_IN_NPS
|
||||
|
||||
#include "nps_ivy.h"
|
||||
#include "nps_autopilot.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
#include "subsystems/radio_control.h"
|
||||
#endif
|
||||
|
||||
#include NPS_SENSORS_PARAMS
|
||||
|
||||
#define MSG_SIZE 128
|
||||
extern uint8_t dl_buffer[MSG_SIZE];
|
||||
|
||||
/* mission specific Datalink Ivy functions */
|
||||
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void on_DL_END_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[]);
|
||||
|
||||
void nps_ivy_mission_commands_init(void)
|
||||
{
|
||||
|
||||
IvyBindMsg(on_DL_MISSION_GOTO_WP, NULL, "^(\\S*) MISSION_GOTO_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_GOTO_WP_LLA, NULL, "^(\\S*) MISSION_GOTO_WP_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_CIRCLE, NULL, "^(\\S*) MISSION_CIRCLE (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_CIRCLE_LLA, NULL,
|
||||
"^(\\S*) MISSION_CIRCLE_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_SEGMENT, NULL,
|
||||
"^(\\S*) MISSION_SEGMENT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_SEGMENT_LLA, NULL,
|
||||
"^(\\S*) MISSION_SEGMENT_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_PATH, NULL,
|
||||
"^(\\S*) MISSION_PATH (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MISSION_PATH_LLA, NULL,
|
||||
"^(\\S*) MISSION_PATH_LLA (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_GOTO_MISSION, NULL, "^(\\S*) GOTO_MISSION (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_NEXT_MISSION, NULL, "^(\\S*) NEXT_MISSION (\\S*)");
|
||||
IvyBindMsg(on_DL_END_MISSION, NULL, "^(\\S*) END_MISSION (\\S*)");
|
||||
|
||||
}
|
||||
|
||||
#include "generated/settings.h"
|
||||
#include "dl_protocol.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t i = 0;
|
||||
float dummy;
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
for (i = 1; i < 5 ; i++) { //target components
|
||||
dummy = (float)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
|
||||
mission_parse_GOTO_WP();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
uint8_t i = 0;
|
||||
int32_t dummy;
|
||||
for (i = 1; i < 4 ; i++) { //target components (lat, lon, alt in int)
|
||||
dummy = (int32_t)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
float d = (float)(atof(argv[6]));
|
||||
memcpy(&dl_buffer[i * 4], &d, 4); // duration
|
||||
|
||||
mission_parse_GOTO_WP_LLA();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t i = 0;
|
||||
float dummy;
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
for (i = 1; i < 6 ; i++) { //target components
|
||||
dummy = (float)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
|
||||
mission_parse_CIRCLE();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
uint8_t i = 0;
|
||||
int32_t dummy;
|
||||
for (i = 1; i < 4 ; i++) { //target components (lat, lon, alt in int)
|
||||
dummy = (int32_t)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
float d = (float)(atof(argv[6])); // radius in m
|
||||
memcpy(&dl_buffer[4 * 4], &d, 4);
|
||||
d = (float)(atof(argv[7])); // duration
|
||||
memcpy(&dl_buffer[5 * 4], &d, 4);
|
||||
|
||||
mission_parse_CIRCLE_LLA();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t i = 0;
|
||||
float dummy;
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
for (i = 1; i < 7 ; i++) { //target components
|
||||
dummy = (float)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
|
||||
mission_parse_SEGMENT();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
uint8_t i = 0;
|
||||
int32_t dummy;
|
||||
for (i = 1; i < 6 ; i++) { //target components
|
||||
dummy = (int32_t)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
float d = (float)(atof(argv[8]));
|
||||
memcpy(&dl_buffer[i * 4], &d, 4);
|
||||
|
||||
mission_parse_SEGMENT_LLA();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t i = 0;
|
||||
float dummy;
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
for (i = 1; i < 13 ; i++) { //target components
|
||||
dummy = (float)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
dl_buffer[i * 4] = (uint8_t)(atoi(argv[2 + i])); //path nb
|
||||
|
||||
mission_parse_PATH();
|
||||
}
|
||||
|
||||
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //insert mode
|
||||
|
||||
uint8_t i = 0;
|
||||
int32_t dummy;
|
||||
for (i = 1; i < 12 ; i++) { //target components
|
||||
dummy = (int32_t)(atof(argv[2 + i]));
|
||||
memcpy(&dl_buffer[i * 4], &dummy, 4);
|
||||
}
|
||||
float d = (float)(atof(argv[2 + 12])); // duration
|
||||
memcpy(&dl_buffer[i * 4], &d, 4);
|
||||
dl_buffer[13 * 4] = (uint8_t)(atoi(argv[2 + 13])); //path nb
|
||||
|
||||
mission_parse_PATH_LLA();
|
||||
}
|
||||
|
||||
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //mission_id
|
||||
|
||||
mission_parse_GOTO_MISSION();
|
||||
}
|
||||
|
||||
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
|
||||
mission_parse_NEXT_MISSION();
|
||||
}
|
||||
|
||||
static void on_DL_END_MISSION(IvyClientPtr app __attribute__((unused)),
|
||||
void *user_data __attribute__((unused)),
|
||||
int argc __attribute__((unused)), char *argv[])
|
||||
{
|
||||
if (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
|
||||
|
||||
mission_parse_END_MISSION();
|
||||
}
|
||||
|
||||
|
||||
#endif /* USE_MISSION_COMMANDS_IN_NPS */
|
||||
@@ -1,64 +0,0 @@
|
||||
#include "nps_ivy.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.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 "firmwares/rotorcraft/navigation.h"
|
||||
#include "state.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*)");
|
||||
|
||||
#if USE_MISSION_COMMANDS_IN_NPS
|
||||
nps_ivy_mission_commands_init();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#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 (!autopilot.datalink_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (atoi(argv[2]) == AC_ID) {
|
||||
uint8_t wp_id = atoi(argv[1]);
|
||||
|
||||
struct LlaCoor_i lla;
|
||||
lla.lat = atoi(argv[3]);
|
||||
lla.lon = atoi(argv[4]);
|
||||
/* WP_alt from message is alt above MSL in mm
|
||||
* lla.alt is above ellipsoid in mm
|
||||
*/
|
||||
lla.alt = atoi(argv[5]) - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
|
||||
waypoint_move_lla(wp_id, &lla);
|
||||
printf("move wp id=%d x=% .4f, y=% .4f, z=% .4f\n", wp_id,
|
||||
WaypointX(wp_id), WaypointY(wp_id), WaypointAlt(wp_id));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user