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

HIL: Added land detector to HIL simulation

Browse Source
This commit is contained in:
Johan Jansen 2015-01-06 13:56:06 +01:00
parent 546b5727b4
commit 57ed27304a
2 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
status{},
hil_local_pos{},
hil_land_detector{},
_control_mode{},
_global_pos_pub(-1),
_local_pos_pub(-1),
@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_land_detector_pub(-1),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
}
}
/* land detector */
{
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
if(hil_land_detector.landed != landed) {
hil_land_detector.landed = landed;
hil_land_detector.timestamp = hrt_absolute_time();
if (_land_detector_pub < 0) {
_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
} else {
orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector);
}
}
}
/* accelerometer */
{
struct accel_report accel;

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -50,6 +50,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
@ -145,6 +146,7 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_land_detected_s hil_land_detector;
struct vehicle_control_mode_s _control_mode;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
@ -171,6 +173,7 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
orb_advert_t _land_detector_pub;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;