diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil new file mode 100644 index 00000000000..63fea2122b6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil @@ -0,0 +1,19 @@ +#!/bin/sh +# +# @name SIH Quadcopter X +# +# @type Simulation +# @class Copter +# +# @maintainer Romain Chiappinelli +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER quad_x +set PWM_OUT 1234 + +# set HIL to avoid sensors startup +param set SYS_HITL 1 + +param set SYS_SIH 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 66972e20239..fca7f6e7094 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_romfs_files( 1000_rc_fw_easystar.hil 1001_rc_quad_x.hil 1002_standard_vtol.hil + 1100_rc_quad_x_sih.hil # [2000, 2999] Standard planes" 2100_standard_plane diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9dc58d36508..ef938cef916 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -337,6 +337,12 @@ else # disable GPS param set GPS_1_CONFIG 0 + # start the SIH if needed + if param compare SYS_SIH 1 + then + sih start + fi + else # # board sensors: rc.sensors diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 50ee37fa0a9..12f8a093e60 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -49,6 +49,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #simulator vmount vtol_att_control diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 2858d338f1d..80773360673 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -69,6 +69,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index f398822f69a..3b55e894cb1 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -84,6 +84,7 @@ px4_add_board( mc_pos_control navigator sensors + sih simulator vmount vtol_att_control diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 3a04541198d..04bbbae4cbe 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -74,6 +74,7 @@ px4_add_board( mc_att_control mc_pos_control sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 9ab0e9cdf0e..4ad315e86c4 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -84,6 +84,7 @@ px4_add_board( mc_pos_control navigator sensors + sih simulator vmount vtol_att_control diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index 18c7e35701b..7934fece4d3 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -74,6 +74,7 @@ px4_add_board( mc_att_control mc_pos_control sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index e19b04e58e7..a62cc1bac74 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 782769e5f92..0ceebaf32c0 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -48,6 +48,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 314e274e989..a6e921156fa 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -46,6 +46,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 3cf7b87cd14..1d454255c70 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -37,6 +37,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #vtol_att_control wind_estimator diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 282b7f6c765..8a66ae5fa92 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -52,6 +52,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #simulator vmount vtol_att_control diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index cfd2dbb81c8..96340c0e7b7 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -50,6 +50,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #simulator vmount vtol_att_control diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 1d6115485e5..5c48c5764bd 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -53,6 +53,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount #vtol_att_control #wind_estimator diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 7bca9069c7d..f8710b8e519 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -56,6 +56,7 @@ px4_add_board( micrortps_bridge navigator sensors + sih vmount #vtol_att_control #wind_estimator diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 857b8849226..dc7dc960d7b 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index fc9c77640ab..25ecce55436 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -65,6 +65,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #vmount #vtol_att_control #wind_estimator diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 5f38f34443b..525277ae283 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -40,6 +40,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index a360022a34f..6b81d64819d 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -84,6 +84,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 41205759da3..cb3a8e6b4b4 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -84,6 +84,7 @@ px4_add_board( micrortps_bridge navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index e6d78488c05..eebec26e93f 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -83,6 +83,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index d83beb27d39..a3b0cc02c35 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -69,6 +69,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 72730122a85..966e66cfc93 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -70,6 +70,7 @@ px4_add_board( micrortps_bridge navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 913db65cf4b..856fb4ffdc8 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -69,6 +69,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 4d46cd82f01..2d515b20d33 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 881ecf6986d..388e827dce0 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -82,6 +82,7 @@ px4_add_board( micrortps_bridge navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 56203b69610..7fef685fd0b 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -83,6 +83,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index c13bae7f16c..2b2d2a386cc 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -66,6 +66,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount wind_estimator diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 57660e82e90..527ea2114fd 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -83,6 +83,7 @@ px4_add_board( micrortps_bridge navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 85ac683482b..9728d25bc63 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -83,6 +83,7 @@ px4_add_board( #micrortps_bridge navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index ba93f231e97..606e2f89d3f 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -45,6 +45,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 1778aed980d..548b2fc27f8 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -43,6 +43,7 @@ px4_add_board( mc_pos_control navigator sensors + sih #simulator vmount vtol_att_control diff --git a/msg/sih.msg b/msg/sih.msg index f685139ed1b..c1fe8a3a935 100644 --- a/msg/sih.msg +++ b/msg/sih.msg @@ -7,3 +7,5 @@ float32[3] omega_b # body rates in body frame [rad/s] float32[3] p_i_local # local inertial position [m] float32[3] v_i # inertial velocity [m] float32[4] u # motor signals [0;1] +uint32 te_us # execution time [us] +uint32 td_us # delay time [us] diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index a066997c5f4..8ffa55f2ffc 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -76,6 +76,19 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); */ PARAM_DEFINE_INT32(SYS_HITL, 0); +/** + * Enable SIH mode on next boot + * + * The simulation in hardware (SIH) will enable a quad simulator to run on the autopilot. + * When disabled the same vehicle can be normally flown outdoors. + * + * @boolean + * @reboot_required true + * + * @group System + */ +PARAM_DEFINE_INT32(SYS_SIH, 0); + /** * Set restart type * diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 794934a42b9..b758ee9a9d4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -765,12 +765,16 @@ Mavlink::set_hil_enabled(bool hil_enabled) if (hil_enabled && !_hil_enabled && _datarate > 5000) { _hil_enabled = true; ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f); + + configure_stream("GROUND_TRUTH", 25.0f); // HIL_STATE_QUATERNION to display the SIH } /* disable HIL */ if (!hil_enabled && _hil_enabled) { _hil_enabled = false; ret = configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f); + + configure_stream("GROUND_TRUTH", 0.0f); } return ret; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6744841cb4f..afbf0b90953 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -4666,12 +4666,13 @@ protected: msg.yawspeed = att.yawspeed; // vehicle_global_position -> hil_state_quaternion - msg.lat = gpos.lat; - msg.lon = gpos.lon; - msg.alt = gpos.alt; - msg.vx = gpos.vel_n; - msg.vy = gpos.vel_e; - msg.vz = gpos.vel_d; + // same units as defined in mavlink/common.xml + msg.lat = gpos.lat * 1e7; + msg.lon = gpos.lon * 1e7; + msg.alt = gpos.alt * 1e3f; + msg.vx = gpos.vel_n * 1e2f; + msg.vy = gpos.vel_e * 1e2f; + msg.vz = gpos.vel_d * 1e2f; msg.ind_airspeed = 0; msg.true_airspeed = 0; msg.xacc = 0; diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt index c44498611ee..ab77255e284 100644 --- a/src/modules/sih/CMakeLists.txt +++ b/src/modules/sih/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 84ad9c43c74..b8d5a598f4e 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -123,7 +123,7 @@ int Sih::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("sih", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, + SCHED_PRIORITY_MAX, //SCHED_PRIORITY_DEFAULT 4096, (px4_main_t)&run_trampoline, (char *const *)argv); @@ -188,108 +188,104 @@ Sih::Sih(int example_param, bool example_flag) void Sih::run() { - - // to subscribe to (read) the actuators_out pwm - int actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + // to subscribe to (read) the actuators_out pwm + _actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs)); // initialize parameters - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - parameters_update_poll(parameter_update_sub); - + _parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + parameters_update_poll(); init_variables(); init_sensors(); // on the AUAVX21: "/dev/ttyS2/" is TELEM2 UART3 --- "/dev/ttyS5/" is Debug UART7 --- "/dev/ttyS4/" is OSD UART8 - int serial_fd=init_serial_port(); // init and open the serial port + // int serial_fd=init_serial_port(); // init and open the serial port const hrt_abstime task_start = hrt_absolute_time(); - hrt_abstime last_run = task_start; - hrt_abstime gps_time = task_start; - hrt_abstime serial_time = task_start; - hrt_abstime now; + _last_run = task_start; + _gps_time = task_start; + _serial_time = task_start; - while (!should_exit() && is_HIL_running(vehicle_status_sub)) { + // hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, this); - now = hrt_absolute_time(); - _dt = (now - last_run) * 1e-6f; - last_run = now; + while (!should_exit()) + { + inner_loop(); - read_motors(actuator_out_sub); - - generate_force_and_torques(); - - equations_of_motion(); - - reconstruct_sensors_signals(); - - send_IMU(now); - - if (now - gps_time > 50000) // gps published at 20Hz - { - gps_time=now; - send_gps(gps_time); - } - - // send uart message every 40 ms - if (now - serial_time > 40000) - { - serial_time=now; - - publish_sih(); // publish _sih message for debug purpose - - send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000); - - parameters_update_poll(parameter_update_sub); // update the parameters if needed - } - // else if (loop_count==5) - // { - // tcflush(serial_fd, TCOFLUSH); // flush output data - // tcdrain(serial_fd); - // } - - usleep(1000); // sleeping time us - + usleep(1000); } - orb_unsubscribe(actuator_out_sub); - orb_unsubscribe(parameter_update_sub); - orb_unsubscribe(vehicle_status_sub); - close(serial_fd); - + // hrt_cancel(&_timer_call); // close the periodic timer interruption + orb_unsubscribe(_actuator_out_sub); + orb_unsubscribe(_parameter_update_sub); + // close(serial_fd); + } -void Sih::parameters_update_poll(int parameter_update_sub) +// timer_callback() is used as a trampoline to inner_loop() +void Sih::timer_callback(void *arg) +{ + (reinterpret_cast(arg))->inner_loop(); +} + +// This is the main execution called periodically by the timer callback +void Sih::inner_loop() +{ + _now = hrt_absolute_time(); + _dt = (_now - _last_run) * 1e-6f; + _last_run = _now; + + read_motors(); + + generate_force_and_torques(); + + equations_of_motion(); + + reconstruct_sensors_signals(); + + send_IMU(); + + if (_now - _gps_time >= 50000) // gps published at 20Hz + { + _gps_time=_now; + send_gps(); + } + + // send uart message every 40 ms + if (_now - _serial_time >= 40000) + { + _serial_time=_now; + + publish_sih(); // publish _sih message for debug purpose + + // send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000); + + parameters_update_poll(); // update the parameters if needed + } + // else if (loop_count==5) + // { + // tcflush(serial_fd, TCOFLUSH); // flush output data + // tcdrain(serial_fd); + // } + + _sih.te_us=hrt_absolute_time()-_now; // execution time (without delay) + +} + +void Sih::parameters_update_poll() { bool updated; struct parameter_update_s param_upd; - orb_check(parameter_update_sub, &updated); + orb_check(_parameter_update_sub, &updated); if (updated) { - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd); + orb_copy(ORB_ID(parameter_update), _parameter_update_sub, ¶m_upd); updateParams(); - parameters_updated(); + parameters_updated(); } } -uint8_t Sih::is_HIL_running(int vehicle_status_sub) -{ - bool updated; - struct vehicle_status_s vehicle_status; - static uint8_t running=false; - - orb_check(vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - running=vehicle_status.hil_state; - } - - return running; -} - // store the parameters in a more convenient form void Sih::parameters_updated() { @@ -395,16 +391,16 @@ int Sih::init_serial_port() } // read the motor signals outputted from the mixer -void Sih::read_motors(const int actuator_out_sub) +void Sih::read_motors() { struct actuator_outputs_s actuators_out {}; // read the actuator outputs bool updated; - orb_check(actuator_out_sub, &updated); + orb_check(_actuator_out_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_outputs), actuator_out_sub, &actuators_out); + orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out); for (int i=0; i // math::radians, #include // to get the physical constants #include // to get the real time +// #include #include #include @@ -49,6 +50,8 @@ #include #include #include +#include // to publish groundtruth +#include // to publish groundtruth using namespace matrix; @@ -84,6 +87,8 @@ public: // generate white Gaussian noise sample as a 3D vector with specified std static Vector3f noiseGauss3f(float stdx, float stdy, float stdz); + static void timer_callback(void *arg); + // static int pack_float(char* uart_msg, int index, void *value); // pack a float to a IEEE754 private: @@ -92,11 +97,9 @@ private: * @param parameter_update_sub uorb subscription to parameter_update * @param force for a parameter update */ - void parameters_update_poll(int parameter_update_sub); + void parameters_update_poll(); void parameters_updated(); - uint8_t is_HIL_running(int vehicle_status_sub); - // to publish the simulator states struct sih_s _sih {}; orb_advert_t _sih_pub{nullptr}; @@ -115,6 +118,15 @@ private: // to publish the gps position struct vehicle_gps_position_s _vehicle_gps_pos {}; orb_advert_t _vehicle_gps_pos_pub{nullptr}; + // attitude groundtruth + struct vehicle_global_position_s _gpos_gt {}; + orb_advert_t _gpos_gt_sub{nullptr}; + // global position groundtruth + struct vehicle_attitude_s _att_gt {}; + orb_advert_t _att_gt_sub{nullptr}; + + int _parameter_update_sub {-1}; + int _actuator_out_sub {-1}; // hard constants static constexpr uint16_t NB_MOTORS = 4; @@ -122,19 +134,27 @@ private: static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre static constexpr uint32_t BAUDS_RATE = 57600; // bauds rate of the serial port + static constexpr hrt_abstime LOOP_INTERVAL = 10000; // 250 Hz real time void init_variables(); void init_sensors(); int init_serial_port(); - void read_motors(const int actuator_out_sub); + void read_motors(); void generate_force_and_torques(); void equations_of_motion(); void reconstruct_sensors_signals(); - void send_IMU(hrt_abstime now); - void send_gps(hrt_abstime now); + void send_IMU(); + void send_gps(); void publish_sih(); void send_serial_msg(int serial_fd, int64_t t_ms); + void inner_loop(); + int32_t _counter = 0; + hrt_call _timer_call; + hrt_abstime _last_run; + hrt_abstime _gps_time; + hrt_abstime _serial_time; + hrt_abstime _now; float _dt; // sampling time [s] char _uart_name[12] = "/dev/ttyS5/"; // serial port name diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index 30485c9f781..91f18c1b94b 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions