diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-19 14:46:26 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-19 14:46:26 +0100 |
| commit | f119d9fbda4b942f58b47b3f1af8addd052f6d9e (patch) | |
| tree | 4cc33ad6c2abd27404de40c958324405cbf1e029 /apps/mavlink/orb_listener.c | |
| parent | bc35bb23dd8cb035c080f8ef8b4cd7a30d5184c2 (diff) | |
| download | px4-firmware-f119d9fbda4b942f58b47b3f1af8addd052f6d9e.tar.gz px4-firmware-f119d9fbda4b942f58b47b3f1af8addd052f6d9e.tar.bz2 px4-firmware-f119d9fbda4b942f58b47b3f1af8addd052f6d9e.zip | |
Added home position concept, uORB struct and MAVLink announcement of home position
Diffstat (limited to 'apps/mavlink/orb_listener.c')
| -rw-r--r-- | apps/mavlink/orb_listener.c | 16 |
1 files changed, 16 insertions, 0 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f5253ea35..8920714ef 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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 */ |