From 8becc86a020b97d40765ed8224c02490ef336b20 Mon Sep 17 00:00:00 2001 From: masierra Date: Wed, 6 Jan 2016 10:16:08 -0800 Subject: [PATCH] [piksi] added heartbeat for piksi --- sw/airborne/subsystems/gps/gps_piksi.c | 17 +++++++++++++++++ sw/airborne/subsystems/gps/gps_piksi.h | 1 + 2 files changed, 18 insertions(+) diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index 2328f87bb4..1fca9b3aec 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.c +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -53,6 +53,18 @@ #define POS_ECEF_TIMEOUT 1000 /* +static uint32_t time_since_last_heartbeat; + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_piksi_heartbeat(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_PIKSI_HEARTBEAT(trans, dev, AC_ID, + &time_since_last_heartbeat); +} + +#endif * Set the Piksi GPS antenna (default is Patch, internal) */ #if USE_PIKSI_EXT_ANTENNA @@ -94,6 +106,7 @@ sbp_msg_callbacks_node_t dops_node; sbp_msg_callbacks_node_t gps_time_node; sbp_msg_callbacks_node_t tracking_state_node; sbp_msg_callbacks_node_t tracking_state_dep_a_node; +sbp_msg_callbacks_node_t heartbeat_node; static void gps_piksi_publish(void); @@ -298,6 +311,7 @@ void gps_impl_init(void) sbp_register_callback(&sbp_state, SBP_MSG_GPS_TIME, &sbp_gps_time_callback, NULL, &gps_time_node); sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE, &sbp_tracking_state_callback, NULL, &tracking_state_node); sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE_DEP_A, &sbp_tracking_state_dep_a_callback, NULL, &tracking_state_dep_a_node); + sbp_register_callback(&sbp_state, SBP_MSG_HEARTBEAT, &spb_heartbeat_callback, NULL, &heartbeat_node); /* Write settings */ sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_ANT_SET), (u8*)(&SBP_ANT_SET), gps_piksi_write); @@ -312,6 +326,9 @@ void gps_impl_init(void) sbp_send_message(&sbp_state, SBP_MSG_BASE_POS, SBP_SENDER_ID, sizeof(msg_base_pos_t), (u8*)(&base_pos), gps_piksi_write);*/ gps.nb_channels = GPS_NB_CHANNELS; + #ifdef PIKSI_HEARTBEAT_MSG + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PIKSI_HEARTBEAT, send_piksi_heartbeat); + #endif } /* diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h index a9024d4965..ecc7d7d44f 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.h +++ b/sw/airborne/subsystems/gps/gps_piksi.h @@ -34,6 +34,7 @@ #define GPS_NB_CHANNELS 10 +#define PIKSI_HEARTBEAT_MSG extern void gps_piksi_event(void); /*