forked from Archive/PX4-Autopilot
Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps
This commit is contained in:
Julian Oes
parent
a79ad17f09
commit
fc4be3e728
|
|
@ -1678,8 +1678,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
|
||||
|
||||
/* check for first, long-term and valid GPS lock -> set home position */
|
||||
float hdop_m = gps_position.eph / 100.0f;
|
||||
float vdop_m = gps_position.epv / 100.0f;
|
||||
float hdop_m = gps_position.eph_m;
|
||||
float vdop_m = gps_position.epv_m;
|
||||
|
||||
/* check if gps fix is ok */
|
||||
// XXX magic number
|
||||
|
|
@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m)
|
||||
&& (vdop_m < dop_threshold_m)
|
||||
&& !home_position_set
|
||||
&& (hrt_absolute_time() - gps_position.timestamp < 2000000)
|
||||
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
|
||||
&& !current_status.flag_system_armed) {
|
||||
warnx("setting home position");
|
||||
|
||||
|
|
@ -1706,11 +1706,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
home.lon = gps_position.lon;
|
||||
home.alt = gps_position.alt;
|
||||
|
||||
home.eph = gps_position.eph;
|
||||
home.epv = gps_position.epv;
|
||||
home.eph_m = gps_position.eph_m;
|
||||
home.epv_m = gps_position.epv_m;
|
||||
|
||||
home.s_variance = gps_position.s_variance;
|
||||
home.p_variance = gps_position.p_variance;
|
||||
home.s_variance_m_s = gps_position.s_variance_m_s;
|
||||
home.p_variance_m = gps_position.p_variance_m;
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
|
|
|
|||
|
|
@ -543,9 +543,9 @@ GPS::print_info()
|
|||
break;
|
||||
}
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK");
|
||||
if (_report.timestamp != 0) {
|
||||
if (_report.timestamp_position != 0) {
|
||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
||||
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
|
||||
warnx("update rate: %6.2f Hz", (double)_rate);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@
|
|||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <systemlib/err.h>
|
||||
|
|
@ -52,7 +53,7 @@ _config_state(UBX_CONFIG_STATE_PRT),
|
|||
_waiting_for_ack(false),
|
||||
_new_nav_posllh(false),
|
||||
_new_nav_timeutc(false),
|
||||
_new_nav_dop(false),
|
||||
//_new_nav_dop(false),
|
||||
_new_nav_sol(false),
|
||||
_new_nav_velned(false)
|
||||
{
|
||||
|
|
@ -211,10 +212,10 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
|
|||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
|
||||
break;
|
||||
// case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
|
||||
// break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
|
||||
|
|
@ -316,10 +317,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
_message_id = NAV_TIMEUTC;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_DOP:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_DOP;
|
||||
break;
|
||||
// case UBX_MESSAGE_NAV_DOP:
|
||||
// _decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
// _message_id = NAV_DOP;
|
||||
// break;
|
||||
|
||||
case UBX_MESSAGE_NAV_SVINFO:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
|
|
@ -412,11 +413,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
gps_position->lon = packet->lon;
|
||||
gps_position->alt = packet->height_msl;
|
||||
|
||||
gps_position->counter_pos_valid++;
|
||||
gps_position->counter++;
|
||||
gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m
|
||||
gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m
|
||||
|
||||
/* Add timestamp to finish the report */
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->timestamp_position = hrt_absolute_time();
|
||||
|
||||
_new_nav_posllh = true;
|
||||
/* set flag to trigger publishing of new position */
|
||||
|
|
@ -439,11 +440,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->fix_type = packet->gpsFix;
|
||||
gps_position->s_variance = packet->sAcc;
|
||||
gps_position->p_variance = packet->pAcc;
|
||||
gps_position->s_variance_m_s = packet->sAcc;
|
||||
gps_position->p_variance_m = packet->pAcc;
|
||||
|
||||
gps_position->counter++;
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->timestamp_variance = hrt_absolute_time();
|
||||
|
||||
|
||||
_new_nav_sol = true;
|
||||
|
|
@ -457,29 +457,28 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
break;
|
||||
}
|
||||
|
||||
case NAV_DOP: {
|
||||
// printf("GOT NAV_DOP MESSAGE\n");
|
||||
gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->eph = packet->hDOP;
|
||||
gps_position->epv = packet->vDOP;
|
||||
|
||||
gps_position->counter++;
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
|
||||
_new_nav_dop = true;
|
||||
|
||||
} else {
|
||||
warnx("NAV_DOP: checksum invalid");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
// case NAV_DOP: {
|
||||
//// printf("GOT NAV_DOP MESSAGE\n");
|
||||
// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
|
||||
//
|
||||
// //Check if checksum is valid and the store the gps information
|
||||
// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
//
|
||||
// gps_position->eph_m = packet->hDOP;
|
||||
// gps_position->epv = packet->vDOP;
|
||||
//
|
||||
// gps_position->timestamp_posdilution = hrt_absolute_time();
|
||||
//
|
||||
// _new_nav_dop = true;
|
||||
//
|
||||
// } else {
|
||||
// warnx("NAV_DOP: checksum invalid");
|
||||
// }
|
||||
//
|
||||
// // Reset state machine to decode next packet
|
||||
// decodeInit();
|
||||
// break;
|
||||
// }
|
||||
|
||||
case NAV_TIMEUTC: {
|
||||
// printf("GOT NAV_TIMEUTC MESSAGE\n");
|
||||
|
|
@ -501,8 +500,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||
|
||||
gps_position->counter++;
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->timestamp_time = hrt_absolute_time();
|
||||
|
||||
_new_nav_timeutc = true;
|
||||
|
||||
|
|
@ -593,8 +591,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
}
|
||||
|
||||
gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
|
||||
gps_position->counter++;
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->timestamp_satellites = hrt_absolute_time();
|
||||
|
||||
// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report
|
||||
|
||||
|
|
@ -614,18 +611,17 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->vel = (uint16_t)packet->speed;
|
||||
gps_position->vel_n = packet->velN / 100.0f;
|
||||
gps_position->vel_e = packet->velE / 100.0f;
|
||||
gps_position->vel_d = packet->velD / 100.0f;
|
||||
gps_position->vel_m_s = (float)packet->speed * 1e-2f;
|
||||
gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
|
||||
gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
|
||||
gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
|
||||
gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
|
||||
gps_position->vel_ned_valid = true;
|
||||
gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
|
||||
|
||||
gps_position->counter++;
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
|
||||
_new_nav_velned = true;
|
||||
|
||||
|
||||
} else {
|
||||
warnx("NAV_VELNED: checksum invalid");
|
||||
}
|
||||
|
|
@ -690,13 +686,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
_config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_DOP;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
|
||||
break;
|
||||
// case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
|
||||
// break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_SOL;
|
||||
|
|
@ -766,7 +762,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
}
|
||||
|
||||
/* return 1 when position updates and the remaining packets updated at least once */
|
||||
if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) {
|
||||
if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) {
|
||||
|
||||
/* we have received everything, this most probably means that the configuration is fine */
|
||||
config_needed = false;
|
||||
|
|
@ -774,7 +770,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
|
|||
/* Reset the flags */
|
||||
_new_nav_posllh = false;
|
||||
_new_nav_timeutc = false;
|
||||
_new_nav_dop = false;
|
||||
// _new_nav_dop = false;
|
||||
_new_nav_sol = false;
|
||||
_new_nav_velned = false;
|
||||
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@
|
|||
#define UBX_MESSAGE_NAV_POSLLH 0x02
|
||||
#define UBX_MESSAGE_NAV_SOL 0x06
|
||||
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
|
||||
#define UBX_MESSAGE_NAV_DOP 0x04
|
||||
//#define UBX_MESSAGE_NAV_DOP 0x04
|
||||
#define UBX_MESSAGE_NAV_SVINFO 0x30
|
||||
#define UBX_MESSAGE_NAV_VELNED 0x12
|
||||
//#define UBX_MESSAGE_RXM_SVSI 0x20
|
||||
|
|
@ -140,18 +140,18 @@ typedef struct {
|
|||
uint8_t ck_b;
|
||||
} gps_bin_nav_timeutc_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */
|
||||
uint16_t pDOP; /**< Position DOP (scaling 0.01) */
|
||||
uint16_t tDOP; /**< Time DOP (scaling 0.01) */
|
||||
uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */
|
||||
uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */
|
||||
uint16_t nDOP; /**< Northing DOP (scaling 0.01) */
|
||||
uint16_t eDOP; /**< Easting DOP (scaling 0.01) */
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_dop_packet_t;
|
||||
//typedef struct {
|
||||
// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */
|
||||
// uint16_t pDOP; /**< Position DOP (scaling 0.01) */
|
||||
// uint16_t tDOP; /**< Time DOP (scaling 0.01) */
|
||||
// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */
|
||||
// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */
|
||||
// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */
|
||||
// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */
|
||||
// uint8_t ck_a;
|
||||
// uint8_t ck_b;
|
||||
//} gps_bin_nav_dop_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||
|
|
@ -311,7 +311,7 @@ typedef enum {
|
|||
NAV_POSLLH,
|
||||
NAV_SOL,
|
||||
NAV_TIMEUTC,
|
||||
NAV_DOP,
|
||||
// NAV_DOP,
|
||||
NAV_SVINFO,
|
||||
NAV_VELNED,
|
||||
// RXM_SVSI,
|
||||
|
|
@ -371,7 +371,7 @@ private:
|
|||
unsigned _payload_size;
|
||||
bool _new_nav_posllh;
|
||||
bool _new_nav_timeutc;
|
||||
bool _new_nav_dop;
|
||||
// bool _new_nav_dop;
|
||||
bool _new_nav_sol;
|
||||
bool _new_nav_velned;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -190,11 +190,12 @@ void KalmanNav::update()
|
|||
if (!_positionInitialized &&
|
||||
_attitudeInitialized && // wait for attitude first
|
||||
gpsUpdate &&
|
||||
_gps.fix_type > 2 &&
|
||||
_gps.counter_pos_valid > 10) {
|
||||
vN = _gps.vel_n;
|
||||
vE = _gps.vel_e;
|
||||
vD = _gps.vel_d;
|
||||
_gps.fix_type > 2
|
||||
//&& _gps.counter_pos_valid > 10
|
||||
) {
|
||||
vN = _gps.vel_n_m_s;
|
||||
vE = _gps.vel_e_m_s;
|
||||
vD = _gps.vel_d_m_s;
|
||||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
|
|
@ -259,7 +260,7 @@ void KalmanNav::updatePublications()
|
|||
|
||||
// position publication
|
||||
_pos.timestamp = _pubTimeStamp;
|
||||
_pos.time_gps_usec = _gps.timestamp;
|
||||
_pos.time_gps_usec = _gps.timestamp_position;
|
||||
_pos.valid = true;
|
||||
_pos.lat = getLatDegE7();
|
||||
_pos.lon = getLonDegE7();
|
||||
|
|
@ -631,8 +632,8 @@ int KalmanNav::correctPos()
|
|||
|
||||
// residual
|
||||
Vector y(5);
|
||||
y(0) = _gps.vel_n - vN;
|
||||
y(1) = _gps.vel_e - vE;
|
||||
y(0) = _gps.vel_n_m_s - vN;
|
||||
y(1) = _gps.vel_e_m_s - vE;
|
||||
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
||||
|
|
@ -651,9 +652,9 @@ int KalmanNav::correctPos()
|
|||
// abort correction and return
|
||||
printf("[kalman_demo] numerical failure in gps correction\n");
|
||||
// fallback to GPS
|
||||
vN = _gps.vel_n;
|
||||
vE = _gps.vel_e;
|
||||
vD = _gps.vel_d;
|
||||
vN = _gps.vel_n_m_s;
|
||||
vE = _gps.vel_e_m_s;
|
||||
vD = _gps.vel_d_m_s;
|
||||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
|
|
|
|||
|
|
@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg)
|
|||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* gps */
|
||||
hil_gps.timestamp = gps.time_usec;
|
||||
hil_gps.counter = hil_counter++;
|
||||
hil_gps.timestamp_position = gps.time_usec;
|
||||
// hil_gps.counter = hil_counter++;
|
||||
hil_gps.time_gps_usec = gps.time_usec;
|
||||
hil_gps.lat = gps.lat;
|
||||
hil_gps.lon = gps.lon;
|
||||
hil_gps.alt = gps.alt;
|
||||
hil_gps.counter_pos_valid = hil_counter++;
|
||||
hil_gps.eph = gps.eph;
|
||||
hil_gps.epv = gps.epv;
|
||||
hil_gps.s_variance = 100;
|
||||
hil_gps.p_variance = 100;
|
||||
hil_gps.vel = gps.vel;
|
||||
hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_d = 0.0f;
|
||||
hil_gps.cog = gps.cog;
|
||||
// hil_gps.counter_pos_valid = hil_counter++;
|
||||
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
|
||||
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
|
||||
hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
|
||||
hil_gps.p_variance_m = 100; // XXX 100 m variance?
|
||||
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
|
||||
hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
|
||||
hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
|
||||
hil_gps.vel_d_m_s = 0.0f;
|
||||
hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
|
||||
hil_gps.fix_type = gps.fix_type;
|
||||
hil_gps.satellites_visible = gps.satellites_visible;
|
||||
|
||||
|
|
|
|||
|
|
@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l)
|
|||
|
||||
/* GPS position */
|
||||
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
|
||||
gps.timestamp,
|
||||
gps.timestamp_position,
|
||||
gps.fix_type,
|
||||
gps.lat,
|
||||
gps.lon,
|
||||
gps.alt,
|
||||
gps.eph,
|
||||
gps.epv,
|
||||
gps.vel,
|
||||
gps.cog,
|
||||
(uint16_t)(gps.eph_m * 1e2f), // from m to cm
|
||||
(uint16_t)(gps.epv_m * 1e2f), // from m to cm
|
||||
(uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
|
||||
(uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
|
||||
gps.satellites_visible);
|
||||
|
||||
if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
|
||||
|
|
|
|||
|
|
@ -61,10 +61,10 @@ struct home_position_s
|
|||
int32_t lat; /**< Latitude in 1E7 degrees */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
|
||||
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
float s_variance; /**< speed accuracy estimate cm/s */
|
||||
float p_variance; /**< position accuracy estimate cm */
|
||||
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float p_variance_m; /**< position accuracy estimate m */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -55,35 +55,38 @@
|
|||
*/
|
||||
struct vehicle_gps_position_s
|
||||
{
|
||||
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
|
||||
uint32_t counter; /**< Count of GPS messages */
|
||||
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
|
||||
uint64_t timestamp_position; /**< Timestamp for position information */
|
||||
int32_t lat; /**< Latitude in 1E7 degrees */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
|
||||
|
||||
int32_t lat; /**< Latitude in 1E7 degrees //LOGME */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees //LOGME */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */
|
||||
uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
|
||||
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
|
||||
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
float s_variance; /**< speed accuracy estimate cm/s */
|
||||
float p_variance; /**< position accuracy estimate cm */
|
||||
uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
|
||||
float vel_n; /**< GPS ground speed in m/s */
|
||||
float vel_e; /**< GPS ground speed in m/s */
|
||||
float vel_d; /**< GPS ground speed in m/s */
|
||||
uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
|
||||
uint64_t timestamp_variance;
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float p_variance_m; /**< position accuracy estimate m */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
|
||||
uint8_t satellite_prn[20]; /**< Global satellite ID */
|
||||
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
|
||||
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
|
||||
uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
|
||||
|
||||
/* flags */
|
||||
float vel_ned_valid; /**< Flag to indicate if NED speed is valid */
|
||||
uint64_t timestamp_velocity; /**< Timestamp for velocity informations */
|
||||
float vel_m_s; /**< GPS ground speed (m/s) */
|
||||
float vel_n_m_s; /**< GPS ground speed in m/s */
|
||||
float vel_e_m_s; /**< GPS ground speed in m/s */
|
||||
float vel_d_m_s; /**< GPS ground speed in m/s */
|
||||
float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */
|
||||
bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */
|
||||
|
||||
uint64_t timestamp_time; /**< Timestamp for time information */
|
||||
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
|
||||
|
||||
uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
|
||||
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
|
||||
uint8_t satellite_prn[20]; /**< Global satellite ID */
|
||||
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
|
||||
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
|
||||
uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue