From 8b6480c1cf57d5e08b6efceaf56dc8dc4edd3a7d Mon Sep 17 00:00:00 2001 From: romain Date: Mon, 18 Mar 2019 11:11:15 -0400 Subject: [PATCH] sih.msg removed, serial port communication removed --- msg/CMakeLists.txt | 1 - msg/sih.msg | 11 ------ src/modules/sih/sih.cpp | 85 ----------------------------------------- src/modules/sih/sih.hpp | 9 ----- 4 files changed, 106 deletions(-) delete mode 100644 msg/sih.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2db2501ba8..b4ae39fe6c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -107,7 +107,6 @@ set(msg_files sensor_preflight.msg sensor_selection.msg servorail_status.msg - sih.msg subsystem_info.msg system_power.msg task_stack_info.msg diff --git a/msg/sih.msg b/msg/sih.msg deleted file mode 100644 index c1fe8a3a93..0000000000 --- a/msg/sih.msg +++ /dev/null @@ -1,11 +0,0 @@ -# simulator in Hardware - Romain Chiappinelli - Jan 8, 2019 -uint64 timestamp # time since system start (microseconds) - -uint32 dt_us # simulator sampling time [us] -float32[3] euler_rpy # euler angles (roll-pitch-yaw) [deg] -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/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 4ccfa24531..f5b7a746c3 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -199,8 +199,6 @@ void Sih::run() 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 const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; @@ -231,7 +229,6 @@ void Sih::run() hrt_cancel(&_timer_call); // close the periodic timer interruption orb_unsubscribe(_actuator_out_sub); orb_unsubscribe(_parameter_update_sub); - // close(serial_fd); } @@ -271,13 +268,8 @@ void Sih::inner_loop() 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 } - - _sih.te_us=hrt_absolute_time()-_now; // execution time (without delay) - } void Sih::parameters_update_poll() @@ -375,29 +367,6 @@ void Sih::init_sensors() _vehicle_gps_pos.vdop = 1.1f; } -int Sih::init_serial_port() -{ - struct termios uart_config; - int serial_fd = open(_uart_name, O_WRONLY | O_NONBLOCK | O_NOCTTY); - - if (serial_fd < 0) { - PX4_ERR("failed to open port: %s", _uart_name); - } - - tcgetattr(serial_fd, &uart_config); // read configuration - - uart_config.c_oflag |= ONLCR; - // try to set Bauds rate - if (cfsetispeed(&uart_config, BAUDS_RATE) < 0 || cfsetospeed(&uart_config, BAUDS_RATE) < 0) { - PX4_WARN("ERR SET BAUD %s\n", _uart_name); - close(serial_fd); - } - - tcsetattr(serial_fd, TCSANOW, &uart_config); // set config - - return serial_fd; -} - // read the motor signals outputted from the mixer void Sih::read_motors() { @@ -579,62 +548,8 @@ void Sih::publish_sih() } else { _att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); } - - Eulerf Euler(_q); - _sih.timestamp=hrt_absolute_time(); - _sih.dt_us=(uint32_t)(_dt*1e6f); - _sih.euler_rpy[0]=degrees(Euler(0)); - _sih.euler_rpy[1]=degrees(Euler(1)); - _sih.euler_rpy[2]=degrees(Euler(2)); - _sih.omega_b[0]=_w_B(0); // wing body rates in body frame - _sih.omega_b[1]=_w_B(1); - _sih.omega_b[2]=_w_B(2); - _sih.p_i_local[0]=_p_I(0); // local inertial position - _sih.p_i_local[1]=_p_I(1); - _sih.p_i_local[2]=_p_I(2); - _sih.v_i[0]=_v_I(0); // inertial velocity - _sih.v_i[1]=_v_I(1); - _sih.v_i[2]=_v_I(2); - _sih.u[0]=_u[0]; - _sih.u[1]=_u[1]; - _sih.u[2]=_u[2]; - _sih.u[3]=_u[3]; - if (_sih_pub != nullptr) { - orb_publish(ORB_ID(sih), _sih_pub, &_sih); - } else { - _sih_pub = orb_advertise(ORB_ID(sih), &_sih); - } } -void Sih::send_serial_msg(int serial_fd, int64_t t_ms) -{ - - char uart_msg[100]; - - uint8_t n; - int32_t GPS_pos[3]; // latitude, longitude in 10^-7 degrees, altitude AMSL in mm - int32_t EA_deci_deg[3]; // Euler angles in deci degrees integers to send to serial - int32_t throttles[4]; // throttles from 0 to 99 - - GPS_pos[0]=(int32_t)(_gps_lat_noiseless*1e7); // Latitude in 1E-7 degrees - GPS_pos[1]=(int32_t)(_gps_lon_noiseless*1e7); // Longitude in 1E-7 degrees - GPS_pos[2]=(int32_t)(_gps_alt_noiseless*1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) - Eulerf Euler(_q); - EA_deci_deg[0]=(int32_t)degrees(Euler(0)*10.0f); // decidegrees - EA_deci_deg[1]=(int32_t)degrees(Euler(1)*10.0f); - EA_deci_deg[2]=(int32_t)degrees(Euler(2)*10.0f); - throttles[0]=(int32_t)(_u[0]*99.0f); - throttles[1]=(int32_t)(_u[1]*99.0f); - throttles[2]=(int32_t)(_u[2]*99.0f); - throttles[3]=(int32_t)(_u[3]*99.0f); - - n = sprintf(uart_msg, "T%07lld,P%+010d%+010d%+08d,A%+05d%+05d%+05d,U%+03d%+03d%+03d%+03d\n", - t_ms,GPS_pos[0],GPS_pos[1],GPS_pos[2], - EA_deci_deg[0],EA_deci_deg[1],EA_deci_deg[2], - throttles[0],throttles[1],throttles[2],throttles[3]); - write(serial_fd, uart_msg, n); -} - float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 { // algorithm 1: diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 6c7ea744da..0a9d09a1bf 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include // to publish groundtruth #include // to publish groundtruth @@ -102,9 +101,6 @@ private: void parameters_update_poll(); void parameters_updated(); - // to publish the simulator states - struct sih_s _sih {}; - orb_advert_t _sih_pub{nullptr}; // to publish the sensor baro struct sensor_baro_s _sensor_baro {}; orb_advert_t _sensor_baro_pub{nullptr}; @@ -135,12 +131,10 @@ private: static constexpr float T1_C = 15.0f; // ground temperature in celcius 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 = 4000; // 4ms => 250 Hz real-time void init_variables(); void init_sensors(); - int init_serial_port(); void read_motors(); void generate_force_and_torques(); void equations_of_motion(); @@ -148,7 +142,6 @@ private: void send_IMU(); void send_gps(); void publish_sih(); - void send_serial_msg(int serial_fd, int64_t t_ms); void inner_loop(); perf_counter_t _loop_perf; @@ -164,8 +157,6 @@ private: hrt_abstime _now; float _dt; // sampling time [s] - char _uart_name[12] = "/dev/ttyS5/"; // serial port name - Vector3f _T_B; // thrust force in body frame [N] Vector3f _Fa_I; // aerodynamic force in inertial frame [N] Vector3f _Mt_B; // thruster moments in the body frame [Nm]