Compare commits

...

2 Commits

Author SHA1 Message Date
Daniel Agar 920dd62918
vectornav fix local position and add perf counts 2023-05-19 16:17:02 -04:00
Daniel Agar 2d5d2a929c
[WIP] VectorNav full INS with onboard (ekf2, etc) disabled 2023-04-17 14:01:31 -04:00
15 changed files with 236 additions and 180 deletions

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
#

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

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
#

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
#

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
#

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
#

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

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
#

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

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;
@ -75,6 +102,16 @@ VectorNav::~VectorNav()
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_accel_pub_interval_perf);
perf_free(_gyro_pub_interval_perf);
perf_free(_mag_pub_interval_perf);
perf_free(_gnss_pub_interval_perf);
perf_free(_baro_pub_interval_perf);
perf_free(_attitude_pub_interval_perf);
perf_free(_local_position_pub_interval_perf);
perf_free(_global_position_pub_interval_perf);
}
void VectorNav::binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex)
@ -108,7 +145,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,17 +155,21 @@ 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
_px4_accel.update(time_now_us, accel.c[0], accel.c[1], accel.c[2]);
perf_count(_accel_pub_interval_perf);
// publish sensor_gyro
_px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]);
perf_count(_gyro_pub_interval_perf);
_time_last_valid_imu_us.store(hrt_absolute_time());
}
// binary output 2
@ -188,9 +229,11 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
sensor_baro.temperature = temperature;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
perf_count(_baro_pub_interval_perf);
// publish sensor_mag
_px4_mag.update(time_now_us, mag.c[0], mag.c[1], mag.c[2]);
perf_count(_mag_pub_interval_perf);
// publish attitude
vehicle_attitude_s attitude{};
@ -201,13 +244,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
attitude.q[3] = quaternion.c[2];
attitude.timestamp = hrt_absolute_time();
_attitude_pub.publish(attitude);
perf_count(_attitude_pub_interval_perf);
// mode
const uint16_t mode = (insStatus & 0b11);
//const bool mode_initializing = (mode == 0b00);
const bool mode_aligning = (mode == 0b01);
const bool mode_tracking = (mode == 0b10);
const bool mode_loss_gnss = (mode == 0b11);
//const bool mode_loss_gnss = (mode == 0b11);
// mode_initializing
@ -228,41 +272,87 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
// - attitude is good
// - treat as mode 0
if ((mode_aligning || mode_tracking) && !mode_loss_gnss) {
if (mode_aligning || mode_tracking) {
// 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 = true;
local_position.z_valid = true;
local_position.v_xy_valid = true;
local_position.v_z_valid = true;
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;
if (_pos_ref.isInitialized()) {
local_position.xy_global = true;
local_position.z_global = true;
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 = false;
local_position.vxy_max = INFINITY;
local_position.vz_max = INFINITY;
local_position.hagl_min = INFINITY;
local_position.hagl_max = INFINITY;
local_position.timestamp = hrt_absolute_time();
_local_position_pub.publish(local_position);
perf_count(_local_position_pub_interval_perf);
// 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);
perf_count(_global_position_pub_interval_perf);
}
}
@ -333,13 +423,14 @@ 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;
sensor_gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(sensor_gps);
perf_count(_gnss_pub_interval_perf);
}
}
}
@ -493,7 +584,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 +642,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 +675,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);
}

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,16 +128,31 @@ 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")};
perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel publish interval")};
perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro publish interval")};
perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": mag publish interval")};
perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": GNSS publish interval")};
perf_counter_t _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": baro publish interval")};
perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": attitude publish interval")};
perf_counter_t _local_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": local position publish interval")};
perf_counter_t _global_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": global position publish interval")};
// TODO: params for GNSS antenna offsets
// A
@ -154,4 +175,8 @@ private:
// VN_GNSS_ANTB_POS_X
// Uncertainty in the X-axis position measurement.
DEFINE_PARAMETERS(
(ParamInt<px4::params::VN_MODE>) _param_vn_mode
)
};

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)

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

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)

View File

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