diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile index a6631f7d14..a761afaa6d 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile @@ -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 diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index a457e41cc5..a457e43fda 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -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 diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 3ecc04b488..3ea3dab4a7 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -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 diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index d66012b172..72370c212d 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -92,7 +92,7 @@ void downlink_init(void) pprzlog_transport_init(); #endif -#if SITL +#if SITL && !USE_NPS ivy_transport_init(); #endif diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 333ecb6dfc..1dae2bb634 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -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" diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c new file mode 100644 index 0000000000..2bc1549e1b --- /dev/null +++ b/sw/simulator/nps/nps_ivy.c @@ -0,0 +1,136 @@ +#include "nps_ivy.h" + +#include +#include +#include +#include + +#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); +} diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 08402edd28..7193f40813 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -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 */ diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c deleted file mode 100644 index d459807f39..0000000000 --- a/sw/simulator/nps/nps_ivy_common.c +++ /dev/null @@ -1,288 +0,0 @@ -#include "nps_ivy.h" - -#include -#include -#include -#include - -#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); -} diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c deleted file mode 100644 index d0671b4ea1..0000000000 --- a/sw/simulator/nps/nps_ivy_fixedwing.c +++ /dev/null @@ -1,64 +0,0 @@ -#include "nps_ivy.h" - -#include -#include -#include - -#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); - } -} diff --git a/sw/simulator/nps/nps_ivy_mission_commands.c b/sw/simulator/nps/nps_ivy_mission_commands.c deleted file mode 100644 index 5ec2475051..0000000000 --- a/sw/simulator/nps/nps_ivy_mission_commands.c +++ /dev/null @@ -1,321 +0,0 @@ -#if USE_MISSION_COMMANDS_IN_NPS - -#include "nps_ivy.h" -#include "nps_autopilot.h" - -#include -#include - -#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 */ diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c deleted file mode 100644 index 7dea7734cb..0000000000 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ /dev/null @@ -1,64 +0,0 @@ -#include "nps_ivy.h" - -#include -#include -#include - -#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)); - } -}