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

Added local position estimator.

Browse Source
This commit is contained in:
jgoppert
2015-10-24 20:34:33 +02:00
committed by Lorenz Meier
parent faa80a51e8
commit 0106be3e89
33 changed files with 3080 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+74
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/estimator_status.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/err.h>
@@ -1271,6 +1272,78 @@ protected:
};
class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamLocalPositionNEDCOV::get_name_static();
}
static const char *get_name_static()
{
return "LOCAL_POSITION_NED_COV";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamLocalPositionNEDCOV(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_est_sub;
uint64_t _est_time;
/* do not allow top copying this class */
MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &);
MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &);
protected:
explicit MavlinkStreamLocalPositionNEDCOV(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)) {
mavlink_local_position_ned_cov_t msg;
msg.time_boot_ms = est.timestamp / 1000;
msg.x = est.states[0];
msg.y = est.states[1];
msg.z = est.states[2];
msg.vx = est.states[3];
msg.vy = est.states[4];
msg.vz = est.states[5];
msg.ax = est.states[6];
msg.ay = est.states[7];
msg.az = est.states[8];
for (int i=0;i<9;i++) {
msg.covariance[i] = est.covariances[i];
}
msg.covariance[9] = est.nan_flags;
msg.covariance[10] = est.health_flags;
msg.covariance[11] = est.timeout_flags;
memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg);
}
}
};
class MavlinkStreamAttPosMocap : public MavlinkStream
{
public:
@@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::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),