fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improve multi-stream handling by template and index usage. Can be consolidated slightly once multiplatform code knows about multi-topics

Browse Source
This commit is contained in:
Lorenz Meier 2015-02-12 09:26:55 +01:00
parent 3a2256dfe7
commit ccc6f0b020
3 changed files with 99 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -11,7 +11,7 @@ mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET -r 30
mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10

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

@ -1403,6 +1403,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VFR_HUD", 10.0f);
configure_stream("SYSTEM_TIME", 1.0f);
configure_stream("TIMESYNC", 10.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
break;
default:

160
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1368,6 +1368,99 @@ protected:
}
};
template <int N>
class MavlinkStreamActuatorControlTarget : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamActuatorControlTarget<N>::get_name_static();
}
static const char *get_name_static()
{
switch (N) {
case 0:
return "ACTUATOR_CONTROL_TARGET0";
case 1:
return "ACTUATOR_CONTROL_TARGET1";
case 2:
return "ACTUATOR_CONTROL_TARGET2";
case 3:
return "ACTUATOR_CONTROL_TARGET3";
}
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamActuatorControlTarget<N>(mavlink);
}
unsigned get_size()
{
return _att_ctrl_sub->is_published() ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_att_ctrl_sub;
uint64_t _att_ctrl_time;
/* do not allow top copying this class */
MavlinkStreamActuatorControlTarget(MavlinkStreamActuatorControlTarget &);
MavlinkStreamActuatorControlTarget& operator = (const MavlinkStreamActuatorControlTarget &);
protected:
explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_ctrl_sub(nullptr),
_att_ctrl_time(0)
{
// XXX this can be removed once the multiplatform system remaps topics
switch (N) {
case 0:
_att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
break;
case 1:
_att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_1));
break;
case 2:
_att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_2));
break;
case 3:
_att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_3));
break;
}
}
void send(const hrt_abstime t)
{
struct actuator_controls_s att_ctrl;
if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) {
mavlink_actuator_control_target_t msg;
msg.time_usec = att_ctrl.timestamp;
msg.group_mlx = N;
for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) {
msg.controls[i] = att_ctrl.control[i];
}
_mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg);
}
}
};
class MavlinkStreamHILControls : public MavlinkStream
{
@ -2006,68 +2099,6 @@ protected:
}
};
class MavlinkStreamAttitudeControls : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamAttitudeControls::get_name_static();
}
static const char *get_name_static()
{
return "ACTUATOR_CONTROL_TARGET";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamAttitudeControls(mavlink);
}
unsigned get_size()
{
return _att_ctrl_sub[0]->is_published() ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_att_ctrl_sub[1];
uint64_t _att_ctrl_time;
/* do not allow top copying this class */
MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &);
MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &);
protected:
explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_ctrl_sub{_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)},
_att_ctrl_time(0)
{}
void send(const hrt_abstime t)
{
struct actuator_controls_s att_ctrl;
if (_att_ctrl_sub[0]->update(&_att_ctrl_time, &att_ctrl)) {
mavlink_actuator_control_target_t msg;
msg.time_usec = att_ctrl.timestamp;
msg.group_mlx = 0;
for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) {
msg.controls[i] = att_ctrl.control[i];
}
_mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg);
}
}
};
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
@ -2296,7 +2327,10 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static),
new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static),
new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static),
new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),