forked from Archive/PX4-Autopilot
Merge pull request #2361 from TSC21/mocap_support_restruct
MOCAP support on firmware [new PR]
This commit is contained in:
Lorenz Meier
commit
bc5cf50f1a
|
|
@ -0,0 +1,10 @@
|
|||
uint32 id # ID of the estimator, commonly the component ID of the incoming message
|
||||
|
||||
uint64 timestamp_boot # time of this estimate, in microseconds since system start
|
||||
uint64 timestamp_computer # timestamp provided by the companion computer, in us
|
||||
|
||||
float32[4] q # Estimated attitude as quaternion
|
||||
|
||||
float32 x # X position in meters in NED earth-fixed frame
|
||||
float32 y # Y position in meters in NED earth-fixed frame
|
||||
float32 z # Z position in meters in NED earth-fixed frame (negative altitude)
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
uint64 timestamp # time of this estimate, in microseconds since system start
|
||||
bool valid # true if position satisfies validity criteria of estimator
|
||||
|
||||
float32 x # X position in meters in NED earth-fixed frame
|
||||
float32 y # Y position in meters in NED earth-fixed frame
|
||||
float32 z # Z position in meters in NED earth-fixed frame (negative altitude)
|
||||
float32 roll
|
||||
float32 pitch
|
||||
float32 yaw
|
||||
float32[4] q # Attitude as quaternion
|
||||
|
|
@ -63,6 +63,7 @@
|
|||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
|
@ -261,6 +262,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
/* subscribe to vision estimate */
|
||||
int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
|
||||
|
||||
/* subscribe to mocap data */
|
||||
int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
|
||||
|
||||
/* advertise attitude */
|
||||
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||
|
||||
|
|
@ -291,6 +295,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
R_decl.identity();
|
||||
|
||||
struct vision_position_estimate_s vision {};
|
||||
struct att_pos_mocap_s mocap {};
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
|
||||
|
|
@ -445,11 +450,30 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
|||
bool vision_updated = false;
|
||||
orb_check(vision_sub, &vision_updated);
|
||||
|
||||
bool mocap_updated = false;
|
||||
orb_check(mocap_sub, &mocap_updated);
|
||||
|
||||
if (vision_updated) {
|
||||
orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
|
||||
}
|
||||
|
||||
if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
|
||||
if (mocap_updated) {
|
||||
orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
|
||||
}
|
||||
|
||||
if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {
|
||||
|
||||
math::Quaternion q(mocap.q);
|
||||
math::Matrix<3, 3> Rmoc = q.to_dcm();
|
||||
|
||||
math::Vector<3> v(1.0f, 0.0f, 0.4f);
|
||||
|
||||
math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
|
||||
// Rrw * Vw = vn. This way we have consistency
|
||||
z_k[6] = vn(0);
|
||||
z_k[7] = vn(1);
|
||||
z_k[8] = vn(2);
|
||||
}else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
|
||||
|
||||
math::Quaternion q(vision.q);
|
||||
math::Matrix<3, 3> Rvis = q.to_dcm();
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
|
|
@ -1199,64 +1199,65 @@ protected:
|
|||
};
|
||||
|
||||
|
||||
class MavlinkStreamViconPositionEstimate : public MavlinkStream
|
||||
class MavlinkStreamAttPosMocap : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkStreamViconPositionEstimate::get_name_static();
|
||||
return MavlinkStreamAttPosMocap::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "VICON_POSITION_ESTIMATE";
|
||||
return "ATT_POS_MOCAP";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
|
||||
return MAVLINK_MSG_ID_ATT_POS_MOCAP;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamViconPositionEstimate(mavlink);
|
||||
return new MavlinkStreamAttPosMocap(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size()
|
||||
{
|
||||
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *_pos_sub;
|
||||
uint64_t _pos_time;
|
||||
MavlinkOrbSubscription *_mocap_sub;
|
||||
uint64_t _mocap_time;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
|
||||
MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
|
||||
MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &);
|
||||
MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &);
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))),
|
||||
_pos_time(0)
|
||||
explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_mocap_sub(_mavlink->add_orb_subscription(ORB_ID(att_pos_mocap))),
|
||||
_mocap_time(0)
|
||||
{}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
struct vehicle_vicon_position_s pos;
|
||||
struct att_pos_mocap_s mocap;
|
||||
|
||||
if (_pos_sub->update(&_pos_time, &pos)) {
|
||||
mavlink_vicon_position_estimate_t msg;
|
||||
if (_mocap_sub->update(&_mocap_time, &mocap)) {
|
||||
mavlink_att_pos_mocap_t msg;
|
||||
|
||||
msg.usec = pos.timestamp;
|
||||
msg.x = pos.x;
|
||||
msg.y = pos.y;
|
||||
msg.z = pos.z;
|
||||
msg.roll = pos.roll;
|
||||
msg.pitch = pos.pitch;
|
||||
msg.yaw = pos.yaw;
|
||||
msg.time_usec = mocap.timestamp_boot;
|
||||
msg.q[0] = mocap.q[0];
|
||||
msg.q[1] = mocap.q[1];
|
||||
msg.q[2] = mocap.q[2];
|
||||
msg.q[3] = mocap.q[3];
|
||||
msg.x = mocap.x;
|
||||
msg.y = mocap.y;
|
||||
msg.z = mocap.z;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -2293,7 +2294,7 @@ const StreamListItem *streams_list[] = {
|
|||
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
|
||||
|
|
|
|||
|
|
@ -118,7 +118,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_rates_sp_pub(nullptr),
|
||||
_force_sp_pub(nullptr),
|
||||
_pos_sp_triplet_pub(nullptr),
|
||||
_vicon_position_pub(nullptr),
|
||||
_att_pos_mocap_pub(nullptr),
|
||||
_vision_position_pub(nullptr),
|
||||
_telemetry_status_pub(nullptr),
|
||||
_rc_pub(nullptr),
|
||||
|
|
@ -169,8 +169,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_set_mode(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
|
||||
handle_message_vicon_position_estimate(msg);
|
||||
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
|
||||
handle_message_att_pos_mocap(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
||||
|
|
@ -560,27 +560,34 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
|
||||
MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_vicon_position_estimate_t pos;
|
||||
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
|
||||
mavlink_att_pos_mocap_t mocap;
|
||||
mavlink_msg_att_pos_mocap_decode(msg, &mocap);
|
||||
|
||||
struct vehicle_vicon_position_s vicon_position;
|
||||
memset(&vicon_position, 0, sizeof(vicon_position));
|
||||
struct att_pos_mocap_s att_pos_mocap;
|
||||
memset(&att_pos_mocap, 0, sizeof(att_pos_mocap));
|
||||
|
||||
vicon_position.timestamp = hrt_absolute_time();
|
||||
vicon_position.x = pos.x;
|
||||
vicon_position.y = pos.y;
|
||||
vicon_position.z = pos.z;
|
||||
vicon_position.roll = pos.roll;
|
||||
vicon_position.pitch = pos.pitch;
|
||||
vicon_position.yaw = pos.yaw;
|
||||
// Use the component ID to identify the mocap system
|
||||
att_pos_mocap.id = msg->compid;
|
||||
|
||||
if (_vicon_position_pub == nullptr) {
|
||||
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
||||
att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time
|
||||
att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
|
||||
|
||||
att_pos_mocap.q[0] = mocap.q[0];
|
||||
att_pos_mocap.q[1] = mocap.q[1];
|
||||
att_pos_mocap.q[2] = mocap.q[2];
|
||||
att_pos_mocap.q[3] = mocap.q[3];
|
||||
|
||||
att_pos_mocap.x = mocap.x;
|
||||
att_pos_mocap.y = mocap.y;
|
||||
att_pos_mocap.z = mocap.z;
|
||||
|
||||
if (_att_pos_mocap_pub == nullptr) {
|
||||
_att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
|
||||
orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@
|
|||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
|
|
@ -119,7 +119,7 @@ private:
|
|||
void handle_message_optical_flow_rad(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_set_mode(mavlink_message_t *msg);
|
||||
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_att_pos_mocap(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
|
||||
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
|
||||
|
|
@ -175,7 +175,7 @@ private:
|
|||
orb_advert_t _rates_sp_pub;
|
||||
orb_advert_t _force_sp_pub;
|
||||
orb_advert_t _pos_sp_triplet_pub;
|
||||
orb_advert_t _vicon_position_pub;
|
||||
orb_advert_t _att_pos_mocap_pub;
|
||||
orb_advert_t _vision_position_pub;
|
||||
orb_advert_t _telemetry_status_pub;
|
||||
orb_advert_t _rc_pub;
|
||||
|
|
|
|||
|
|
@ -42,5 +42,5 @@ SRCS = position_estimator_inav_main.c \
|
|||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
EXTRACFLAGS = -Wframe-larger-than=3500
|
||||
EXTRACFLAGS = -Wframe-larger-than=3800
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
* Model-identification based position estimator for multirotors
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Nuno Marques <n.marques21@hotmail.com>
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
|
|
@ -63,6 +64,7 @@
|
|||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
|
@ -86,6 +88,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */
|
|||
static bool inav_verbose_mode = false;
|
||||
|
||||
static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s
|
||||
static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s
|
||||
static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s
|
||||
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
|
||||
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
|
||||
|
|
@ -184,7 +187,9 @@ int position_estimator_inav_main(int argc, char *argv[])
|
|||
return 1;
|
||||
}
|
||||
|
||||
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
|
||||
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2],
|
||||
float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p,
|
||||
float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v)
|
||||
{
|
||||
return;
|
||||
FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a");
|
||||
|
|
@ -196,10 +201,14 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
|
|||
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
|
||||
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
|
||||
fwrite(s, 1, n, f);
|
||||
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
|
||||
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n",
|
||||
(double)acc[0], (double)acc[1], (double)acc[2],
|
||||
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
|
||||
(double)w_xy_gps_p, (double)w_xy_gps_v);
|
||||
(double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], (double)w_mocap_p);
|
||||
fwrite(s, 1, n, f);
|
||||
n = snprintf(s, 256, "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n",
|
||||
(double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], (double)corr_vision[1][1], (double)corr_vision[2][1],
|
||||
(double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v);
|
||||
fwrite(s, 1, n, f);
|
||||
free(s);
|
||||
}
|
||||
|
|
@ -239,6 +248,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
float eph_vision = 0.2f;
|
||||
float epv_vision = 0.2f;
|
||||
|
||||
float eph_mocap = 0.05f;
|
||||
float epv_mocap = 0.05f;
|
||||
|
||||
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
|
||||
memset(x_est_prev, 0, sizeof(x_est_prev));
|
||||
memset(y_est_prev, 0, sizeof(y_est_prev));
|
||||
|
|
@ -265,6 +277,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
uint16_t gps_updates = 0;
|
||||
uint16_t attitude_updates = 0;
|
||||
uint16_t flow_updates = 0;
|
||||
uint16_t vision_updates = 0;
|
||||
uint16_t mocap_updates = 0;
|
||||
|
||||
hrt_abstime updates_counter_start = hrt_absolute_time();
|
||||
hrt_abstime pub_last = hrt_absolute_time();
|
||||
|
|
@ -289,6 +303,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
{ 0.0f, 0.0f }, // D (pos, vel)
|
||||
};
|
||||
|
||||
float corr_mocap[3][1] = {
|
||||
{ 0.0f }, // N (pos)
|
||||
{ 0.0f }, // E (pos)
|
||||
{ 0.0f }, // D (pos)
|
||||
};
|
||||
|
||||
float corr_sonar = 0.0f;
|
||||
float corr_sonar_filtered = 0.0f;
|
||||
|
||||
|
|
@ -304,7 +324,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
bool sonar_valid = false; // sonar is valid
|
||||
bool flow_valid = false; // flow is valid
|
||||
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
|
||||
bool vision_valid = false;
|
||||
bool vision_valid = false; // vision is valid
|
||||
bool mocap_valid = false; // mocap is valid
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct actuator_controls_s actuator;
|
||||
|
|
@ -325,6 +346,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
memset(&flow, 0, sizeof(flow));
|
||||
struct vision_position_estimate_s vision;
|
||||
memset(&vision, 0, sizeof(vision));
|
||||
struct att_pos_mocap_s mocap;
|
||||
memset(&mocap, 0, sizeof(mocap));
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
|
||||
|
|
@ -337,6 +360,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
|
||||
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
|
||||
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
|
||||
int home_position_sub = orb_subscribe(ORB_ID(home_position));
|
||||
|
||||
/* advertise */
|
||||
|
|
@ -697,9 +721,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
corr_vision[2][1] = 0.0f - z_est[1];
|
||||
}
|
||||
|
||||
vision_updates++;
|
||||
}
|
||||
}
|
||||
|
||||
/* vehicle mocap position */
|
||||
orb_check(att_pos_mocap_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
|
||||
|
||||
/* reset position estimate on first mocap update */
|
||||
if (!mocap_valid) {
|
||||
x_est[0] = mocap.x;
|
||||
y_est[0] = mocap.y;
|
||||
z_est[0] = mocap.z;
|
||||
|
||||
mocap_valid = true;
|
||||
|
||||
warnx("MOCAP data valid");
|
||||
mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
|
||||
}
|
||||
|
||||
/* calculate correction for position */
|
||||
corr_mocap[0][0] = mocap.x - x_est[0];
|
||||
corr_mocap[1][0] = mocap.y - y_est[0];
|
||||
corr_mocap[2][0] = mocap.z - z_est[0];
|
||||
|
||||
mocap_updates++;
|
||||
}
|
||||
|
||||
/* vehicle GPS position */
|
||||
orb_check(vehicle_gps_position_sub, &updated);
|
||||
|
||||
|
|
@ -830,6 +881,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
|
||||
}
|
||||
|
||||
/* check for timeout on mocap topic */
|
||||
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
|
||||
mocap_valid = false;
|
||||
warnx("MOCAP timeout");
|
||||
mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
|
||||
}
|
||||
|
||||
/* check for sonar measurement timeout */
|
||||
if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
|
||||
corr_sonar = 0.0f;
|
||||
|
|
@ -854,10 +912,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
/* use VISION if it's valid and has a valid weight parameter */
|
||||
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
|
||||
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
|
||||
/* use MOCAP if it's valid and has a valid weight parameter */
|
||||
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
|
||||
/* use flow if it's valid and (accurate or no GPS available) */
|
||||
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
|
||||
|
||||
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
|
||||
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
|
||||
|
||||
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
|
||||
|
||||
|
|
@ -881,6 +941,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
float w_xy_vision_v = params.w_xy_vision_v;
|
||||
float w_z_vision_p = params.w_z_vision_p;
|
||||
|
||||
float w_mocap_p = params.w_mocap_p;
|
||||
|
||||
/* reduce GPS weight if optical flow is good */
|
||||
if (use_flow && flow_accurate) {
|
||||
w_xy_gps_p *= params.w_gps_flow;
|
||||
|
|
@ -938,6 +1000,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
|
||||
}
|
||||
|
||||
/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
|
||||
accel_bias_corr[0] = 0.0f;
|
||||
accel_bias_corr[1] = 0.0f;
|
||||
accel_bias_corr[2] = 0.0f;
|
||||
|
||||
if (use_mocap) {
|
||||
accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
|
||||
accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
|
||||
accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
|
||||
}
|
||||
|
||||
/* transform error vector from NED frame to body frame */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
float c = 0.0f;
|
||||
|
|
@ -980,7 +1053,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
inertial_filter_predict(dt, z_est, acc[2]);
|
||||
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
|
||||
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
|
||||
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
|
||||
memcpy(z_est, z_est_prev, sizeof(z_est));
|
||||
}
|
||||
|
||||
|
|
@ -999,11 +1074,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
|
||||
}
|
||||
|
||||
if (use_mocap) {
|
||||
epv = fminf(epv, epv_mocap);
|
||||
inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
|
||||
}
|
||||
|
||||
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
|
||||
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
|
||||
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
|
||||
memcpy(z_est, z_est_prev, sizeof(z_est));
|
||||
memset(corr_gps, 0, sizeof(corr_gps));
|
||||
memset(corr_vision, 0, sizeof(corr_vision));
|
||||
memset(corr_mocap, 0, sizeof(corr_mocap));
|
||||
corr_baro = 0;
|
||||
|
||||
} else {
|
||||
|
|
@ -1016,7 +1099,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
inertial_filter_predict(dt, y_est, acc[1]);
|
||||
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
|
||||
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
|
||||
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
|
||||
memcpy(x_est, x_est_prev, sizeof(x_est));
|
||||
memcpy(y_est, y_est_prev, sizeof(y_est));
|
||||
}
|
||||
|
|
@ -1053,12 +1138,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
if (use_mocap) {
|
||||
eph = fminf(eph, eph_mocap);
|
||||
|
||||
inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
|
||||
inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
|
||||
}
|
||||
|
||||
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
|
||||
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
|
||||
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
|
||||
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
|
||||
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
|
||||
memcpy(x_est, x_est_prev, sizeof(x_est));
|
||||
memcpy(y_est, y_est_prev, sizeof(y_est));
|
||||
memset(corr_gps, 0, sizeof(corr_gps));
|
||||
memset(corr_vision, 0, sizeof(corr_vision));
|
||||
memset(corr_mocap, 0, sizeof(corr_mocap));
|
||||
memset(corr_flow, 0, sizeof(corr_flow));
|
||||
|
||||
} else {
|
||||
|
|
@ -1076,18 +1171,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
if (t > updates_counter_start + updates_counter_len) {
|
||||
float updates_dt = (t - updates_counter_start) * 0.000001f;
|
||||
warnx(
|
||||
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
|
||||
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
|
||||
(double)(accel_updates / updates_dt),
|
||||
(double)(baro_updates / updates_dt),
|
||||
(double)(gps_updates / updates_dt),
|
||||
(double)(attitude_updates / updates_dt),
|
||||
(double)(flow_updates / updates_dt));
|
||||
(double)(flow_updates / updates_dt),
|
||||
(double)(vision_updates / updates_dt),
|
||||
(double)(mocap_updates / updates_dt));
|
||||
updates_counter_start = t;
|
||||
accel_updates = 0;
|
||||
baro_updates = 0;
|
||||
gps_updates = 0;
|
||||
attitude_updates = 0;
|
||||
flow_updates = 0;
|
||||
vision_updates = 0;
|
||||
mocap_updates = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -140,6 +140,18 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
|
||||
|
||||
/**
|
||||
* Weight for mocap system
|
||||
*
|
||||
* Weight (cutoff frequency) for mocap position measurements.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group Position Estimator INAV
|
||||
*/
|
||||
|
||||
PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f);
|
||||
|
||||
/**
|
||||
* XY axis weight for optical flow
|
||||
*
|
||||
|
|
@ -312,6 +324,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
|
|||
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
|
||||
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
|
||||
h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
|
||||
h->w_mocap_p = param_find("INAV_W_MOC_P");
|
||||
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
|
||||
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
|
||||
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
|
||||
|
|
@ -339,6 +352,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
|
|||
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
|
||||
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
|
||||
param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
|
||||
param_get(h->w_mocap_p, &(p->w_mocap_p));
|
||||
param_get(h->w_xy_flow, &(p->w_xy_flow));
|
||||
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
|
||||
param_get(h->w_gps_flow, &(p->w_gps_flow));
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@ struct position_estimator_inav_params {
|
|||
float w_xy_gps_v;
|
||||
float w_xy_vision_p;
|
||||
float w_xy_vision_v;
|
||||
float w_mocap_p;
|
||||
float w_xy_flow;
|
||||
float w_xy_res_v;
|
||||
float w_gps_flow;
|
||||
|
|
@ -76,6 +77,7 @@ struct position_estimator_inav_param_handles {
|
|||
param_t w_xy_gps_v;
|
||||
param_t w_xy_vision_p;
|
||||
param_t w_xy_vision_v;
|
||||
param_t w_mocap_p;
|
||||
param_t w_xy_flow;
|
||||
param_t w_xy_res_v;
|
||||
param_t w_gps_flow;
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@
|
|||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/satellite_info.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
|
|
@ -1071,7 +1071,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct vehicle_local_position_setpoint_s local_pos_sp;
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct position_setpoint_triplet_s triplet;
|
||||
struct vehicle_vicon_position_s vicon_pos;
|
||||
struct att_pos_mocap_s att_pos_mocap;
|
||||
struct vision_position_estimate_s vision_pos;
|
||||
struct optical_flow_s flow;
|
||||
struct rc_channels_s rc;
|
||||
|
|
@ -1129,7 +1129,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_EST2_s log_EST2;
|
||||
struct log_EST3_s log_EST3;
|
||||
struct log_PWR_s log_PWR;
|
||||
struct log_VICN_s log_VICN;
|
||||
struct log_MOCP_s log_MOCP;
|
||||
struct log_VISN_s log_VISN;
|
||||
struct log_GS0A_s log_GS0A;
|
||||
struct log_GS0B_s log_GS0B;
|
||||
|
|
@ -1164,7 +1164,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int triplet_sub;
|
||||
int gps_pos_sub;
|
||||
int sat_info_sub;
|
||||
int vicon_pos_sub;
|
||||
int att_pos_mocap_sub;
|
||||
int vision_pos_sub;
|
||||
int flow_sub;
|
||||
int rc_sub;
|
||||
|
|
@ -1199,7 +1199,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
subs.local_pos_sp_sub = -1;
|
||||
subs.global_pos_sub = -1;
|
||||
subs.triplet_sub = -1;
|
||||
subs.vicon_pos_sub = -1;
|
||||
subs.att_pos_mocap_sub = -1;
|
||||
subs.vision_pos_sub = -1;
|
||||
subs.flow_sub = -1;
|
||||
subs.rc_sub = -1;
|
||||
|
|
@ -1683,16 +1683,17 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
|
||||
log_msg.msg_type = LOG_VICN_MSG;
|
||||
log_msg.body.log_VICN.x = buf.vicon_pos.x;
|
||||
log_msg.body.log_VICN.y = buf.vicon_pos.y;
|
||||
log_msg.body.log_VICN.z = buf.vicon_pos.z;
|
||||
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
|
||||
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
|
||||
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
|
||||
LOGBUFFER_WRITE_AND_COUNT(VICN);
|
||||
/* --- MOCAP ATTITUDE AND POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) {
|
||||
log_msg.msg_type = LOG_MOCP_MSG;
|
||||
log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0];
|
||||
log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1];
|
||||
log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2];
|
||||
log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3];
|
||||
log_msg.body.log_MOCP.x = buf.att_pos_mocap.x;
|
||||
log_msg.body.log_MOCP.y = buf.att_pos_mocap.y;
|
||||
log_msg.body.log_MOCP.z = buf.att_pos_mocap.z;
|
||||
LOGBUFFER_WRITE_AND_COUNT(MOCP);
|
||||
}
|
||||
|
||||
/* --- VISION POSITION --- */
|
||||
|
|
|
|||
|
|
@ -321,15 +321,16 @@ struct log_PWR_s {
|
|||
uint8_t high_power_rail_overcurrent;
|
||||
};
|
||||
|
||||
/* --- VICN - VICON POSITION --- */
|
||||
#define LOG_VICN_MSG 25
|
||||
struct log_VICN_s {
|
||||
/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
|
||||
#define LOG_MOCP_MSG 25
|
||||
struct log_MOCP_s {
|
||||
float qw;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
};
|
||||
|
||||
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
|
||||
|
|
@ -439,10 +440,10 @@ struct log_VISN_s {
|
|||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
float qw;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float qw;
|
||||
};
|
||||
|
||||
/* --- ENCODERS - ENCODER DATA --- */
|
||||
|
|
@ -537,8 +538,8 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"),
|
||||
LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
|
||||
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
|
||||
LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
|
||||
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),
|
||||
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
|
|
|
|||
|
|
@ -108,8 +108,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
|
|||
#include "topics/vehicle_local_position.h"
|
||||
ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
|
||||
|
||||
#include "topics/vehicle_vicon_position.h"
|
||||
ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
|
||||
#include "topics/att_pos_mocap.h"
|
||||
ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s);
|
||||
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
|
||||
|
|
|
|||
Loading…
Reference in New Issue