dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #415 from PX4/gps_cleanup 

GPS cleanup
This commit is contained in:
Lorenz Meier 2013-09-21 03:06:46 -07:00
parent 463565ad13 b56723af2a
commit 7033aa173b
7 changed files with 165 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
src/drivers/gps/gps.cpp
View File

@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev
{
public:
GPS(const char* uart_path);
GPS(const char *uart_path);
virtual ~GPS();
virtual int init();
@ -156,7 +156,7 @@ GPS *g_dev;
}
GPS::GPS(const char* uart_path) :
GPS::GPS(const char *uart_path) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
@ -192,6 +192,7 @@ GPS::~GPS()
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
g_dev = nullptr;
}
@ -270,19 +271,24 @@ GPS::task_main()
}
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
}
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
@ -294,6 +300,7 @@ GPS::task_main()
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
@ -310,10 +317,30 @@ GPS::task_main()
}
if (!_healthy) {
warnx("module found");
char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
mode_str = "UBX";
break;
case GPS_DRIVER_MODE_MTK:
mode_str = "MTK";
break;
case GPS_DRIVER_MODE_NMEA:
mode_str = "NMEA";
break;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_healthy = false;
@ -322,25 +349,29 @@ GPS::task_main()
lock();
}
lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
}
}
debug("exiting");
warnx("exiting");
::close(_serial_fd);
@ -361,23 +392,29 @@ void
GPS::print_info()
{
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
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_position) / 1000000.0f));
warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
@ -428,6 +465,7 @@ start(const char *uart_path)
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
goto fail;
}
exit(0);
fail:
@ -503,7 +541,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
char* device_name = GPS_DEFAULT_UART_PORT;
char *device_name = GPS_DEFAULT_UART_PORT;
/*
* Start/load the driver.
@ -513,15 +551,18 @@ gps_main(int argc, char *argv[])
if (argc > 3) {
if (!strcmp(argv[2], "-d")) {
device_name = argv[3];
} else {
goto out;
}
}
gps::start(device_name);
}
if (!strcmp(argv[1], "stop"))
gps::stop();
/*
* Test the driver/device.
*/

7
src/drivers/gps/gps_helper.cpp
View File

@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
case 115200: speed = B115200; break;
warnx("try baudrate: %d\n", speed);
warnx("try baudrate: %d\n", speed);
default:
warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
return -1;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate (tcsetattr)\n");
return -1;
}
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
return 0;
}

2
src/drivers/gps/gps_helper.h
View File

@ -33,7 +33,7 @@
*
****************************************************************************/
/**
/**
* @file gps_helper.h
*/

26
src/drivers/gps/mtk.cpp
View File

@ -48,9 +48,9 @@
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0)
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0)
{
decode_init();
}
@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
handle_message(packet);
return 1;
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
if (time_started + timeout * 1000 < hrt_absolute_time()) {
return -1;
}
j++;
}
/* everything is read */
j = count = 0;
}
@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
if (b == MTK_SYNC1_V16) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 16;
} else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19;
@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
add_byte_to_checksum(b);
// Fill packet buffer
((uint8_t*)(&packet))[_rx_count] = b;
((uint8_t *)(&packet))[_rx_count] = b;
_rx_count++;
/* Packet size minus checksum, XXX ? */
@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
/* Compare checksum */
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
ret = 1;
} else {
warnx("MTK Checksum invalid");
ret = -1;
}
// Reset state machine to decode next packet
decode_init();
}
}
return ret;
}
@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
if (_mtk_revision == 16) {
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
} else if (_mtk_revision == 19) {
_gps_position->lat = packet.latitude; // both degrees*1e7
_gps_position->lon = packet.longitude; // both degrees*1e7
} else {
warnx("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;

58
src/drivers/gps/ubx.cpp
View File

@ -60,13 +60,14 @@
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_configured(false),
_waiting_for_ack(false),
_disable_cmd_counter(0)
_disable_cmd_last(0)
{
decode_init();
}
@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
warnx("ubx: msg rate configuration failed: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SOL\n");
warnx("ubx: msg rate configuration failed: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV VELNED\n");
warnx("ubx: msg rate configuration failed: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
warnx("ubx: msg rate configuration failed: NAV SVINFO");
return 1;
}
@ -271,11 +272,18 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
warnx("ubx: poll error");
return -1;
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
return handled ? 1 : -1;
if (handled) {
return 1;
} else {
warnx("ubx: timeout - no messages");
return -1;
}
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
@ -292,8 +300,6 @@ UBX::receive(unsigned timeout)
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
if (parse_char(buf[i]) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message() > 0)
handled = true;
}
@ -303,6 +309,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
warnx("ubx: timeout - no useful messages");
return -1;
}
}
@ -453,16 +460,16 @@ UBX::handle_message()
timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp?
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = packet->time_nanoseconds;
clock_settime(CLOCK_REALTIME,&ts);
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp?
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = packet->time_nanoseconds;
clock_settime(CLOCK_REALTIME, &ts);
#endif
_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->timestamp_time = hrt_absolute_time();
@ -564,10 +571,13 @@ UBX::handle_message()
if (ret == 0) {
/* message not handled */
warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
hrt_abstime t = hrt_absolute_time();
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */
_disable_cmd_last = t;
warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
ck_b = ck_b + ck_a;
}
/* The checksum is written to the last to bytes of a message */
/* the checksum is written to the last to bytes of a message */
message[length - 2] = ck_a;
message[length - 1] = ck_b;
}
@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
ssize_t ret = 0;
/* Calculate the checksum now */
/* calculate the checksum now */
add_checksum_to_message(packet, length);
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
/* Start with the two sync bytes */
/* start with the two sync bytes */
ret += write(fd, sync_bytes, sizeof(sync_bytes));
ret += write(fd, packet, length);
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
warnx("ubx: config write fail");
warnx("ubx: configuration write fail");
}
void
@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
// Configure receive check
/* configure ACK check */
_message_class_needed = msg_class;
_message_id_needed = msg_id;

6
src/drivers/gps/ubx.h
View File

@ -347,7 +347,7 @@ private:
/**
* Add the two checksum bytes to an outgoing message
*/
void add_checksum_to_message(uint8_t* message, const unsigned length);
void add_checksum_to_message(uint8_t *message, const unsigned length);
/**
* Helper to send a config packet
@ -358,7 +358,7 @@ private:
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
@ -376,7 +376,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
uint8_t _disable_cmd_counter;
uint8_t _disable_cmd_last;
};
#endif /* UBX_H_ */

23
src/modules/mavlink/orb_listener.c
View File

@ -99,6 +99,8 @@ struct listener {
uintptr_t arg;
};
uint16_t cm_uint16_from_m_float(float m);
static void l_sensor_combined(const struct listener *l);
static void l_vehicle_attitude(const struct listener *l);
static void l_vehicle_gps_position(const struct listener *l);
@ -150,6 +152,19 @@ static const struct listener listeners[] = {
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
uint16_t
cm_uint16_from_m_float(float m)
{
if (m < 0.0f) {
return 0;
} else if (m > 655.35f) {
return 65535;
}
return (uint16_t)(m * 100.0f);
}
void
l_sensor_combined(const struct listener *l)
{
@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l)
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F;
@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
gps.lat,
gps.lon,
gps.alt,
gps.eph_m * 1e2f, // from m to cm
gps.epv_m * 1e2f, // from m to cm
cm_uint16_from_m_float(gps.eph_m),
cm_uint16_from_m_float(gps.epv_m),
gps.vel_m_s * 1e2f, // from m/s to cm/s
cog_deg * 1e2f, // from rad to deg * 100
cog_deg * 1e2f, // from deg to deg * 100
gps.satellites_visible);
/* update SAT info every 10 seconds */