Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "../FastSerial/FastSerial.h"
00013 #include "AP_GPS_406.h"
00014 #include "WProgram.h"
00015
00016 static const char init_str[] = "$PSRF100,0,57600,8,1,0*37";
00017
00018
00019 AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s)
00020 {
00021 }
00022
00023
00024 void AP_GPS_406::init(void)
00025 {
00026 _change_to_sirf_protocol();
00027 _configure_gps();
00028
00029 AP_GPS_SIRF::init();
00030 }
00031
00032
00033
00034 void
00035 AP_GPS_406::_configure_gps(void)
00036 {
00037 const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
00038 const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
00039 const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
00040 const uint8_t gps_ender[] = {0xB0, 0xB3};
00041
00042 for(int z = 0; z < 2; z++){
00043 for(int x = 0; x < 5; x++){
00044 _port->write(gps_header, sizeof(gps_header));
00045 _port->write(gps_payload[x]);
00046 for(int y = 0; y < 6; y++)
00047 _port->write((uint8_t)0);
00048 _port->write(gps_checksum[x]);
00049 _port->write(gps_ender[0]);
00050 _port->write(gps_ender[1]);
00051 }
00052 }
00053 }
00054
00055
00056
00057
00058
00059
00060
00061 void
00062 AP_GPS_406::_change_to_sirf_protocol(void)
00063 {
00064 FastSerial *fs = (FastSerial *)_port;
00065
00066 fs->begin(4800);
00067 delay(300);
00068 _port->print(init_str);
00069 delay(300);
00070
00071 fs->begin(9600);
00072 delay(300);
00073 _port->print(init_str);
00074 delay(300);
00075
00076 fs->begin(GPS_406_BITRATE);
00077 delay(300);
00078 _port->print(init_str);
00079 delay(300);
00080 }
00081