mirror of https://github.com/ArduPilot/ardupilot
63 lines
1.4 KiB
C++
63 lines
1.4 KiB
C++
#include "SIM_config.h"
|
|
|
|
#if AP_SIM_GPS_UBLOX_ENABLED
|
|
|
|
#include "SIM_GPS.h"
|
|
|
|
namespace SITL {
|
|
|
|
class GPS_UBlox : public GPS_Backend {
|
|
public:
|
|
CLASS_NO_COPY(GPS_UBlox);
|
|
|
|
using GPS_Backend::GPS_Backend;
|
|
|
|
void publish(const GPS_Data *d) override;
|
|
|
|
private:
|
|
enum RELPOSNED {
|
|
gnssFixOK = 1U << 0,
|
|
diffSoln = 1U << 1,
|
|
relPosValid = 1U << 2,
|
|
carrSolnFloat = 1U << 3,
|
|
|
|
carrSolnFixed = 1U << 4,
|
|
isMoving = 1U << 5,
|
|
refPosMiss = 1U << 6,
|
|
refObsMiss = 1U << 7,
|
|
|
|
relPosHeadingValid = 1U << 8,
|
|
relPosNormalized = 1U << 9
|
|
};
|
|
struct PACKED ubx_nav_relposned {
|
|
uint8_t version;
|
|
uint8_t reserved1;
|
|
uint16_t refStationId;
|
|
uint32_t iTOW;
|
|
int32_t relPosN;
|
|
int32_t relPosE;
|
|
int32_t relPosD;
|
|
int32_t relPosLength;
|
|
int32_t relPosHeading;
|
|
uint8_t reserved2[4];
|
|
int8_t relPosHPN;
|
|
int8_t relPosHPE;
|
|
int8_t relPosHPD;
|
|
int8_t relPosHPLength;
|
|
uint32_t accN;
|
|
uint32_t accE;
|
|
uint32_t accD;
|
|
uint32_t accLength;
|
|
uint32_t accHeading;
|
|
uint8_t reserved3[4];
|
|
uint32_t flags;
|
|
};
|
|
|
|
void update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg);
|
|
void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
|
|
};
|
|
|
|
};
|
|
|
|
#endif // AP_SIM_GPS_UBLOX_ENABLED
|