|
|
|
@ -1243,3 +1243,55 @@ void GCS_MAVLINK::send_opticalflow(AP_AHRS_NavEKF &ahrs, const OpticalFlow &optf
@@ -1243,3 +1243,55 @@ void GCS_MAVLINK::send_opticalflow(AP_AHRS_NavEKF &ahrs, const OpticalFlow &optf
|
|
|
|
|
hagl); // ground distance (in meters) set to zero
|
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
send AUTOPILOT_VERSION packet |
|
|
|
|
*/ |
|
|
|
|
void GCS_MAVLINK::send_autopilot_version(void) const |
|
|
|
|
{ |
|
|
|
|
uint16_t capabilities = 0; |
|
|
|
|
uint32_t flight_sw_version = 0; |
|
|
|
|
uint32_t middleware_sw_version = 0; |
|
|
|
|
uint32_t os_sw_version = 0; |
|
|
|
|
uint32_t board_version = 0; |
|
|
|
|
uint8_t flight_custom_version[8]; |
|
|
|
|
uint8_t middleware_custom_version[8]; |
|
|
|
|
uint8_t os_custom_version[8]; |
|
|
|
|
uint16_t vendor_id = 0; |
|
|
|
|
uint16_t product_id = 0; |
|
|
|
|
uint64_t uid = 0; |
|
|
|
|
|
|
|
|
|
#if defined(GIT_VERSION) |
|
|
|
|
strncpy((char *)flight_custom_version, GIT_VERSION, 8); |
|
|
|
|
#else |
|
|
|
|
memset(middleware_custom_version,0,8); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if defined(PX4_GIT_VERSION) |
|
|
|
|
strncpy((char *)middleware_custom_version, PX4_GIT_VERSION, 8); |
|
|
|
|
#else |
|
|
|
|
memset(middleware_custom_version,0,8); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#if defined(NUTTX_GIT_VERSION) |
|
|
|
|
strncpy((char *)os_custom_version, NUTTX_GIT_VERSION, 8); |
|
|
|
|
#else |
|
|
|
|
memset(os_custom_version,0,8); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
mavlink_msg_autopilot_version_send( |
|
|
|
|
chan, |
|
|
|
|
capabilities, |
|
|
|
|
flight_sw_version, |
|
|
|
|
middleware_sw_version, |
|
|
|
|
os_sw_version, |
|
|
|
|
board_version, |
|
|
|
|
flight_custom_version, |
|
|
|
|
middleware_custom_version, |
|
|
|
|
os_custom_version, |
|
|
|
|
vendor_id, |
|
|
|
|
product_id, |
|
|
|
|
uid |
|
|
|
|
); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|