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]);