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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
unknown
2013-07-09 17:10:49 -04:00
parent a9b327b1fe c3d07030dd
commit c264267814
7 changed files with 346 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ubx.cpp
+35 -25
View File
@@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate)
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
1);
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
@@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate)
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
0);
UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
@@ -224,35 +224,18 @@ UBX::receive(unsigned timeout)
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
uint8_t buf[128];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t count = 0;
bool position_updated = false;
while (true) {
/* pass received bytes to the packet decoder */
while (j < count) {
if (parse_char(buf[j]) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message() > 0)
return 1;
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
j++;
}
/* everything is read */
j = count = 0;
/* then poll for new data */
/* poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
if (ret < 0) {
@@ -272,8 +255,26 @@ UBX::receive(unsigned timeout)
* available, we'll go back to poll() again...
*/
count = ::read(_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
if (parse_char(buf[i])) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message())
position_updated = true;
}
}
}
}
/* return success after receiving a packet */
if (position_updated)
return 1;
/* abort after timeout if no packet parsed successfully */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
}
}
@@ -327,6 +328,7 @@ UBX::parse_char(uint8_t b)
}
break;
case UBX_DECODE_GOT_CLASS:
{
add_byte_to_checksum(b);
switch (_message_class) {
case NAV:
@@ -413,6 +415,14 @@ UBX::parse_char(uint8_t b)
// config_needed = true;
break;
}
// Evaluate state machine - if the state changed,
// the state machine was reset via decode_init()
// and we want to tell the module to stop sending this message
// disable unknown message
//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
//configure_message_rate(_message_class, b, 0);
}
break;
case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b);
src/drivers/mpu6000/mpu6000.cpp
+24 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -272,6 +272,11 @@ private:
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(uint16_t desired_sample_rate_hz);
};
/**
@@ -378,7 +383,8 @@ MPU6000::init()
up_udelay(1000);
// SAMPLE RATE
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
_set_sample_rate(200); // default sample rate = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -493,6 +499,18 @@ MPU6000::probe()
return -EIO;
}
/*
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
*/
void
MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
{
uint8_t div = 1000 / desired_sample_rate_hz;
if(div>200) div=200;
if(div<1) div=1;
write_reg(MPUREG_SMPLRT_DIV, div-1);
}
/*
set the DLPF filter frequency. This affects both accel and gyro.
*/
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
_set_sample_rate(arg);
return OK;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS: