mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-17 14:51:35 +08:00
Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal
This commit is contained in:
@@ -1127,6 +1127,93 @@ protected:
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamAttitudeControls : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name()
|
||||
{
|
||||
return "ATTITUDE_CONTROLS";
|
||||
}
|
||||
|
||||
MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamAttitudeControls();
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *att_ctrl_sub;
|
||||
struct actuator_controls_s *att_ctrl;
|
||||
|
||||
protected:
|
||||
void subscribe(Mavlink *mavlink)
|
||||
{
|
||||
att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
|
||||
}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
if (att_ctrl_sub->update(t)) {
|
||||
|
||||
// send, add spaces so that string buffer is at least 10 chars long
|
||||
mavlink_msg_named_value_float_send(_channel,
|
||||
att_ctrl->timestamp / 1000,
|
||||
"rll ctrl ",
|
||||
att_ctrl->control[0]);
|
||||
mavlink_msg_named_value_float_send(_channel,
|
||||
att_ctrl->timestamp / 1000,
|
||||
"ptch ctrl ",
|
||||
att_ctrl->control[1]);
|
||||
mavlink_msg_named_value_float_send(_channel,
|
||||
att_ctrl->timestamp / 1000,
|
||||
"yaw ctrl ",
|
||||
att_ctrl->control[2]);
|
||||
mavlink_msg_named_value_float_send(_channel,
|
||||
att_ctrl->timestamp / 1000,
|
||||
"thr ctrl ",
|
||||
att_ctrl->control[3]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamNamedValueFloat : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name()
|
||||
{
|
||||
return "NAMED_VALUE_FLOAT";
|
||||
}
|
||||
|
||||
MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamNamedValueFloat();
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *debug_sub;
|
||||
struct debug_key_value_s *debug;
|
||||
|
||||
protected:
|
||||
void subscribe(Mavlink *mavlink)
|
||||
{
|
||||
debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
|
||||
debug = (struct debug_key_value_s *)debug_sub->get_data();
|
||||
}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
if (debug_sub->update(t)) {
|
||||
|
||||
// Enforce null termination
|
||||
debug->key[sizeof(debug->key) - 1] = '\0';
|
||||
|
||||
mavlink_msg_named_value_float_send(_channel,
|
||||
debug->timestamp_ms,
|
||||
debug->key,
|
||||
debug->value);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
MavlinkStream *streams_list[] = {
|
||||
new MavlinkStreamHeartbeat(),
|
||||
@@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = {
|
||||
new MavlinkStreamRCChannelsRaw(),
|
||||
new MavlinkStreamManualControl(),
|
||||
new MavlinkStreamOpticalFlow(),
|
||||
new MavlinkStreamAttitudeControls(),
|
||||
new MavlinkStreamNamedValueFloat(),
|
||||
nullptr
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//void
|
||||
//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
|
||||
//{
|
||||
// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
|
||||
//
|
||||
// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
|
||||
// /* send, add spaces so that string buffer is at least 10 chars long */
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// l->listener->last_sensor_timestamp / 1000,
|
||||
// "ctrl0 ",
|
||||
// l->listener->actuators_0.control[0]);
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// l->listener->last_sensor_timestamp / 1000,
|
||||
// "ctrl1 ",
|
||||
// l->listener->actuators_0.control[1]);
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// l->listener->last_sensor_timestamp / 1000,
|
||||
// "ctrl2 ",
|
||||
// l->listener->actuators_0.control[2]);
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// l->listener->last_sensor_timestamp / 1000,
|
||||
// "ctrl3 ",
|
||||
// l->listener->actuators_0.control[3]);
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//void
|
||||
//MavlinkOrbListener::l_debug_key_value(const struct listener *l)
|
||||
//{
|
||||
// struct debug_key_value_s debug;
|
||||
//
|
||||
// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
|
||||
//
|
||||
// /* Enforce null termination */
|
||||
// debug.key[sizeof(debug.key) - 1] = '\0';
|
||||
//
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// l->listener->last_sensor_timestamp / 1000,
|
||||
// debug.key,
|
||||
// debug.value);
|
||||
//}
|
||||
//
|
||||
//void
|
||||
//MavlinkOrbListener::l_nav_cap(const struct listener *l)
|
||||
//{
|
||||
//
|
||||
// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
|
||||
//
|
||||
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
// hrt_absolute_time() / 1000,
|
||||
// "turn dist",
|
||||
// l->listener->nav_cap.turn_distance);
|
||||
//
|
||||
//}
|
||||
|
||||
Reference in New Issue
Block a user