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

MAVLink: Send vibration status message

Browse Source
This commit is contained in:
Lorenz Meier 2016-02-25 15:57:53 +01:00
parent 29d417beb7
commit a516450868
2 changed files with 68 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1770,6 +1770,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ALTITUDE", 1.0f);
configure_stream("VISION_POSITION_NED", 10.0f);
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("ESTIMATOR_STATUS", 0.5f);
break;
case MAVLINK_MODE_ONBOARD:
@ -1799,6 +1800,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ALTITUDE", 10.0f);
configure_stream("VISION_POSITION_NED", 10.0f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("ESTIMATOR_STATUS", 1.0f);
break;
case MAVLINK_MODE_OSD:
@ -1814,6 +1816,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("EXTENDED_SYS_STATE", 1.0f);
configure_stream("ALTITUDE", 1.0f);
configure_stream("ESTIMATOR_STATUS", 1.0f);
break;
case MAVLINK_MODE_MAGIC:
@ -1847,6 +1850,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ALTITUDE", 10.0f);
configure_stream("VISION_POSITION_NED", 10.0f);
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
configure_stream("ESTIMATOR_STATUS", 5.0f);
default:
break;
}

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

@ -1431,6 +1431,69 @@ protected:
}
};
class MavlinkStreamEstimatorStatus : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamEstimatorStatus::get_name_static();
}
static const char *get_name_static()
{
return "ESTIMATOR_STATUS";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_VIBRATION;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamEstimatorStatus(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_VIBRATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_est_sub;
uint64_t _est_time;
/* do not allow top copying this class */
MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &);
MavlinkStreamEstimatorStatus& operator = (const MavlinkStreamEstimatorStatus &);
protected:
explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
_est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
_est_time(0)
{}
void send(const hrt_abstime t)
{
struct estimator_status_s est;
if (_est_sub->update(&_est_time, &est)) {
// To be added and filled
// mavlink_estimator_status_t msg = {};
//_mavlink->send_message(MAVLINK_MSG_ID_ESTIMATOR_STATUS, &msg);
mavlink_vibration_t msg = {};
msg.vibration_x = est.vibe[0];
msg.vibration_y = est.vibe[1];
msg.vibration_z = est.vibe[2];
_mavlink->send_message(MAVLINK_MSG_ID_VIBRATION, &msg);
}
}
};
class MavlinkStreamAttPosMocap : public MavlinkStream
{
public:
@ -2753,6 +2816,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static),
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),