本文整理汇总了C++中UAS类的典型用法代码示例。如果您正苦于以下问题:C++ UAS类的具体用法?C++ UAS怎么用?C++ UAS使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了UAS类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: handle_local_position_ned
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
auto orientation = uas->get_attitude_orientation();
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
tf::pointEigenToMsg(position, pose->pose.position);
// XXX FIX ME #319
tf::quaternionTFToMsg(orientation, pose->pose.orientation);
local_position.publish(pose);
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
transform.transform.rotation = pose->pose.orientation;
tf::vectorEigenToMsg(position, transform.transform.translation);
tf2_broadcaster.sendTransform(transform);
}
}
示例2: command_int
void command_int(bool broadcast,
uint8_t frame, uint16_t command,
uint8_t current, uint8_t autocontinue,
float param1, float param2,
float param3, float param4,
int32_t x, int32_t y,
float z)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
frame,
command,
current,
autocontinue,
param1, param2,
param3, param4,
x, y, z);
UAS_FCU(uas)->send_message(&msg);
}
示例3: command_long
void command_long(bool broadcast,
uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
command,
confirmation_fixed,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg);
}
示例4: connection_cb
void connection_cb(bool connected) {
// if connection changes, start delayed version request
version_retries = RETRIES_COUNT;
if (connected)
autopilot_version_timer.start();
else
autopilot_version_timer.stop();
// add/remove APM diag tasks
if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
"Extra diagnostic disabled.");
#else
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
}
else {
UAS_DIAG(uas).removeByName(mem_diag.getName());
UAS_DIAG(uas).removeByName(hwst_diag.getName());
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
}
}
示例5: arming
/*
* Arming/disarming the UAV
*/
void arming(const mms::Arm::ConstPtr msg){
mavlink_message_t msg_mav;
if (msg->arm_disarm){
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 1; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav);
ROS_INFO("Arming UAV");
} else {
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 0; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav); //TODO decide if send or not disarm by software
ROS_INFO("Disarming UAV");
}
}
示例6: set_mode_cb
bool set_mode_cb(mavros::SetMode::Request &req,
mavros::SetMode::Response &res) {
mavlink_message_t msg;
uint8_t base_mode = req.base_mode;
uint32_t custom_mode = 0;
if (req.custom_mode != "") {
if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
res.success = false;
return true;
}
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
}
mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
uas->get_tgt_system(),
base_mode,
custom_mode);
UAS_FCU(uas)->send_message(&msg);
res.success = true;
return true;
}
示例7: handle_timesync
void handle_timesync(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync);
uint64_t now_ns = ros::Time::now().toNSec();
if (tsync.tc1 == 0) {
send_timesync_msg(now_ns, tsync.ts1);
return;
}
else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2;
int64_t dt = time_offset_ns - offset_ns;
if (std::abs(dt) > 10000000) { // 10 millisecond skew
time_offset_ns = offset_ns; // hard-set it.
uas->set_time_offset(time_offset_ns);
dt_diag.clear();
dt_diag.set_timestamp(tsync.tc1);
ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). "
"Hard syncing clocks.", dt / 1e9);
}
else {
average_offset(offset_ns);
dt_diag.tick(dt, tsync.tc1, time_offset_ns);
uas->set_time_offset(time_offset_ns);
}
}
}
示例8: handle_heartbeat
void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (!uas->is_my_target(sysid)) {
ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
return;
}
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
// update context && setup connection timeout
uas->update_heartbeat(hb.type, hb.autopilot);
uas->update_connection_status(true);
timeout_timer.stop();
timeout_timer.start();
// build state message after updating uas
auto state_msg = boost::make_shared<mavros::State>();
state_msg->header.stamp = ros::Time::now();
state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);
state_pub.publish(state_msg);
hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
}
示例9: handle_gps_raw_int
void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type > 2)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else {
ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}
fill_lla(raw_gps, fix);
float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;
if (!isnan(eph)) {
const double hdop = eph;
// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
}
// store & publish
uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
raw_fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
raw_vel_pub.publish(vel);
}
}
示例10: send_command_long_and_wait
/**
* Common function for command service callbacks.
*
* NOTE: success is bool in messages, but has unsigned char type in C++
*/
bool send_command_long_and_wait(uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7,
unsigned char &success, uint8_t &result) {
unique_lock lock(mutex);
/* check transactions */
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == command) {
ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command);
return false;
}
//! @note APM always send COMMAND_ACK, while PX4 never.
bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
if (is_ack_required)
ack_waiting_list.push_back(new CommandTransaction(command));
command_long(command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required) {
auto it = ack_waiting_list.begin();
for (; it != ack_waiting_list.end(); it++)
if ((*it)->expected_command == command)
break;
if (it == ack_waiting_list.end()) {
ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command);
return false;
}
lock.unlock();
bool is_not_timeout = wait_ack_for(*it);
lock.lock();
success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
result = (*it)->result;
delete *it;
ack_waiting_list.erase(it);
}
else {
success = true;
result = MAV_RESULT_ACCEPTED;
}
return true;
}
示例11: uas_store_attitude
void uas_store_attitude(tf::Quaternion &orientation,
geometry_msgs::Vector3 &gyro_vec,
geometry_msgs::Vector3 &acc_vec)
{
tf::Vector3 angular_velocity;
tf::Vector3 linear_acceleration;
tf::vector3MsgToTF(gyro_vec, angular_velocity);
tf::vector3MsgToTF(acc_vec, linear_acceleration);
uas->set_attitude_orientation(orientation);
uas->set_attitude_angular_velocity(angular_velocity);
uas->set_attitude_linear_acceleration(linear_acceleration);
}
示例12: gps_diag_run
/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
int fix_type, satellites_visible;
float eph, epv;
uas->get_gps_epts(eph, epv, fix_type, satellites_visible);
if (satellites_visible <= 0)
stat.summary(2, "No satellites");
else if (fix_type < 2)
stat.summary(1, "No fix");
else if (fix_type == 2)
stat.summary(0, "2D fix");
else if (fix_type >= 3)
stat.summary(0, "3D fix");
stat.addf("Satellites visible", "%zd", satellites_visible);
stat.addf("Fix type", "%d", fix_type);
if (!isnan(eph))
stat.addf("EPH (m)", "%.2f", eph);
else
stat.add("EPH (m)", "Unknown");
if (!isnan(epv))
stat.addf("EPV (m)", "%.2f", epv);
else
stat.add("EPV (m)", "Unknown");
}
示例13: send_setpoint_transform
/**
* Send transform to FCU position controller
*
* Note: send only XYZ, Yaw
*/
void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
// ENU frame
tf::Vector3 origin = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
/* Documentation start from bit 1 instead 0,
* Ignore velocity and accel vectors, yaw rate
*/
uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
if (uas->is_px4()) {
/**
* Current PX4 has bug: it cuts throttle if there no velocity sp
* Issue #273.
*
* @todo Revesit this quirk later. Should be fixed in firmware.
*/
ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
}
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyz_y,
origin.y(), origin.x(), -origin.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
tf::getYaw(q), 0.0);
}
示例14: autopilot_version_cb
void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
示例15: handle_statustext
void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_statustext_t textm;
mavlink_msg_statustext_decode(msg, &textm);
std::string text(textm.text,
strnlen(textm.text, sizeof(textm.text)));
if (uas->is_ardupilotmega())
process_statustext_apm_quirk(textm.severity, text);
else
process_statustext_normal(textm.severity, text);
}