yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merged release_v1.0.0 into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-23 11:07:10 +02:00
parent 38535b4d2f ccac324f5b
commit 8fd00f4d6d
12 changed files with 114 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/modules/mavlink/mavlink_main.cpp
View File

@@ -69,6 +69,8 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/mcu_version.h>
#include <systemlib/git_version.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
//#include <mathlib/mathlib.h>
@@ -949,6 +951,47 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
mavlink_logbuffer_write(&_logbuffer, &logmsg);
}
void Mavlink::send_autopilot_capabilites() {
struct vehicle_status_s status;
MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
if (status_sub->update(&status)) {
mavlink_autopilot_version_t msg = {};
msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
msg.flight_sw_version = 0;
msg.middleware_sw_version = 0;
msg.os_sw_version = 0;
msg.board_version = 0;
memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version));
memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version));
memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version));
#ifdef CONFIG_CDCACM_VENDORID
msg.vendor_id = CONFIG_CDCACM_VENDORID;
#else
msg.vendor_id = 0;
#endif
#ifdef CONFIG_CDCACM_PRODUCTID
msg.product_id = CONFIG_CDCACM_PRODUCTID;
#else
msg.product_id = 0;
#endif
uint32_t uid[3];
mcu_unique_id(uid);
msg.uid = (((uint64_t)uid[1]) << 32) | uid[2];
this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg);
}
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
{
/* check if already subscribed to this topic */
@@ -1482,6 +1525,8 @@ Mavlink::task_main(int argc, char *argv[])
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
send_autopilot_capabilites();
while (!_task_should_exit) {
/* main loop */
usleep(_main_loop_delay);