Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[nps] add possibility to simulate loss of datalink/downlink

Browse Source 
It is a bit of a hack, especially we can't disable changing settings if datalink is disabled,
since we need to parse that to enable the datalink via settings again.

Nevertheless it should suffice for most cases and close #934
This commit is contained in:
Felix Ruess
2015-03-26 19:00:06 +01:00
parent 8d91861ea8
commit bdd7ceb43e
8 changed files with 74 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/nps.xml
+2
View File
@@ -7,6 +7,8 @@
<dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="bypass_ahrs" values="No|Yes"/>
<dl_setting var="nps_bypass_ins" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="bypass_ins" values="No|Yes"/>
<dl_setting var="gps_has_fix" min="0" step="1" max="1" module="subsystems/gps/gps_sim_nps" shortname="gps_fix" values="No|Yes"/>
<dl_setting var="autopilot.datalink_enabled" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="datalink" values="OFF|ON"/>
<dl_setting var="ivy_tp.ivy_dl_enabled" min="0" step="1" max="1" module="subsystems/datalink/ivy_transport" shortname="downlink" values="OFF|ON"/>
<dl_setting var="nps_electrical.supply_voltage" min="0" step="0.1" max="24" module="nps/nps_electrical" shortname="bat_voltage" unit="V"/>
<dl_setting var="nps_atmosphere.wind_speed" min="0" step="0.1" max="25" module="nps/nps_atmosphere" shortname="wind_speed" unit="m/s"/>
<dl_setting var="nps_atmosphere.wind_dir" min="0" step="1" max="360" module="nps/nps_atmosphere" shortname="wind_dir" unit="rad" alt_unit="deg"/>
sw/simulator/nps/nps_autopilot.h
+1
View File
@@ -27,6 +27,7 @@
struct NpsAutopilot {
double commands[NPS_COMMANDS_NB];
bool_t launch;
bool_t datalink_enabled;
};
extern struct NpsAutopilot autopilot;
sw/simulator/nps/nps_autopilot_fixedwing.c
+8
View File
@@ -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) {
sw/simulator/nps/nps_autopilot_rotorcraft.c
+8
View File
@@ -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;
}
}
sw/simulator/nps/nps_ivy_common.c
+19
View File
@@ -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]);
sw/simulator/nps/nps_ivy_fixedwing.c
+3
View File
@@ -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]);
sw/simulator/nps/nps_ivy_mission_commands.c
+30
View File
@@ -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
sw/simulator/nps/nps_ivy_rotorcraft.c
+3
View File
@@ -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]);