fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 03:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: extract OpenDroneID function to lib

Browse Source 
This extracts the function mapping from MAV_TYPE to MAV_ODID_UA_TYPE to
the library, so that it can be re-used later by the remote ID
implementation over DroneCAN.

Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
Julian Oes
2024-05-09 14:30:55 +12:00
parent 4c63e9e4f9
commit 87a63e75be
7 changed files with 358 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/CMakeLists.txt
+1
View File
@@ -135,6 +135,7 @@ px4_add_module(
sensor_calibration
geo
mavlink_c
open_drone_id
timesync
tunes
variable_length_ringbuffer
src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp
+2 -93
View File
@@ -35,6 +35,7 @@
#define OPEN_DRONE_ID_BASIC_ID_HPP
#include <uORB/topics/vehicle_status.h>
#include <lib/open_drone_id/open_drone_id_translations.hpp>
class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream
{
@@ -57,98 +58,6 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type)
{
switch (system_type) {
case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER;
case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE;
case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP;
case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON;
case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET;
case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE;
case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER;
case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER;
case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER;
case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE;
case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_TILTWING: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE;
case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE;
default: return MAV_ODID_UA_TYPE_OTHER;
}
}
bool send() override
@@ -167,7 +76,7 @@ private:
msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER;
// ua_type: MAV_ODID_UA_TYPE
msg.ua_type = odidTypeForMavType(vehicle_status.system_type);
msg.ua_type = open_drone_id_translations::odidTypeForMavType(vehicle_status.system_type);
// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
+6 -58
View File
@@ -144,22 +144,7 @@ private:
const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
// speed_accuracy
if (vehicle_gps_position.s_variance_m_s < 0.3f) {
msg.speed_accuracy = MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND;
} else if (vehicle_gps_position.s_variance_m_s < 1.f) {
msg.speed_accuracy = MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND;
} else if (vehicle_gps_position.s_variance_m_s < 3.f) {
msg.speed_accuracy = MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND;
} else if (vehicle_gps_position.s_variance_m_s < 10.f) {
msg.speed_accuracy = MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND;
} else {
msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
}
msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s);
updated = true;
}
@@ -173,45 +158,9 @@ private:
msg.altitude_geodetic = static_cast<float>(round(vehicle_gps_position.altitude_msl_m)); // [m]
}
// horizontal_accuracy
if (vehicle_gps_position.eph < 1.f) {
msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER;
msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph);
} else if (vehicle_gps_position.eph < 3.f) {
msg.horizontal_accuracy = MAV_ODID_HOR_ACC_3_METER;
} else if (vehicle_gps_position.eph < 10.f) {
msg.horizontal_accuracy = MAV_ODID_HOR_ACC_10_METER;
} else if (vehicle_gps_position.eph < 30.f) {
msg.horizontal_accuracy = MAV_ODID_HOR_ACC_30_METER;
} else {
msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
}
// vertical_accuracy
if (vehicle_gps_position.epv < 1.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_1_METER;
} else if (vehicle_gps_position.epv < 3.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_3_METER;
} else if (vehicle_gps_position.epv < 10.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_10_METER;
} else if (vehicle_gps_position.epv < 25.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_25_METER;
} else if (vehicle_gps_position.epv < 45.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_45_METER;
} else if (vehicle_gps_position.epv < 150.f) {
msg.vertical_accuracy = MAV_ODID_VER_ACC_150_METER;
} else {
msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
}
msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv);
updated = true;
}
@@ -221,9 +170,8 @@ private:
uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
if (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s) {
msg.timestamp_accuracy = MAV_ODID_TIME_ACC_1_0_SECOND; // TODO
}
msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time(
&vehicle_gps_position.timestamp));
updated = true;
}
@@ -236,7 +184,7 @@ private:
if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
msg.barometer_accuracy = MAV_ODID_VER_ACC_150_METER; // TODO
msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration.
updated = true;
}
}