diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index 837a3a2161..090c16b223 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -5,49 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - -# -# LPE -# -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_apps b/ROMFS/px4fmu_common/init.d/rc.balloon_apps index 2adb216ec1..7f42a3b27e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_apps +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_apps @@ -4,39 +4,3 @@ # # NOTE: Script variables are declared/initialized/unset in the rcS script. # - -# -# Start the attitude and position estimator. -# - -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index f3c559298e..81af5e1acd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -5,11 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -# -# Start the attitude and position estimator. -# -ekf2 start & - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8158ab8b03..39d62c3fe5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -9,45 +9,6 @@ # Begin Estimator Group Selection # ############################################################################### -# -# LPE -# -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index 4d04b53faa..24904ca064 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -5,13 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -# -# Start the attitude and position estimator. -# -ekf2 start & -#attitude_estimator_q start -#local_position_estimator start - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps index d2eb13208c..f604b64452 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -5,16 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - -ekf2 start & - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 4f8497bc19..a5fb089048 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -5,6 +5,41 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # +# Start the attitude and position estimator. +if param compare -s SYS_MC_EST_GROUP 1 +then + # + # Try to start LPE. If it fails, start EKF2 as a default. + # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. + # + if attitude_estimator_q start + then + echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." + local_position_estimator start + else + echo "ERROR [init] Estimator LPE not available. Using EKF2" + param set SYS_MC_EST_GROUP 2 + param save + reboot + fi +else + # Q estimator (attitude estimation only) + if param compare -s SYS_MC_EST_GROUP 3 + then + attitude_estimator_q start + fi +fi + +if param compare -s SYS_MC_EST_GROUP 2 +then + param set EKF2_EN 1 +fi + +if param compare -s EKF2_EN 1 +then + ekf2 start & +fi + # # Fixed wing setup. # @@ -67,5 +102,4 @@ fi if [ $VEHICLE_TYPE = none ] then echo "No autostart ID found" - ekf2 start & fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index fdc16af83d..c28adef56a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -5,16 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator group selection # -############################################################################### - -ekf2 start & - -############################################################################### -# End Estimator group selection # -############################################################################### - # # Start Control Allocator # diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 066fb52e1e..8c3d5bd49e 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -79,7 +79,6 @@ CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 8b3bc95145..21a0bafe9c 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -38,9 +38,15 @@ #include +using matrix::Vector2f; + VectorNav::VectorNav(const char *port) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _attitude_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_attitude) : ORB_ID(vehicle_attitude)), + _local_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_local_position) : ORB_ID(vehicle_local_position)), + _global_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_global_position) : ORB_ID( + vehicle_global_position)) { // store port name strncpy(_port, port, sizeof(_port) - 1); @@ -48,6 +54,27 @@ VectorNav::VectorNav(const char *port) : // enforce null termination _port[sizeof(_port) - 1] = '\0'; + // VN_MODE 1 full INS + if (_param_vn_mode.get() == 1) { + int32_t v = 0; + + // EKF2_EN 0 (disabled) + v = 0; + param_set(param_find("EKF2_EN"), &v); + + // SYS_MC_EST_GROUP 0 (disabled) + v = 0; + param_set(param_find("SYS_MC_EST_GROUP"), &v); + + // SENS_IMU_MODE (VN handles sensor selection) + v = 0; + param_set(param_find("SENS_IMU_MODE"), &v); + + // SENS_MAG_MODE (VN handles sensor selection) + v = 0; + param_set(param_find("SENS_MAG_MODE"), &v); + } + device::Device::DeviceId device_id{}; device_id.devid_s.devtype = DRV_INS_DEVTYPE_VN300; device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; @@ -108,7 +135,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) if (VnUartPacket_isCompatible(packet, COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -118,10 +145,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet) uint64_t time_startup = VnUartPacket_extractUint64(packet); (void)time_startup; - // IMUGROUP_UNCOMPACCEL + // IMUGROUP_ACCEL vec3f accel = VnUartPacket_extractVec3f(packet); - // IMUGROUP_UNCOMPGYRO + // IMUGROUP_ANGULARRATE vec3f angular_rate = VnUartPacket_extractVec3f(packet); // publish sensor_accel @@ -129,6 +156,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet) // publish sensor_gyro _px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]); + + _time_last_valid_imu_us.store(hrt_absolute_time()); } // binary output 2 @@ -232,24 +261,59 @@ void VectorNav::sensorCallback(VnUartPacket *packet) if ((mode_aligning || mode_tracking) && !mode_loss_gnss) { // publish local_position - // TODO: projection + const double lat = positionEstimatedLla.c[0]; + const double lon = positionEstimatedLla.c[1]; + const float alt = positionEstimatedLla.c[2]; + + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(lat, lon, time_now_us); + _gps_alt_ref = alt; + } + + const Vector2f pos_ned = _pos_ref.project(lat, lon); + vehicle_local_position_s local_position{}; local_position.timestamp_sample = time_now_us; - local_position.ax = accelerationLinearNed.c[0]; - local_position.ay = accelerationLinearNed.c[1]; - local_position.az = accelerationLinearNed.c[2]; - local_position.x = positionEstimatedLla.c[0]; // TODO - local_position.y = positionEstimatedLla.c[1]; // TODO - local_position.z = positionEstimatedLla.c[2]; // TODO + + local_position.xy_valid = !mode_loss_gnss; + local_position.z_valid = !mode_loss_gnss; + local_position.v_xy_valid = !mode_loss_gnss; + local_position.v_z_valid = !mode_loss_gnss; + + local_position.x = pos_ned(0); + local_position.y = pos_ned(1); + local_position.z = -(alt - _gps_alt_ref); + local_position.vx = velocityEstimatedNed.c[0]; local_position.vy = velocityEstimatedNed.c[1]; local_position.vz = velocityEstimatedNed.c[2]; + local_position.z_deriv = velocityEstimatedNed.c[2]; // TODO + + local_position.ax = accelerationLinearNed.c[0]; + local_position.ay = accelerationLinearNed.c[1]; + local_position.az = accelerationLinearNed.c[2]; + + matrix::Quatf q{attitude.q}; + local_position.heading = matrix::Eulerf{q}.psi(); + local_position.heading_good_for_control = mode_tracking; + + local_position.xy_global = mode_loss_gnss; + local_position.z_global = mode_loss_gnss; + + local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position.ref_lat = _pos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _pos_ref.getProjectionReferenceLon(); + local_position.ref_alt = _gps_alt_ref; + + local_position.dist_bottom_valid = false; + local_position.eph = positionUncertaintyEstimated; local_position.epv = positionUncertaintyEstimated; local_position.evh = velocityUncertaintyEstimated; local_position.evv = velocityUncertaintyEstimated; - local_position.xy_valid = true; - local_position.heading_good_for_control = mode_tracking; + + local_position.dead_reckoning = mode_loss_gnss; + local_position.timestamp = hrt_absolute_time(); _local_position_pub.publish(local_position); @@ -258,9 +322,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet) // publish global_position vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = positionEstimatedLla.c[0]; - global_position.lon = positionEstimatedLla.c[1]; - global_position.alt = positionEstimatedLla.c[2]; + global_position.lat = lat; + global_position.lon = lon; + global_position.alt = alt; + global_position.alt = alt; + + global_position.eph = positionUncertaintyEstimated; + global_position.epv = positionUncertaintyEstimated; + global_position.timestamp = hrt_absolute_time(); _global_position_pub.publish(global_position); } @@ -333,7 +402,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.hdop = dop.hDOP; sensor_gps.vdop = dop.vDOP; - sensor_gps.eph = positionUncertaintyGpsNed.c[0]; + sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1])); sensor_gps.epv = positionUncertaintyGpsNed.c[2]; sensor_gps.s_variance_m_s = velocityUncertaintyGps; @@ -493,7 +562,7 @@ bool VectorNav::configure() 1, // divider COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -551,6 +620,8 @@ bool VectorNav::configure() VnSensor_registerAsyncPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); VnSensor_registerErrorPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); + _time_configured_us.store(hrt_absolute_time()); + return true; } @@ -582,11 +653,42 @@ void VectorNav::Run() _initialized = true; } else { - ScheduleDelayed(3_s); + ScheduleDelayed(1_s); return; } } + if (_connected && _configured && _initialized) { + + // check for timeout + const hrt_abstime time_configured_us = _time_configured_us.load(); + const hrt_abstime time_last_valid_imu_us = _time_last_valid_imu_us.load(); + + if (_param_vn_mode.get() == 1) { + if ((time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) < 3_s)) + + // update sensor_selection if configured in INS mode + if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) { + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _px4_accel.get_device_id(); + sensor_selection.gyro_device_id = _px4_gyro.get_device_id(); + sensor_selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(sensor_selection); + } + } + + if ((time_configured_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 5_s) + && (time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 1_s) + ) { + PX4_ERR("timeout, reinitializing"); + VnSensor_unregisterAsyncPacketReceivedHandler(&_vs); + VnSensor_disconnect(&_vs); + _connected = false; + _configured = false; + _initialized = false; + } + } + ScheduleDelayed(100_ms); } diff --git a/src/drivers/ins/vectornav/VectorNav.hpp b/src/drivers/ins/vectornav/VectorNav.hpp index bc1244a009..fe69c91dab 100644 --- a/src/drivers/ins/vectornav/VectorNav.hpp +++ b/src/drivers/ins/vectornav/VectorNav.hpp @@ -56,6 +56,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -99,6 +101,9 @@ private: static void binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex); + // return the square of two floating point numbers + static constexpr float sq(float var) { return var * var; } + void sensorCallback(VnUartPacket *packet); char _port[20] {}; @@ -108,7 +113,8 @@ private: bool _connected{false}; bool _configured{false}; - hrt_abstime _last_read{0}; + px4::atomic _time_configured_us{false}; + px4::atomic _time_last_valid_imu_us{false}; VnSensor _vs{}; @@ -122,12 +128,17 @@ private: PX4Gyroscope _px4_gyro{0}; PX4Magnetometer _px4_mag{0}; + MapProjection _pos_ref{}; + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - uORB::PublicationMulti _attitude_pub{ORB_ID(external_ins_attitude)}; - uORB::PublicationMulti _local_position_pub{ORB_ID(external_ins_local_position)}; - uORB::PublicationMulti _global_position_pub{ORB_ID(external_ins_global_position)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMulti _local_position_pub; + uORB::PublicationMulti _global_position_pub; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; @@ -154,4 +165,8 @@ private: // VN_GNSS_ANTB_POS_X // Uncertainty in the X-axis position measurement. + + DEFINE_PARAMETERS( + (ParamInt) _param_vn_mode + ) }; diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c index 00b735982b..2bfd85f542 100644 --- a/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c +++ b/src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c @@ -234,7 +234,7 @@ void VnCompositeData_processBinaryPacketImuGroup(VnCompositeData* compositeData, if (imuGroup & IMUGROUP_UNCOMPMAG) compositeData->magneticUncompensated = VnUartPacket_extractVec3f(packet); - if (imuGroup & IMUGROUP_UNCOMPACCEL) + if (imuGroup & IMUGROUP_ACCEL) compositeData->accelerationUncompensated = VnUartPacket_extractVec3f(packet); if (imuGroup & IMUGROUP_UNCOMPGYRO) diff --git a/src/drivers/ins/vectornav/module.yaml b/src/drivers/ins/vectornav/module.yaml index c95bfeb608..349fb9869b 100644 --- a/src/drivers/ins/vectornav/module.yaml +++ b/src/drivers/ins/vectornav/module.yaml @@ -1,7 +1,22 @@ module_name: VectorNav (VN-100, VN-200, VN-300) + serial_config: - command: vectornav start -d ${SERIAL_DEV} port_config_param: name: SENS_VN_CFG group: Sensors +parameters: + - group: Sensors + definitions: + + VN_MODE: + description: + short: VectorNav driver mode + long: INS or sensors + category: System + type: enum + values: + 0: Sensors Only (default) + 1: INS + default: 0 diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index f019b6f2d0..f318e43e01 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -88,6 +88,7 @@ PARAM_DEFINE_INT32(SYS_HITL, 0); * * Set the group of estimators used for multicopters and VTOLs * + * @value 0 disabled (onboard estimators not started) * @value 1 local_position_estimator, attitude_estimator_q (unsupported) * @value 2 ekf2 (recommended) * @value 3 Q attitude estimator (no position) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 0e3e21b3ea..bd84e733df 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -39,6 +39,14 @@ * */ +/** + * EKF2 enable + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_EN, 1); + /** * EKF prediction period *