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

Added home position concept, uORB struct and MAVLink announcement of home position

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-19 14:46:26 +01:00
parent bc35bb23dd
commit f119d9fbda
5 changed files with 160 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/orb_listener.c
+16
View File
@@ -114,6 +114,7 @@ static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l);
static void l_vehicle_rates_setpoint(struct listener *l);
static void l_home(struct listener *l);
struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -137,6 +138,7 @@ struct listener listeners[] = {
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
{l_home, &mavlink_subs.home_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -621,6 +623,16 @@ l_optical_flow(struct listener *l)
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
}
void
l_home(struct listener *l)
{
struct home_position_s home;
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
}
static void *
uorb_receive_thread(void *arg)
{
@@ -688,6 +700,10 @@ uorb_receive_start(void)
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */
/* --- HOME POSITION --- */
mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */
/* --- SYSTEM STATE --- */
status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
apps/mavlink/orb_topics.h
+2
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@@ -81,6 +82,7 @@ struct mavlink_subscriptions {
int input_rc_sub;
int optical_flow;
int rates_setpoint_sub;
int home_sub;
};
extern struct mavlink_subscriptions mavlink_subs;