diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml
index fd58df0a73..fbd42226ea 100644
--- a/conf/settings/nps.xml
+++ b/conf/settings/nps.xml
@@ -7,6 +7,8 @@
+
+
diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h
index ce18596952..c25b66b2ae 100644
--- a/sw/simulator/nps/nps_autopilot.h
+++ b/sw/simulator/nps/nps_autopilot.h
@@ -27,6 +27,7 @@
struct NpsAutopilot {
double commands[NPS_COMMANDS_NB];
bool_t launch;
+ bool_t datalink_enabled;
};
extern struct NpsAutopilot autopilot;
diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c
index 0e1bc09855..9271269d0d 100644
--- a/sw/simulator/nps/nps_autopilot_fixedwing.c
+++ b/sw/simulator/nps/nps_autopilot_fixedwing.c
@@ -52,6 +52,9 @@
// for launch
#include "firmwares/fixedwing/autopilot.h"
+// for datalink_time hack
+#include "subsystems/datalink/datalink.h"
+
struct NpsAutopilot autopilot;
bool_t nps_bypass_ahrs;
bool_t nps_bypass_ins;
@@ -72,6 +75,7 @@ bool_t nps_bypass_ins;
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
autopilot.launch = FALSE;
+ autopilot.datalink_enabled = TRUE;
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_electrical_init();
@@ -169,6 +173,10 @@ PRINT_CONFIG_VAR(COMMAND_YAW)
if (!launch)
autopilot.commands[COMMAND_THROTTLE] = 0;
+ // hack to reset datalink_time, since we don't use actual dl_parse_msg
+ if (autopilot.datalink_enabled) {
+ datalink_time = 0;
+ }
}
void sim_overwrite_ahrs(void) {
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index d93dd90b88..6eb5a71f00 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -42,6 +42,9 @@
#include "messages.h"
#include "subsystems/datalink/downlink.h"
+// for datalink_time hack
+#include "subsystems/datalink/datalink.h"
+
struct NpsAutopilot autopilot;
bool_t nps_bypass_ahrs;
bool_t nps_bypass_ins;
@@ -60,6 +63,7 @@ bool_t nps_bypass_ins;
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
autopilot.launch = TRUE;
+ autopilot.datalink_enabled = TRUE;
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_electrical_init();
@@ -134,6 +138,10 @@ void nps_autopilot_run_step(double time) {
for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
+ // hack to reset datalink_time, since we don't use actual dl_parse_msg
+ if (autopilot.datalink_enabled) {
+ datalink_time = 0;
+ }
}
diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c
index 5ef6f19d7b..9c5fdf76a4 100644
--- a/sw/simulator/nps/nps_ivy_common.c
+++ b/sw/simulator/nps/nps_ivy_common.c
@@ -87,6 +87,13 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
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);
@@ -99,6 +106,8 @@ static void on_DL_GET_SETTING(IvyClientPtr app __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);
@@ -109,6 +118,9 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
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);
}
@@ -117,6 +129,8 @@ static void on_DL_BLOCK(IvyClientPtr app __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);
@@ -127,6 +141,9 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
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]);
@@ -139,6 +156,8 @@ static void on_DL_RC_4CH(IvyClientPtr app __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]);
diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c
index f94e099a05..cbd617bc07 100644
--- a/sw/simulator/nps/nps_ivy_fixedwing.c
+++ b/sw/simulator/nps/nps_ivy_fixedwing.c
@@ -8,6 +8,7 @@
#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)),
@@ -33,6 +34,8 @@ void nps_ivy_init(char* ivy_bus) {
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]);
diff --git a/sw/simulator/nps/nps_ivy_mission_commands.c b/sw/simulator/nps/nps_ivy_mission_commands.c
index 1e6e64416d..968202fbb6 100644
--- a/sw/simulator/nps/nps_ivy_mission_commands.c
+++ b/sw/simulator/nps/nps_ivy_mission_commands.c
@@ -93,6 +93,9 @@ void nps_ivy_mission_commands_init(void) {
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
@@ -109,6 +112,9 @@ static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)),
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;
+
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -125,6 +131,9 @@ static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)),
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
@@ -141,6 +150,9 @@ static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)),
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;
+
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -157,6 +169,9 @@ static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)),
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
@@ -173,6 +188,9 @@ static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)),
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;
+
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -189,6 +207,9 @@ static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)),
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
@@ -206,6 +227,9 @@ static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)),
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;
+
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -223,6 +247,8 @@ static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)),
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
@@ -233,6 +259,8 @@ static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
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
@@ -242,6 +270,8 @@ static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)),
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
diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c
index 9ab8dfc3b0..c3b7935309 100644
--- a/sw/simulator/nps/nps_ivy_rotorcraft.c
+++ b/sw/simulator/nps/nps_ivy_rotorcraft.c
@@ -41,6 +41,9 @@ void nps_ivy_init(char* ivy_bus) {
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]);