diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index fc0f8b2890..a72e3d740a 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -75,6 +75,7 @@ px4_add_board( mc_pos_control navigator sensors + sih vmount vtol_att_control wind_estimator diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b4ae39fe6c..2db2501ba8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -107,6 +107,7 @@ 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 new file mode 100644 index 0000000000..f685139ed1 --- /dev/null +++ b/msg/sih.msg @@ -0,0 +1,9 @@ +# 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] diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt new file mode 100644 index 0000000000..c44498611e --- /dev/null +++ b/src/modules/sih/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__sih + MAIN sih + STACK_MAIN 1200 + STACK_MAX 3500 + COMPILE_FLAGS + SRCS + sih.cpp + DEPENDS + mathlib + ) \ No newline at end of file diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp new file mode 100644 index 0000000000..84ad9c43c7 --- /dev/null +++ b/src/modules/sih/sih.cpp @@ -0,0 +1,632 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sih.cpp + * Simulator in Hardware + * + * @author Romain Chiappinelli + * + * Coriolis g Corporation - January 2019 + */ + +#include "sih.hpp" + +#include +#include +#include + +#include // to get PWM flags +#include // to get the HIL status + +#include // +#include // +#include // +#include // + +using namespace math; + +int Sih::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Simulator in Hardware +This module provide a simulator for quadrotors running fully +inside the hardware autopilot. + +This simulator subscribes to "actuator_outputs" which are the actuator pwm +signals given by the mixer. + +This simulator publishes the sensors signals corrupted with realistic noise +in order to incorporate the state estimator in the loop. + +### Implementation +The simulator implements the equations of motion using matrix algebra. +Quaternion representation is used for the attitude. +Forward Euler is used for integration. +Most of the variables are declared global in the .hpp file to avoid stack overflow. + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sih", "sih"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 4096, "Optional example parameter", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int Sih::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int Sih::custom_command(int argc, char *argv[]) +{ + /* + if (!is_running()) { + print_usage("not running"); + return 1; + } + + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); +} + + +int Sih::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("sih", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +Sih *Sih::instantiate(int argc, char *argv[]) +{ + int example_param = 0; + bool example_flag = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + // parse CLI arguments + while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + example_param = (int)strtol(myoptarg, nullptr, 10); + break; + + case 'f': + example_flag = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + Sih *instance = new Sih(example_param, example_flag); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +Sih::Sih(int example_param, bool example_flag) + : ModuleParams(nullptr) +{ +} + +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)); + + // initialize parameters + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + parameters_update_poll(parameter_update_sub); + + + 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(); + hrt_abstime last_run = task_start; + hrt_abstime gps_time = task_start; + hrt_abstime serial_time = task_start; + hrt_abstime now; + + while (!should_exit() && is_HIL_running(vehicle_status_sub)) { + + now = hrt_absolute_time(); + _dt = (now - last_run) * 1e-6f; + last_run = now; + + 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 + + } + + orb_unsubscribe(actuator_out_sub); + orb_unsubscribe(parameter_update_sub); + orb_unsubscribe(vehicle_status_sub); + close(serial_fd); + +} + +void Sih::parameters_update_poll(int parameter_update_sub) +{ + bool updated; + struct parameter_update_s param_upd; + + orb_check(parameter_update_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd); + updateParams(); + 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() +{ + + _T_MAX = _sih_t_max.get(); + _Q_MAX = _sih_q_max.get(); + _L_ROLL = _sih_l_roll.get(); + _L_PITCH = _sih_l_pitch.get(); + _KDV = _sih_kdv.get(); + _KDW = _sih_kdw.get(); + _H0 = _sih_h0.get(); + + _LAT0 = (double)_sih_lat0.get()*1.0e-7; + _LON0 = (double)_sih_lon0.get()*1.0e-7; + _COS_LAT0=cosl(radians(_LAT0)); + + _MASS=_sih_mass.get(); + + _W_I=Vector3f(0.0f,0.0f,_MASS*CONSTANTS_ONE_G); + + _I=diag(Vector3f(_sih_ixx.get(),_sih_iyy.get(),_sih_izz.get())); + _I(0,1)=_I(1,0)=_sih_ixy.get(); + _I(0,2)=_I(2,0)=_sih_ixz.get(); + _I(1,2)=_I(2,1)=_sih_iyz.get(); + + _Im1=inv(_I); + + _mu_I=Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get()); + +} + +// initialization of the variables for the simulator +void Sih::init_variables() +{ + srand(1234); // initialize the random seed once before calling generate_wgn() + + _p_I=Vector3f(0.0f,0.0f,0.0f); + _v_I=Vector3f(0.0f,0.0f,0.0f); + _q=Quatf(1.0f,0.0f,0.0f,0.0f); + _w_B=Vector3f(0.0f,0.0f,0.0f); + + _u[0]=_u[1]=_u[2]=_u[3]=0.0f; + +} + +void Sih::init_sensors() +{ + + _sensor_accel.device_id=1; + _sensor_accel.error_count=0; + _sensor_accel.integral_dt=0; + _sensor_accel.temperature=T1_C; + _sensor_accel.scaling=0.0f; + + _sensor_gyro.device_id=1; + _sensor_gyro.error_count=0; + _sensor_gyro.integral_dt=0; + _sensor_gyro.temperature=T1_C; + _sensor_gyro.scaling=0.0f; + + _sensor_mag.device_id=1; + _sensor_mag.error_count=0; + _sensor_mag.temperature=T1_C; + _sensor_mag.scaling=0.0f; + _sensor_mag.is_external=false; + + _sensor_baro.error_count=0; + _sensor_baro.device_id=1; + + _vehicle_gps_pos.fix_type=3; // 3D fix + _vehicle_gps_pos.satellites_used=8; + _vehicle_gps_pos.heading=NAN; + _vehicle_gps_pos.heading_offset=NAN; + _vehicle_gps_pos.s_variance_m_s = 0.5f; + _vehicle_gps_pos.c_variance_rad = 0.1f; + _vehicle_gps_pos.eph = 0.9f; + _vehicle_gps_pos.epv = 1.78f; + _vehicle_gps_pos.hdop = 0.7f; + _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(const int actuator_out_sub) +{ + struct actuator_outputs_s actuators_out {}; + + // read the actuator outputs + bool updated; + orb_check(actuator_out_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_outputs), actuator_out_sub, &actuators_out); + for (int i=0; i0.0f && (_v_I_dot(2)>0.0f || _v_I(2)>0.0f)) + { + _v_I.setZero(); + _w_B.setZero(); + _v_I_dot.setZero(); + } + else + { + // integration: Euler forward + _p_I = _p_I + _p_I_dot*_dt; + _v_I = _v_I + _v_I_dot*_dt; + _q = _q+_q_dot*_dt; // as given in attitude_estimator_q_main.cpp + _q.normalize(); + _w_B = _w_B + _w_B_dot*_dt; + + } +} + +// reconstruct the noisy sensor signals +void Sih::reconstruct_sensors_signals() +{ + + // The sensor signals reconstruction and noise levels are from + // Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." + // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. + + // IMU + _acc=_C_IB.transpose()*(_v_I_dot-Vector3f(0.0f,0.0f,CONSTANTS_ONE_G))+noiseGauss3f(0.5f,1.7f,1.4f); + _gyro=_w_B+noiseGauss3f(0.14f,0.07f,0.03f); + _mag=_C_IB.transpose()*_mu_I+noiseGauss3f(0.02f,0.02f,0.03f); + + // barometer + float altitude=(_H0-_p_I(2))+generate_wgn()*0.14f; // altitude with noise + _baro_p_mBar=CONSTANTS_STD_PRESSURE_MBAR* // reconstructed pressure in mBar + powf((1.0f+altitude*TEMP_GRADIENT/T1_K),-CONSTANTS_ONE_G/(TEMP_GRADIENT*CONSTANTS_AIR_GAS_CONST)); + _baro_temp_c=T1_K+CONSTANTS_ABSOLUTE_NULL_CELSIUS+TEMP_GRADIENT*altitude; // reconstructed temperture in celcius + + // GPS + _gps_lat_noiseless=_LAT0+degrees((double)_p_I(0)/CONSTANTS_RADIUS_OF_EARTH); + _gps_lon_noiseless=_LON0+degrees((double)_p_I(1)/CONSTANTS_RADIUS_OF_EARTH)/_COS_LAT0; + _gps_alt_noiseless=_H0-_p_I(2); + + _gps_lat=_gps_lat_noiseless+(double)(generate_wgn()*7.2e-6f); // latitude in degrees + _gps_lon=_gps_lon_noiseless+(double)(generate_wgn()*1.75e-5f); // longitude in degrees + _gps_alt=_gps_alt_noiseless+generate_wgn()*1.78f; + _gps_vel=_v_I+noiseGauss3f(0.06f,0.077f,0.158f); +} + +void Sih::send_IMU(hrt_abstime now) +{ + _sensor_accel.timestamp=now; + _sensor_accel.x=_acc(0); + _sensor_accel.y=_acc(1); + _sensor_accel.z=_acc(2); + if (_sensor_accel_pub != nullptr) { + orb_publish(ORB_ID(sensor_accel), _sensor_accel_pub, &_sensor_accel); + } else { + _sensor_accel_pub = orb_advertise(ORB_ID(sensor_accel), &_sensor_accel); + } + + _sensor_gyro.timestamp=now; + _sensor_gyro.x=_gyro(0); + _sensor_gyro.y=_gyro(1); + _sensor_gyro.z=_gyro(2); + if (_sensor_gyro_pub != nullptr) { + orb_publish(ORB_ID(sensor_gyro), _sensor_gyro_pub, &_sensor_gyro); + } else { + _sensor_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &_sensor_gyro); + } + + _sensor_mag.timestamp=now; + _sensor_mag.x=_mag(0); + _sensor_mag.y=_mag(1); + _sensor_mag.z=_mag(2); + if (_sensor_mag_pub != nullptr) { + orb_publish(ORB_ID(sensor_mag), _sensor_mag_pub, &_sensor_mag); + } else { + _sensor_mag_pub = orb_advertise(ORB_ID(sensor_mag), &_sensor_mag); + } + + _sensor_baro.timestamp=now; + _sensor_baro.pressure=_baro_p_mBar; + _sensor_baro.temperature=_baro_temp_c; + if (_sensor_baro_pub != nullptr) { + orb_publish(ORB_ID(sensor_baro), _sensor_baro_pub, &_sensor_baro); + } else { + _sensor_baro_pub = orb_advertise(ORB_ID(sensor_baro), &_sensor_baro); + } +} + +void Sih::send_gps(hrt_abstime now) +{ + _vehicle_gps_pos.timestamp=now; + _vehicle_gps_pos.lat=(int32_t)(_gps_lat*1e7); // Latitude in 1E-7 degrees + _vehicle_gps_pos.lon=(int32_t)(_gps_lon*1e7); // Longitude in 1E-7 degrees + _vehicle_gps_pos.alt=(int32_t)(_gps_alt*1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) + _vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt*1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres) + _vehicle_gps_pos.vel_ned_valid=true; // True if NED velocity is valid + _vehicle_gps_pos.vel_m_s=sqrtf(_gps_vel(0)*_gps_vel(0)+_gps_vel(1)*_gps_vel(1)); // GPS ground speed, (metres/sec) + _vehicle_gps_pos.vel_n_m_s=_gps_vel(0); // GPS North velocity, (metres/sec) + _vehicle_gps_pos.vel_e_m_s=_gps_vel(1); // GPS East velocity, (metres/sec) + _vehicle_gps_pos.vel_d_m_s=_gps_vel(2); // GPS Down velocity, (metres/sec) + _vehicle_gps_pos.cog_rad=atan2(_gps_vel(1),_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + if (_vehicle_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos); + } else { + _vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos); + } +} + +void Sih::publish_sih() +{ + + 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 +{ + float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + + return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); +} + +Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz) // generate white Gaussian noise sample with specified std +{ + return Vector3f(generate_wgn()*stdx,generate_wgn()*stdy,generate_wgn()*stdz); +} // there is another wgn algorithm in BlockRandGauss.hpp + +int sih_main(int argc, char *argv[]) +{ + return Sih::main(argc, argv); +} + +// int Sih::pack_float(char* uart_msg, int index, void *value) +// { +// uint32_t value_raw=(uint32_t)(value*); + +// for (int i=3; i>=0; i=i-1) { +// buffer[index+i]=(char)(value_raw&0xFF); +// value_raw=value_raw>>8; +// } + +// return index+4; // points to the index for the next value +// } \ No newline at end of file diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp new file mode 100644 index 0000000000..1c8de14612 --- /dev/null +++ b/src/modules/sih/sih.hpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include // matrix, vectors, dcm, quaterions +#include // math::radians, +#include // to get the physical constants +#include // to get the real time + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +extern "C" __EXPORT int sih_main(int argc, char *argv[]); + +class Sih : public ModuleBase, public ModuleParams +{ +public: + Sih(int example_param, bool example_flag); + + virtual ~Sih() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Sih *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + static float generate_wgn(); // generate white Gaussian noise sample + + // generate white Gaussian noise sample as a 3D vector with specified std + static Vector3f noiseGauss3f(float stdx, float stdy, float stdz); + + // static int pack_float(char* uart_msg, int index, void *value); // pack a float to a IEEE754 +private: + + /** + * Check for parameter changes and update them if needed. + * @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_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}; + // to publish the sensor baro + struct sensor_baro_s _sensor_baro {}; + orb_advert_t _sensor_baro_pub{nullptr}; + // to publish the sensor mag + struct sensor_mag_s _sensor_mag {}; + orb_advert_t _sensor_mag_pub{nullptr}; + // to publish the sensor gyroscope + struct sensor_gyro_s _sensor_gyro {}; + orb_advert_t _sensor_gyro_pub{nullptr}; + // to publish the sensor accelerometer + struct sensor_accel_s _sensor_accel {}; + orb_advert_t _sensor_accel_pub{nullptr}; + // to publish the gps position + struct vehicle_gps_position_s _vehicle_gps_pos {}; + orb_advert_t _vehicle_gps_pos_pub{nullptr}; + + // hard constants + static constexpr uint16_t NB_MOTORS = 4; + 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 + + void init_variables(); + void init_sensors(); + int init_serial_port(); + void read_motors(const int actuator_out_sub); + 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 publish_sih(); + void send_serial_msg(int serial_fd, int64_t t_ms); + + 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] + Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm] + Vector3f _p_I; // inertial position [m] + Vector3f _v_I; // inertial velocity [m/s] + Vector3f _v_B; // body frame velocity [m/s] + Vector3f _p_I_dot; // inertial position differential + Vector3f _v_I_dot; // inertial velocity differential + Quatf _q; // quaternion attitude + Dcmf _C_IB; // body to inertial transformation + Vector3f _w_B; // body rates in body frame [rad/s] + Quatf _q_dot; // quaternion differential + Vector3f _w_B_dot; // body rates differential + float _u[NB_MOTORS]; // thruster signals + + + // sensors reconstruction + Vector3f _acc; + Vector3f _mag; + Vector3f _gyro; + Vector3f _gps_vel; + double _gps_lat, _gps_lat_noiseless; + double _gps_lon, _gps_lon_noiseless; + float _gps_alt, _gps_alt_noiseless; + float _baro_p_mBar; // reconstructed (simulated) pressure in mBar + float _baro_temp_c; // reconstructed (simulated) barometer temperature in celcius + + // parameters + float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0; + double _LAT0, _LON0, _COS_LAT0; + Vector3f _W_I; // weight of the vehicle in inertial frame [N] + Matrix3f _I; // vehicle inertia matrix + Matrix3f _Im1; // inverse of the intertia matrix + Vector3f _mu_I; // NED magnetic field in inertial frame [G] + + // parameters defined in sih_params.c + DEFINE_PARAMETERS( + (ParamFloat) _sih_mass, + (ParamFloat) _sih_ixx, + (ParamFloat) _sih_iyy, + (ParamFloat) _sih_izz, + (ParamFloat) _sih_ixy, + (ParamFloat) _sih_ixz, + (ParamFloat) _sih_iyz, + (ParamFloat) _sih_t_max, + (ParamFloat) _sih_q_max, + (ParamFloat) _sih_l_roll, + (ParamFloat) _sih_l_pitch, + (ParamFloat) _sih_kdv, + (ParamFloat) _sih_kdw, + (ParamInt) _sih_lat0, + (ParamInt) _sih_lon0, + (ParamFloat) _sih_h0, + (ParamFloat) _sih_mu_x, + (ParamFloat) _sih_mu_y, + (ParamFloat) _sih_mu_z + ) +}; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c new file mode 100644 index 0000000000..30485c9f78 --- /dev/null +++ b/src/modules/sih/sih_params.c @@ -0,0 +1,345 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sih_params.c + * Parameters for quadcopter X simulator in hardware. + * + * @author Romain Chiappinelli + * February 2019 + */ + +/** + * Vehicle mass + * + * This value can be measured by weighting the quad on a scale. + * + * @unit kg + * @min 0.0 + * @decimal 2 + * @increment 0.1 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_MASS, 1.0f); + +/** + * Vehicle inertia about X axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg*m*m + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IXX, 0.025f); + +/** + * Vehicle inertia about Y axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg*m*m + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IYY, 0.025f); + +/** + * Vehicle inertia about Z axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg*m*m + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IZZ, 0.030f); + +/** + * Vehicle cross term inertia xy + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg*m*m + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IXY, 0.0f); + +/** + * Vehicle cross term inertia xz + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg*m*m + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IXZ, 0.0f); + +/** + * Vehicle cross term inertia yz + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg*m*m + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_IYZ, 0.0f); + +/** + * Max propeller thrust force + * + * This is the maximum force delivered by one propeller + * when the motor is running at full speed. + * + * This value is usually about 5 times the mass of the quadrotor. + * + * @unit N + * @min 0.0 + * @decimal 2 + * @increment 0.5 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_T_MAX, 5.0f); + +/** + * Max propeller torque + * + * This is the maximum torque delivered by one propeller + * when the motor is running at full speed. + * + * This value is usually about few percent of the maximum thrust force. + * + * @unit Nm + * @min 0.0 + * @decimal 3 + * @increment 0.05 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_Q_MAX, 0.1f); + +/** + * Roll arm length + * + * This is the arm length generating the rolling moment + * + * This value can be measured with a ruler. + * This corresponds to half the distance between the left and right motors. + * + * @unit m + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_L_ROLL, 0.2f); + +/** + * Pitch arm length + * + * This is the arm length generating the pitching moment + * + * This value can be measured with a ruler. + * This corresponds to half the distance between the front and rear motors. + * + * @unit m + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_L_PITCH, 0.2f); + +/** + * First order drag coefficient + * + * Physical coefficient representing the friction with air particules. + * The greater this value, the slower the quad will move. + * + * Drag force function of velocity: D=-KDV*V. + * The maximum freefall velocity can be computed as V=10*MASS/KDV [m/s] + * + * @unit N/(m/s) + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_KDV, 1.0f); + +/** + * First order angular damper coefficient + * + * Physical coefficient representing the friction with air particules during rotations. + * The greater this value, the slower the quad will rotate. + * + * Aerodynamic moment function of body rate: Ma=-KDW*W_B. + * This value can be set to 0 if unknown. + * + * @unit Nm/(rad/s) + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f); + +/** + * Initial geodetic latitude + * + * This value represents the North-South location on Earth where the simulation begins. + * A value of 45 deg should be written 450000000. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit 1e-7 deg + * @min -850000000 + * @max 850000000 + * @group SIH + */ +PARAM_DEFINE_INT32(SIH_LAT0, 454671160); + +/** + * Initial geodetic longitude + * + * This value represents the East-West location on Earth where the simulation begins. + * A value of 45 deg should be written 450000000. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit 1e-7 deg + * @min -1800000000 + * @max 1800000000 + * @group SIH + */ +PARAM_DEFINE_INT32(SIH_LON0, -737578370); + +/** + * Initial AMSL ground altitude + * + * This value represents the Above Mean Sea Level (AMSL) altitude where the simulation begins. + * + * If using FlightGear as a visual animation, + * this value can be tweaked such that the vehicle lies on the ground at takeoff. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * + * @unit m + * @min -420.0 + * @max 8848.0 + * @decimal 2 + * @increment 0.01 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_H0, 32.34f); + +/** + * North magnetic field at the initial location + * + * This value represents the North magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit Gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_MU_X, 0.179f); + +/** + * East magnetic field at the initial location + * + * This value represents the East magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit Gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_MU_Y, -0.045f); + +/** + * Down magnetic field at the initial location + * + * This value represents the Down magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit Gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group SIH + */ +PARAM_DEFINE_FLOAT(SIH_MU_Z, 0.504f); \ No newline at end of file