fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 04:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged with newer, cleaned up code, fixed the checksum error

Browse Source
This commit is contained in:
Julian Oes
2013-02-04 16:27:01 -08:00
parent cb0fd834ae
commit 039d394c20
5 changed files with 191 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/gps/gps.cpp
+72 -58
View File
@@ -33,7 +33,7 @@
/**
* @file gps.cpp
* Driver for the GPS on UART6
* Driver for the GPS on a serial port
*/
#include <nuttx/config.h>
@@ -107,46 +107,41 @@ public:
private:
int _task_should_exit;
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate;
unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES];
uint8_t _send_buffer[SEND_BUFFER_LENGTH];
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
volatile int _task; ///< worker task
bool _task_should_exit; ///< flag to make the main worker task exit
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to
volatile int _task; ///< worker task
bool _config_needed; ///< flag to signal that configuration of GPS is needed
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _mode_changed; ///< flag that the GPS mode has changed
gps_driver_mode_t _mode; ///< current mode
GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position
float _rate; ///< position update rate
bool _response_received;
bool _config_needed;
bool _baudrate_changed;
bool _mode_changed;
bool _healthy;
gps_driver_mode_t _mode;
unsigned _messages_received;
float _rate; ///< position update rate
GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper
struct vehicle_gps_position_s _report; ///< the current GPS report
orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message
void recv();
/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
void config();
/**
* Trampoline to the worker task
*/
static void task_main_trampoline(void *arg);
/**
* Worker task.
* Worker task: main GPS thread that configures the GPS and parses incoming data, always running
*/
void task_main(void);
/**
* Set the module baud rate
* Set the baudrate of the UART to the GPS
*/
int set_baudrate(unsigned baud);
int set_baudrate(unsigned baud);
/**
* Send a reset command to the GPS
@@ -176,12 +171,10 @@ GPS::GPS() :
_config_needed(true),
_baudrate_changed(false),
_mode_changed(true),
_healthy(false),
_mode(GPS_DRIVER_MODE_UBX),
_messages_received(0),
_Helper(nullptr),
_rate(0.0f),
_report_pub(-1)
_report_pub(-1),
_rate(0.0f)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -216,7 +209,7 @@ GPS::init()
if (CDev::init() != OK)
goto out;
/* start the IO interface task */
/* start the GPS driver worker task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
@@ -275,17 +268,22 @@ void
GPS::config()
{
int length = 0;
uint8_t send_buffer[SEND_BUFFER_LENGTH];
_Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH);
_Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH);
/* the message needs to be sent at the old baudrate */
/* The config message is sent sent at the old baudrate */
if (length > 0) {
if (length != ::write(_serial_fd, _send_buffer, length)) {
if (length != ::write(_serial_fd, send_buffer, length)) {
debug("write config failed");
return;
}
}
/* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed
* from 9600 to 38400
*/
if (_baudrate_changed) {
set_baudrate(_baudrate);
_baudrate_changed = false;
@@ -304,11 +302,10 @@ GPS::task_main()
log("starting");
/* open the serial port */
_serial_fd = ::open("/dev/ttyS3", O_RDWR);
_serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters
uint8_t buf[256];
int baud_i = 0;
uint64_t time_before_configuration;
/* buffer to read from the serial port */
uint8_t buf[32];
if (_serial_fd < 0) {
log("failed to open serial port: %d", errno);
@@ -326,16 +323,16 @@ GPS::task_main()
/* lock against the ioctl handler */
lock();
unsigned baud_i = 0;
_baudrate = _baudrates_to_try[baud_i];
set_baudrate(_baudrate);
time_before_configuration = hrt_absolute_time();
uint64_t time_before_configuration = hrt_absolute_time();
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;
/* loop handling received serial bytes */
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_mode_changed) {
@@ -364,14 +361,17 @@ GPS::task_main()
_mode_changed = false;
}
/* If a configuration does not finish in the config timeout, change the baudrate */
if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) {
baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES;
_baudrate = _baudrates_to_try[baud_i];
set_baudrate(_baudrate);
_Helper->reset();
time_before_configuration = hrt_absolute_time();
}
/* during configuration, the timeout should be small, so that we can send config messages in between parsing,
* but during normal operation, it should never timeout because updates should arrive with 5Hz */
int poll_timeout;
if (_config_needed) {
poll_timeout = 50;
@@ -409,11 +409,21 @@ GPS::task_main()
/* pass received bytes to the packet decoder */
int j;
int ret_parse = 0;
for (j = 0; j < count; j++) {
_messages_received += _Helper->parse(buf[j], &_report);
ret_parse += _Helper->parse(buf[j], &_report);
}
if (_messages_received > 0) {
if (_config_needed == true) {
if (ret_parse < 0) {
/* This means something went wrong in the parser, let's reconfigure */
if (!_config_needed) {
_config_needed = true;
debug("Lost GPS module");
}
config();
} else if (ret_parse > 0) {
/* Looks like we got a valid position update, stop configuring and publish it */
if (_config_needed) {
_config_needed = false;
debug("Found GPS module");
}
@@ -424,8 +434,6 @@ GPS::task_main()
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
_messages_received = 0;
last_rate_count++;
/* measure update rate every 5 seconds */
@@ -435,10 +443,10 @@ GPS::task_main()
last_rate_count = 0;
}
}
/* else if ret_parse == 0: just keep parsing */
}
}
}
out:
debug("exiting");
::close(_serial_fd);
@@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud)
warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
@@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud)
uart_config.c_cflag &= ~(CSTOPB | PARENB);
/* set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state);
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
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(_serial_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;
}
void
GPS::cmd_reset()
{
_healthy = false;
_config_needed = true;
}
void
@@ -524,10 +533,12 @@ GPS::print_info()
warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK");
if (_report.timestamp != 0) {
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
(double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
}
usleep(100000);
}
/**
@@ -583,6 +594,9 @@ fail:
errx(1, "driver start failed");
}
/**
* Stop the driver.
*/
void
stop()
{