sih.msg removed, serial port communication removed

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

msg/sih.msg

@@ -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]

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,60 +548,6 @@ 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

src/modules/sih/sih.hpp

@@ -50,7 +50,6 @@
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/sih.h>
 #include <uORB/topics/vehicle_global_position.h> 	// to publish groundtruth
 #include <uORB/topics/vehicle_attitude.h> 			// 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]