diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index a62460df79..688d5b7cb4 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -56,6 +56,7 @@ 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_flightgear.c \ diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index f57c246e8c..14a04e502f 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -38,7 +38,7 @@ else endif -nps.srcs += $(NPSDIR)/nps_main.c \ +nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_fdm_jsbsim.c \ $(NPSDIR)/nps_random.c \ $(NPSDIR)/nps_sensors.c \ @@ -51,17 +51,18 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ - $(NPSDIR)/nps_autopilot_rotorcraft.c \ - $(NPSDIR)/nps_ivy.c \ + $(NPSDIR)/nps_autopilot_rotorcraft.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_rotorcraft.c \ $(NPSDIR)/nps_flightgear.c \ nps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT -nps.srcs += firmwares/rotorcraft/main.c -nps.srcs += mcu.c -nps.srcs += $(SRC_ARCH)/mcu_arch.c +nps.srcs += firmwares/rotorcraft/main.c +nps.srcs += mcu.c +nps.srcs += $(SRC_ARCH)/mcu_arch.c nps.srcs += mcu_periph/i2c.c nps.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 0e58a7ee02..09bed64681 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -85,11 +85,12 @@ void nps_autopilot_run_systime_step( void ) { void nps_autopilot_run_step(double time __attribute__ ((unused))) { +#ifdef RADIO_CONTROL_TYPE_PPM if (nps_radio_control_available(time)) { radio_control_feed(); Fbw(event_task); - Ap(event_task); } +#endif if (nps_sensors_gyro_available()) { imu_feed_gyro_accel(); diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 64011b7a1f..db8b993365 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -1,6 +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); diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy_common.c similarity index 85% rename from sw/simulator/nps/nps_ivy.c rename to sw/simulator/nps/nps_ivy_common.c index 93847c425c..90766d756e 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -10,7 +10,6 @@ #include "nps_fdm.h" #include "nps_sensors.h" #include "subsystems/ins.h" -#include "firmwares/rotorcraft/navigation.h" #ifdef RADIO_CONTROL_TYPE_DATALINK #include "subsystems/radio_control.h" @@ -36,10 +35,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); -static void on_DL_MOVE_WP(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)), @@ -50,7 +45,7 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); #endif -void nps_ivy_init(char* ivy_bus) { +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); @@ -58,7 +53,6 @@ void nps_ivy_init(char* ivy_bus) { 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*)"); - IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); #ifdef RADIO_CONTROL_TYPE_DATALINK IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)"); @@ -117,25 +111,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), printf("goto block %d\n", block); } -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]) { - uint8_t wp_id = atoi(argv[1]); - - struct LlaCoor_i lla; - struct EnuCoor_i enu; - lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); - lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); - lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); - printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); -} - #ifdef RADIO_CONTROL_TYPE_DATALINK static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c index 386f35ad5e..a7bd73cbba 100644 --- a/sw/simulator/nps/nps_ivy_fixedwing.c +++ b/sw/simulator/nps/nps_ivy_fixedwing.c @@ -2,114 +2,45 @@ #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 "subsystems/ins.h" #include "subsystems/navigation/common_nav.h" -#include NPS_SENSORS_PARAMS - - -/* 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[]); - -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); +/* 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) { - 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_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*)"); - IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); + /* init ivy and bind some messages common to fw and rotorcraft */ + nps_ivy_common_init(ivy_bus); -#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); - } + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); } -//TODO use datalink parsing from booz or fw instead of doing it here explicitly -//FIXME currently parsed correctly for booz only - +//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" -static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]) { - 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[]) { - 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); -} +#define MOfCm(_x) (((float)(_x))/100.) -static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[] __attribute__ ((unused))) { - 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[]){ - int block = atoi(argv[1]); - nav_goto_block(block); - printf("goto block %d\n", block); -} - -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]) { +void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]) { if (atoi(argv[2]) == AC_ID) { uint8_t wp_id = atoi(argv[1]); - float a = atoi(argv[5]) * 0.1; + float a = MOfCm(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); @@ -120,58 +51,6 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), 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 alt=%f\n", wp_id, utm.east, utm.north, a); + printf("move wp id=%d east=%f north=%f zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a); } } - - -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; - FLOAT_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); -} diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c new file mode 100644 index 0000000000..bf0ef5562b --- /dev/null +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -0,0 +1,58 @@ +#include "nps_ivy.h" + +#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 "subsystems/ins.h" +#include "firmwares/rotorcraft/navigation.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*)"); + +} + +//TODO use datalink parsing from rotorcraft instead of doing it here explicitly + +#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 (atoi(argv[2]) == AC_ID) { + uint8_t wp_id = atoi(argv[1]); + + struct LlaCoor_i lla; + struct EnuCoor_i enu; + lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); + lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); + lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; + enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); + enu.x = POS_BFP_OF_REAL(enu.x)/100; + enu.y = POS_BFP_OF_REAL(enu.y)/100; + enu.z = POS_BFP_OF_REAL(enu.z)/100; + VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); + printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); + } +}