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

Minor quick cleanups

Browse Source
This commit is contained in:
Lorenz Meier
2013-02-04 17:55:58 +01:00
parent 30f028908a
commit 13ec067570
2 changed files with 89 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/gps/gps.cpp
+75 -33
View File
@@ -107,30 +107,29 @@ public:
private: private:
int _task_should_exit; int _task_should_exit;
int _serial_fd; ///< serial interface to GPS int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; unsigned _baudrate;
unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES];
uint8_t _send_buffer[SEND_BUFFER_LENGTH]; uint8_t _send_buffer[SEND_BUFFER_LENGTH];
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows; perf_counter_t _buffer_overflows;
volatile int _task; ///< worker task volatile int _task; ///< worker task
bool _response_received; bool _response_received;
bool _config_needed; bool _config_needed;
bool _baudrate_changed; bool _baudrate_changed;
bool _mode_changed; bool _mode_changed;
bool _healthy; bool _healthy;
gps_driver_mode_t _mode; gps_driver_mode_t _mode;
unsigned _messages_received; unsigned _messages_received;
float _rate; ///< position update rate
GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper
struct vehicle_gps_position_s _report; struct vehicle_gps_position_s _report; ///< the current GPS report
orb_advert_t _report_pub; orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message
orb_advert_t _gps_topic;
void recv(); void recv();
@@ -140,16 +139,19 @@ private:
/** /**
* worker task * Worker task.
*/ */
void task_main(void); void task_main(void);
int set_baudrate(unsigned baud); /**
* Set the module baud rate
*/
int set_baudrate(unsigned baud);
/** /**
* Send a reset command to the GPS * Send a reset command to the GPS
*/ */
void cmd_reset(); void cmd_reset();
}; };
@@ -177,13 +179,15 @@ GPS::GPS() :
_healthy(false), _healthy(false),
_mode(GPS_DRIVER_MODE_UBX), _mode(GPS_DRIVER_MODE_UBX),
_messages_received(0), _messages_received(0),
_Helper(nullptr) _Helper(nullptr),
_rate(0.0f),
_report_pub(-1)
{ {
/* we need this potentially before it could be set in task_main */ /* we need this potentially before it could be set in task_main */
g_dev = this; g_dev = this;
memset(&_report, 0, sizeof(_report));
_debug_enabled = true; _debug_enabled = true;
debug("[gps driver] instantiated");
} }
GPS::~GPS() GPS::~GPS()
@@ -213,16 +217,13 @@ GPS::init()
goto out; goto out;
/* start the IO interface task */ /* start the IO interface task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) { if (_task < 0) {
debug("task start failed: %d", errno); warnx("task start failed: %d", errno);
return -errno; return -errno;
} }
// if (_gps_topic < 0)
// debug("failed to create GPS object");
ret = OK; ret = OK;
out: out:
return ret; return ret;
@@ -231,6 +232,8 @@ out:
int int
GPS::ioctl(struct file *filp, int cmd, unsigned long arg) GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
{ {
lock();
int ret = OK; int ret = OK;
switch (cmd) { switch (cmd) {
@@ -263,6 +266,8 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
break; break;
} }
unlock();
return ret; return ret;
} }
@@ -298,10 +303,6 @@ GPS::task_main()
{ {
log("starting"); log("starting");
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
memset(&_report, 0, sizeof(_report));
/* open the serial port */ /* open the serial port */
_serial_fd = ::open("/dev/ttyS3", O_RDWR); _serial_fd = ::open("/dev/ttyS3", O_RDWR);
@@ -311,7 +312,9 @@ GPS::task_main()
if (_serial_fd < 0) { if (_serial_fd < 0) {
log("failed to open serial port: %d", errno); log("failed to open serial port: %d", errno);
goto out; /* tell the dtor that we are exiting, set error code */
_task = -1;
_exit(1);
} }
/* poll descriptor */ /* poll descriptor */
@@ -329,6 +332,9 @@ GPS::task_main()
time_before_configuration = hrt_absolute_time(); 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 */
while (!_task_should_exit) { while (!_task_should_exit) {
@@ -369,9 +375,9 @@ GPS::task_main()
if (_config_needed) { if (_config_needed) {
poll_timeout = 50; poll_timeout = 50;
} else { } else {
poll_timeout = 250; poll_timeout = 400;
} }
/* sleep waiting for data, but no more than 1000ms */ /* sleep waiting for data, but no more than the poll timeout */
unlock(); unlock();
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout);
lock(); lock();
@@ -386,7 +392,7 @@ GPS::task_main()
config(); config();
if (_config_needed == false) { if (_config_needed == false) {
_config_needed = true; _config_needed = true;
debug("Lost GPS module"); warnx("lost GPS module");
} }
} else if (ret > 0) { } else if (ret > 0) {
/* if we have new data from GPS, go handle it */ /* if we have new data from GPS, go handle it */
@@ -411,8 +417,22 @@ GPS::task_main()
debug("Found GPS module"); debug("Found GPS module");
} }
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); /* 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);
}
_messages_received = 0; _messages_received = 0;
last_rate_count++;
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > 5000000) {
_rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
}
} }
} }
} }
@@ -484,7 +504,29 @@ GPS::cmd_reset()
void void
GPS::print_info() GPS::print_info()
{ {
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK19:
warnx("protocol: MTK 1.9");
break;
case GPS_DRIVER_MODE_MTK16:
warnx("protocol: MTK 1.6");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
}
warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK");
if (_report.timestamp != 0) {
warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type,
int((hrt_absolute_time() - _report.timestamp) / 1000000));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
}
} }
/** /**
apps/drivers/gps/ubx.cpp
+14 -14
View File
@@ -371,7 +371,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
} }
break; break;
default: //should not happen because we set the class default: //should not happen because we set the class
printf("UBX Error, we set a class that we don't know\n"); warnx("UBX Error, we set a class that we don't know\n");
decodeInit(); decodeInit();
break; break;
} }
@@ -410,7 +410,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
_new_nav_posllh = true; _new_nav_posllh = true;
} else { } else {
printf("[gps] NAV_POSLLH: checksum invalid\n"); warnx("NAV_POSLLH: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -437,7 +437,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
_new_nav_sol = true; _new_nav_sol = true;
} else { } else {
printf("[gps] NAV_SOL: checksum invalid\n"); warnx("NAV_SOL: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -463,7 +463,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
_new_nav_dop = true; _new_nav_dop = true;
} else { } else {
printf("[gps] NAV_DOP: checksum invalid\n"); warnx("NAV_DOP: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -624,7 +624,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
_new_nav_velned = true; _new_nav_velned = true;
} else { } else {
printf("[gps] NAV_VELNED: checksum invalid\n"); warnx("NAV_VELNED: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -651,7 +651,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
// ret = 1; // ret = 1;
// //
// } else { // } else {
// printf("[gps] RXM_SVSI: checksum invalid\n"); // warnx("RXM_SVSI: checksum invalid\n");
// } // }
// //
// // Reset state machine to decode next packet // // Reset state machine to decode next packet
@@ -719,7 +719,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
break; break;
} }
} else { } else {
printf("[gps] ACK_ACK: checksum invalid\n"); warnx("ACK_ACK: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -736,11 +736,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
//Check if checksum is valid //Check if checksum is valid
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
printf("[gps] the ubx gps returned: not acknowledged\n"); warnx("the ubx gps returned: not acknowledged\n");
ret = 1; ret = 1;
} else { } else {
printf("[gps] ACK_NAK: checksum invalid\n"); warnx("ACK_NAK: checksum invalid\n");
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
@@ -768,16 +768,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
break; break;
} }
/* return 1 when all needed messages have arrived */ /* 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) {
gps_position->timestamp = hrt_absolute_time(); gps_position->timestamp = hrt_absolute_time();
ret = 1; ret = 1;
_new_nav_posllh = false; _new_nav_posllh = false;
_new_nav_timeutc = false; // _new_nav_timeutc = false;
_new_nav_dop = false; // _new_nav_dop = false;
_new_nav_sol = false; // _new_nav_sol = false;
_new_nav_velned = false; // _new_nav_velned = false;
} }
return ret; return ret;