AP_GPS: GPS calculates MAVLink messages for GPS and GPS_RTK
This commit is contained in:
parent
baf0697f48
commit
fa78634959
@ -23,6 +23,7 @@
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Math.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include "GPS_detect_state.h"
|
||||
|
||||
/**
|
||||
|
@ -64,6 +64,7 @@ AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriv
|
||||
last_heatbeat_received_ms(0),
|
||||
last_tracking_state_ms(0),
|
||||
iar_num_hypotheses(-1),
|
||||
baseline_recv_rate(0),
|
||||
|
||||
dgps_corrections_incoming(false),
|
||||
rtk_corrections_incoming(false),
|
||||
@ -174,6 +175,8 @@ AP_GPS_SBP::read(void)
|
||||
float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000;
|
||||
float full_update_hz = full_update_counter / (float) elapsed * 1000;
|
||||
|
||||
baseline_recv_rate = uint8_t (baseline_msg_hz * 10);
|
||||
|
||||
pos_msg_counter = 0;
|
||||
vel_msg_counter = 0;
|
||||
baseline_msg_counter = 0;
|
||||
|
@ -249,6 +249,7 @@ private:
|
||||
uint32_t last_baseline_received_ms;
|
||||
uint32_t last_heatbeat_received_ms;
|
||||
uint32_t last_tracking_state_ms;
|
||||
uint8_t baseline_recv_rate; //in hertz * 10
|
||||
int32_t iar_num_hypotheses;
|
||||
|
||||
//Sticky bits to track updating of state
|
||||
|
Loading…
Reference in New Issue
Block a user