Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 19:57:12 +08:00
Code Issues Actions 14 Packages Projects Releases Wiki Activity

send MAVLink GROUND_TRUTH at 25 Hz, only in SIH mode. And minor cleanup

Browse Source
This commit is contained in:
Beat Küng
2019-04-11 10:42:58 +02:00
parent 9adb4410bd
commit 744b50b478
6 changed files with 408 additions and 453 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+1 -1
View File
@@ -337,7 +337,7 @@ else
param set GPS_1_CONFIG 0 param set GPS_1_CONFIG 0
# start the simulator in hardware if needed # start the simulator in hardware if needed
if param compare SYS_HITL 2 if param compare SYS_HITL 2
then then
sih start sih start
fi fi
src/modules/mavlink/mavlink_main.cpp
+6 -1
View File
@@ -766,7 +766,12 @@ Mavlink::set_hil_enabled(bool hil_enabled)
_hil_enabled = true; _hil_enabled = true;
ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f); ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
configure_stream("GROUND_TRUTH", 200.0f); // HIL_STATE_QUATERNION to display the SIH if (_param_sys_hitl.get() == 2) { // Simulation in Hardware enabled ?
configure_stream("GROUND_TRUTH", 25.0f); // HIL_STATE_QUATERNION to display the SIH
} else {
configure_stream("GROUND_TRUTH", 0.0f);
}
} }
/* disable HIL */ /* disable HIL */
src/modules/mavlink/mavlink_main.h
+2 -1
View File
@@ -656,7 +656,8 @@ private:
(ParamInt<px4::params::MAV_BROADCAST>) _param_mav_broadcast, (ParamInt<px4::params::MAV_BROADCAST>) _param_mav_broadcast,
(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en, (ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en, (ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp (ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
) )
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
src/modules/sih/sih.cpp
+292 -340
View File
File diff suppressed because it is too large Load Diff
src/modules/sih/sih.hpp
+101 -104
View File
@@ -1,35 +1,35 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * are met:
* *
* 1. Redistributions of source code must retain the above copyright * 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright * 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in * notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the * the documentation and/or other materials provided with the
* distribution. * distribution.
* 3. Neither the name PX4 nor the names of its contributors may be * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software * used to endorse or promote products derived from this software
* without specific prior written permission. * without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
* *
****************************************************************************/ ****************************************************************************/
#pragma once #pragma once
@@ -37,10 +37,10 @@
#include <px4_module_params.h> #include <px4_module_params.h>
#include <px4_posix.h> #include <px4_posix.h>
#include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions #include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions
#include <conversion/rotation.h> // math::radians, #include <conversion/rotation.h> // math::radians,
#include <ecl/geo/geo.h> // to get the physical constants #include <ecl/geo/geo.h> // to get the physical constants
#include <drivers/drv_hrt.h> // to get the real time #include <drivers/drv_hrt.h> // to get the real time
#include <perf/perf_counter.h> #include <perf/perf_counter.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
@@ -50,10 +50,8 @@
#include <uORB/topics/sensor_accel.h> #include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_mag.h> #include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth #include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth #include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
using namespace matrix;
extern "C" __EXPORT int sih_main(int argc, char *argv[]); extern "C" __EXPORT int sih_main(int argc, char *argv[]);
@@ -82,56 +80,55 @@ public:
/** @see ModuleBase::print_status() */ /** @see ModuleBase::print_status() */
int print_status() override; int print_status() override;
static float generate_wgn(); // generate white Gaussian noise sample static float generate_wgn(); // generate white Gaussian noise sample
// generate white Gaussian noise sample as a 3D vector with specified std // generate white Gaussian noise sample as a 3D vector with specified std
static Vector3f noiseGauss3f(float stdx, float stdy, float stdz); static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
// timer called periodically to post the semaphore // timer called periodically to post the semaphore
static void timer_callback(void *sem); static void timer_callback(void *sem);
// static int pack_float(char* uart_msg, int index, void *value); // pack a float to a IEEE754
private: private:
/** /**
* Check for parameter changes and update them if needed. * Check for parameter changes and update them if needed.
* @param parameter_update_sub uorb subscription to parameter_update * @param parameter_update_sub uorb subscription to parameter_update
* @param force for a parameter update * @param force for a parameter update
*/ */
void parameters_update_poll(); void parameters_update_poll();
void parameters_updated(); void parameters_updated();
// to publish the sensor baro // to publish the sensor baro
struct sensor_baro_s _sensor_baro {}; struct sensor_baro_s _sensor_baro {};
orb_advert_t _sensor_baro_pub{nullptr}; orb_advert_t _sensor_baro_pub{nullptr};
// to publish the sensor mag // to publish the sensor mag
struct sensor_mag_s _sensor_mag {}; struct sensor_mag_s _sensor_mag {};
orb_advert_t _sensor_mag_pub{nullptr}; orb_advert_t _sensor_mag_pub{nullptr};
// to publish the sensor gyroscope // to publish the sensor gyroscope
struct sensor_gyro_s _sensor_gyro {}; struct sensor_gyro_s _sensor_gyro {};
orb_advert_t _sensor_gyro_pub{nullptr}; orb_advert_t _sensor_gyro_pub{nullptr};
// to publish the sensor accelerometer // to publish the sensor accelerometer
struct sensor_accel_s _sensor_accel {}; struct sensor_accel_s _sensor_accel {};
orb_advert_t _sensor_accel_pub{nullptr}; orb_advert_t _sensor_accel_pub{nullptr};
// to publish the gps position // to publish the gps position
struct vehicle_gps_position_s _vehicle_gps_pos {}; struct vehicle_gps_position_s _vehicle_gps_pos {};
orb_advert_t _vehicle_gps_pos_pub{nullptr}; orb_advert_t _vehicle_gps_pos_pub{nullptr};
// attitude groundtruth // attitude groundtruth
struct vehicle_global_position_s _gpos_gt {}; struct vehicle_global_position_s _gpos_gt {};
orb_advert_t _gpos_gt_sub{nullptr}; orb_advert_t _gpos_gt_sub{nullptr};
// global position groundtruth // global position groundtruth
struct vehicle_attitude_s _att_gt {}; struct vehicle_attitude_s _att_gt {};
orb_advert_t _att_gt_sub{nullptr}; orb_advert_t _att_gt_sub{nullptr};
int _parameter_update_sub {-1}; int _parameter_update_sub {-1};
int _actuator_out_sub {-1}; int _actuator_out_sub {-1};
// hard constants // hard constants
static constexpr uint16_t NB_MOTORS = 4; static constexpr uint16_t NB_MOTORS = 4;
static constexpr float T1_C = 15.0f; // ground temperature in celcius 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 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 float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
static constexpr hrt_abstime LOOP_INTERVAL = 4000; // 4ms => 250 Hz real-time static constexpr hrt_abstime LOOP_INTERVAL = 4000; // 4ms => 250 Hz real-time
void init_variables(); void init_variables();
void init_sensors(); void init_sensors();
@@ -144,54 +141,54 @@ private:
void publish_sih(); void publish_sih();
void inner_loop(); void inner_loop();
perf_counter_t _loop_perf; perf_counter_t _loop_perf;
perf_counter_t _sampling_perf; perf_counter_t _sampling_perf;
px4_sem_t _data_semaphore; px4_sem_t _data_semaphore;
int32_t _counter = 0; int32_t _counter = 0;
hrt_call _timer_call; hrt_call _timer_call;
hrt_abstime _last_run; hrt_abstime _last_run;
hrt_abstime _gps_time; hrt_abstime _gps_time;
hrt_abstime _serial_time; hrt_abstime _serial_time;
hrt_abstime _now; hrt_abstime _now;
float _dt; // sampling time [s] float _dt; // sampling time [s]
Vector3f _T_B; // thrust force in body frame [N] matrix::Vector3f _T_B; // thrust force in body frame [N]
Vector3f _Fa_I; // aerodynamic force in inertial frame [N] matrix::Vector3f _Fa_I; // aerodynamic force in inertial frame [N]
Vector3f _Mt_B; // thruster moments in the body frame [Nm] matrix::Vector3f _Mt_B; // thruster moments in the body frame [Nm]
Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm] matrix::Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm]
Vector3f _p_I; // inertial position [m] matrix::Vector3f _p_I; // inertial position [m]
Vector3f _v_I; // inertial velocity [m/s] matrix::Vector3f _v_I; // inertial velocity [m/s]
Vector3f _v_B; // body frame velocity [m/s] matrix::Vector3f _v_B; // body frame velocity [m/s]
Vector3f _p_I_dot; // inertial position differential matrix::Vector3f _p_I_dot; // inertial position differential
Vector3f _v_I_dot; // inertial velocity differential matrix::Vector3f _v_I_dot; // inertial velocity differential
Quatf _q; // quaternion attitude matrix::Quatf _q; // quaternion attitude
Dcmf _C_IB; // body to inertial transformation matrix::Dcmf _C_IB; // body to inertial transformation
Vector3f _w_B; // body rates in body frame [rad/s] matrix::Vector3f _w_B; // body rates in body frame [rad/s]
Quatf _q_dot; // quaternion differential matrix::Quatf _q_dot; // quaternion differential
Vector3f _w_B_dot; // body rates differential matrix::Vector3f _w_B_dot; // body rates differential
float _u[NB_MOTORS]; // thruster signals float _u[NB_MOTORS]; // thruster signals
// sensors reconstruction // sensors reconstruction
Vector3f _acc; matrix::Vector3f _acc;
Vector3f _mag; matrix::Vector3f _mag;
Vector3f _gyro; matrix::Vector3f _gyro;
Vector3f _gps_vel; matrix::Vector3f _gps_vel;
double _gps_lat, _gps_lat_noiseless; double _gps_lat, _gps_lat_noiseless;
double _gps_lon, _gps_lon_noiseless; double _gps_lon, _gps_lon_noiseless;
float _gps_alt, _gps_alt_noiseless; float _gps_alt, _gps_alt_noiseless;
float _baro_p_mBar; // reconstructed (simulated) pressure in mBar float _baro_p_mBar; // reconstructed (simulated) pressure in mBar
float _baro_temp_c; // reconstructed (simulated) barometer temperature in celcius float _baro_temp_c; // reconstructed (simulated) barometer temperature in celcius
// parameters // parameters
float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0; float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0;
double _LAT0, _LON0, _COS_LAT0; double _LAT0, _LON0, _COS_LAT0;
Vector3f _W_I; // weight of the vehicle in inertial frame [N] matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N]
Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _I; // vehicle inertia matrix
Matrix3f _Im1; // inverse of the intertia matrix matrix::Matrix3f _Im1; // inverse of the intertia matrix
Vector3f _mu_I; // NED magnetic field in inertial frame [G] matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
// parameters defined in sih_params.c // parameters defined in sih_params.c
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
@@ -208,11 +205,11 @@ private:
(ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch, (ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch,
(ParamFloat<px4::params::SIH_KDV>) _sih_kdv, (ParamFloat<px4::params::SIH_KDV>) _sih_kdv,
(ParamFloat<px4::params::SIH_KDW>) _sih_kdw, (ParamFloat<px4::params::SIH_KDW>) _sih_kdw,
(ParamInt<px4::params::SIH__LAT0>) _sih_lat0, (ParamInt<px4::params::SIH_LOC_LAT0>) _sih_lat0,
(ParamInt<px4::params::SIH__LON0>) _sih_lon0, (ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
(ParamFloat<px4::params::SIH__H0>) _sih_h0, (ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
(ParamFloat<px4::params::SIH__MU_X>) _sih_mu_x, (ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
(ParamFloat<px4::params::SIH__MU_Y>) _sih_mu_y, (ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
(ParamFloat<px4::params::SIH__MU_Z>) _sih_mu_z (ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z
) )
}; };
src/modules/sih/sih_params.c
+6 -6
View File
@@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
* @max 850000000 * @max 850000000
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_INT32(SIH__LAT0, 454671160); PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160);
/** /**
* Initial geodetic longitude * Initial geodetic longitude
@@ -261,7 +261,7 @@ PARAM_DEFINE_INT32(SIH__LAT0, 454671160);
* @max 1800000000 * @max 1800000000
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_INT32(SIH__LON0, -737578370); PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
/** /**
* Initial AMSL ground altitude * Initial AMSL ground altitude
@@ -282,7 +282,7 @@ PARAM_DEFINE_INT32(SIH__LON0, -737578370);
* @increment 0.01 * @increment 0.01
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_FLOAT(SIH__H0, 32.34f); PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
/** /**
* North magnetic field at the initial location * North magnetic field at the initial location
@@ -302,7 +302,7 @@ PARAM_DEFINE_FLOAT(SIH__H0, 32.34f);
* @increment 0.001 * @increment 0.001
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_FLOAT(SIH__MU_X, 0.179f); PARAM_DEFINE_FLOAT(SIH_LOC_MU_X, 0.179f);
/** /**
* East magnetic field at the initial location * East magnetic field at the initial location
@@ -322,7 +322,7 @@ PARAM_DEFINE_FLOAT(SIH__MU_X, 0.179f);
* @increment 0.001 * @increment 0.001
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_FLOAT(SIH__MU_Y, -0.045f); PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
/** /**
* Down magnetic field at the initial location * Down magnetic field at the initial location
@@ -342,4 +342,4 @@ PARAM_DEFINE_FLOAT(SIH__MU_Y, -0.045f);
* @increment 0.001 * @increment 0.001
* @group Simulation In Hardware * @group Simulation In Hardware
*/ */
PARAM_DEFINE_FLOAT(SIH__MU_Z, 0.504f); PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f);