mirror of https://github.com/ArduPilot/ardupilot
Bug fixes for EM-406
git-svn-id: https://arducopter.googlecode.com/svn/trunk@539 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
ac2e14c4ec
commit
34aa823003
|
@ -5,6 +5,7 @@
|
|||
|
||||
#include "AP_GPS_NMEA.h"
|
||||
#include "AP_GPS_SIRF.h"
|
||||
#include "AP_GPS_406.h"
|
||||
#include "AP_GPS_UBLOX.h"
|
||||
#include "AP_GPS_MTK.h"
|
||||
#include "AP_GPS_IMU.h"
|
||||
|
|
|
@ -68,6 +68,14 @@ AP_GPS_406::_change_to_sirf_protocol(void)
|
|||
_port->print(init_str);
|
||||
delay(300);
|
||||
|
||||
fs->begin(9600);
|
||||
delay(300);
|
||||
_port->print(init_str);
|
||||
delay(300);
|
||||
|
||||
fs->begin(GPS_406_BITRATE);
|
||||
delay(300);
|
||||
_port->print(init_str);
|
||||
delay(300);
|
||||
}
|
||||
|
||||
|
|
|
@ -165,7 +165,8 @@ AP_GPS_SIRF::_parse_gps(void)
|
|||
switch(_msg_id) {
|
||||
case MSG_GEONAV:
|
||||
time = _swapl(&_buffer.nav.time);
|
||||
fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
|
||||
//fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
|
||||
fix = (0 == _buffer.nav.fix_invalid);
|
||||
latitude = _swapl(&_buffer.nav.latitude);
|
||||
longitude = _swapl(&_buffer.nav.longitude);
|
||||
altitude = _swapl(&_buffer.nav.altitude_msl);
|
||||
|
|
Loading…
Reference in New Issue