dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[WIP] VectorNav full INS with onboard (ekf2, etc) disabled

This commit is contained in:
Daniel Agar 2023-04-17 11:06:39 -04:00
parent a11cced4b2
commit 2d5d2a929c
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
15 changed files with 200 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
ROMFS/px4fmu_common/init.d/rc.airship_apps
View File

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

36
ROMFS/px4fmu_common/init.d/rc.balloon_apps
View File

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

5
ROMFS/px4fmu_common/init.d/rc.fw_apps
View File

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

39
ROMFS/px4fmu_common/init.d/rc.mc_apps
View File

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

7
ROMFS/px4fmu_common/init.d/rc.rover_apps
View File

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

10
ROMFS/px4fmu_common/init.d/rc.uuv_apps
View File

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

36
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View File

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

10
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

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

1
boards/px4/fmu-v5x/default.px4board
View File

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

140
src/drivers/ins/vectornav/VectorNav.cpp
View File

@ -38,9 +38,15 @@
#include <fcntl.h>
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);
}

23
src/drivers/ins/vectornav/VectorNav.hpp
View File

@ -56,6 +56,7 @@ extern "C" {
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
@ -65,6 +66,7 @@ extern "C" {
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
@ -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<hrt_abstime> _time_configured_us{false};
px4::atomic<hrt_abstime> _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_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub{ORB_ID(external_ins_attitude)};
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub{ORB_ID(external_ins_local_position)};
uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub{ORB_ID(external_ins_global_position)};
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
uORB::PublicationMulti<vehicle_global_position_s> _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<px4::params::VN_MODE>) _param_vn_mode
)
};

2
src/drivers/ins/vectornav/libvnc/src/vn/sensors/compositedata.c
View File

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

15
src/drivers/ins/vectornav/module.yaml
View File

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

1
src/lib/systemlib/system_params.c
View File

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

8
src/modules/ekf2/ekf2_params.c
View File

@ -39,6 +39,14 @@
*
*/
/**
* EKF2 enable
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_EN, 1);
/**
* EKF prediction period
*