aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Jason Short 2011-10-28 21:29:18 -07:00
commit 7266c5372a
57 changed files with 3922 additions and 4605 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/system.pde
View File

@ -82,7 +82,7 @@ static void init_ardupilot()
#endif
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"),
"\n\nFree RAM: %u\n"),
memcheck_available_memory());

2
ArduPlane/Log.pde
View File

@ -650,7 +650,7 @@ static void Log_Read(int start_page, int end_page)
Serial.printf_P(PSTR((AIRFRAME_NAME)
#endif
Serial.printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %lu\n"),
"\nFree RAM: %u\n"),
memcheck_available_memory());
DataFlash.StartRead(start_page);

2
ArduPlane/system.pde
View File

@ -79,7 +79,7 @@ static void init_ardupilot()
Serial1.begin(38400, 128, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"),
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
//

12
Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs
View File

@ -75,8 +75,14 @@ namespace System.IO.Ports
string dest = Port;
string host = "127.0.0.1";
ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host);
ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest);
if (Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote host", "Enter host name/ip (ensure remote end is already started)", ref host))
{
return;
}
if (Windows.Forms.DialogResult.Cancel == ArdupilotMega.Common.InputBox("remote Port", "Enter remote port", ref dest))
{
return;
}
Port = dest;
client = new TcpClient(host, int.Parse(Port));
@ -169,7 +175,7 @@ namespace System.IO.Ports
VerifyConnected();
int size = client.Available;
byte[] crap = new byte[size];
Console.WriteLine("UdpSerial DiscardInBuffer {0}",size);
Console.WriteLine("TcpSerial DiscardInBuffer {0}",size);
Read(crap, 0, size);
}

6
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -188,17 +188,17 @@ namespace ArdupilotMega
private DateTime lastupdate = DateTime.Now;
private DateTime lastwindcalc = DateTime.Now;
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz

137
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs generated
View File

@ -142,6 +142,7 @@
this.label52 = new System.Windows.Forms.Label();
this.TabAC2 = new System.Windows.Forms.TabPage();
this.groupBox5 = new System.Windows.Forms.GroupBox();
this.label14 = new System.Windows.Forms.Label();
this.THR_RATE_IMAX = new System.Windows.Forms.DomainUpDown();
this.THR_RATE_I = new System.Windows.Forms.DomainUpDown();
this.label20 = new System.Windows.Forms.Label();
@ -223,6 +224,7 @@
this.RATE_RLL_P = new System.Windows.Forms.DomainUpDown();
this.label91 = new System.Windows.Forms.Label();
this.TabPlanner = new System.Windows.Forms.TabPage();
this.label26 = new System.Windows.Forms.Label();
this.CMB_videoresolutions = new System.Windows.Forms.ComboBox();
this.label12 = new System.Windows.Forms.Label();
this.CHK_GDIPlus = new System.Windows.Forms.CheckBox();
@ -273,8 +275,20 @@
this.BUT_load = new ArdupilotMega.MyButton();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_compare = new ArdupilotMega.MyButton();
this.label14 = new System.Windows.Forms.Label();
this.label26 = new System.Windows.Forms.Label();
this.groupBox17 = new System.Windows.Forms.GroupBox();
this.ACRO_PIT_IMAX = new System.Windows.Forms.DomainUpDown();
this.label27 = new System.Windows.Forms.Label();
this.ACRO_PIT_I = new System.Windows.Forms.DomainUpDown();
this.label29 = new System.Windows.Forms.Label();
this.ACRO_PIT_P = new System.Windows.Forms.DomainUpDown();
this.label33 = new System.Windows.Forms.Label();
this.groupBox18 = new System.Windows.Forms.GroupBox();
this.ACRO_RLL_IMAX = new System.Windows.Forms.DomainUpDown();
this.label40 = new System.Windows.Forms.Label();
this.ACRO_RLL_I = new System.Windows.Forms.DomainUpDown();
this.label44 = new System.Windows.Forms.Label();
this.ACRO_RLL_P = new System.Windows.Forms.DomainUpDown();
this.label48 = new System.Windows.Forms.Label();
((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
this.ConfigTabs.SuspendLayout();
this.TabAPM2.SuspendLayout();
@ -304,6 +318,8 @@
this.groupBox25.SuspendLayout();
this.TabPlanner.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.NUM_tracklength)).BeginInit();
this.groupBox17.SuspendLayout();
this.groupBox18.SuspendLayout();
this.SuspendLayout();
//
// Params
@ -995,7 +1011,9 @@
//
// TabAC2
//
this.TabAC2.Controls.Add(this.groupBox17);
this.TabAC2.Controls.Add(this.groupBox5);
this.TabAC2.Controls.Add(this.groupBox18);
this.TabAC2.Controls.Add(this.CHK_lockrollpitch);
this.TabAC2.Controls.Add(this.groupBox4);
this.TabAC2.Controls.Add(this.groupBox6);
@ -1022,6 +1040,11 @@
this.groupBox5.Name = "groupBox5";
this.groupBox5.TabStop = false;
//
// label14
//
resources.ApplyResources(this.label14, "label14");
this.label14.Name = "label14";
//
// THR_RATE_IMAX
//
resources.ApplyResources(this.THR_RATE_IMAX, "THR_RATE_IMAX");
@ -1050,8 +1073,11 @@
// CHK_lockrollpitch
//
resources.ApplyResources(this.CHK_lockrollpitch, "CHK_lockrollpitch");
this.CHK_lockrollpitch.Checked = true;
this.CHK_lockrollpitch.CheckState = System.Windows.Forms.CheckState.Checked;
this.CHK_lockrollpitch.Name = "CHK_lockrollpitch";
this.CHK_lockrollpitch.UseVisualStyleBackColor = true;
this.CHK_lockrollpitch.CheckedChanged += new System.EventHandler(this.CHK_lockrollpitch_CheckedChanged);
//
// groupBox4
//
@ -1545,6 +1571,11 @@
resources.ApplyResources(this.TabPlanner, "TabPlanner");
this.TabPlanner.Name = "TabPlanner";
//
// label26
//
resources.ApplyResources(this.label26, "label26");
this.label26.Name = "label26";
//
// CMB_videoresolutions
//
this.CMB_videoresolutions.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
@ -1911,15 +1942,89 @@
this.BUT_compare.UseVisualStyleBackColor = true;
this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click);
//
// label14
// groupBox17
//
resources.ApplyResources(this.label14, "label14");
this.label14.Name = "label14";
this.groupBox17.Controls.Add(this.ACRO_PIT_IMAX);
this.groupBox17.Controls.Add(this.label27);
this.groupBox17.Controls.Add(this.ACRO_PIT_I);
this.groupBox17.Controls.Add(this.label29);
this.groupBox17.Controls.Add(this.ACRO_PIT_P);
this.groupBox17.Controls.Add(this.label33);
resources.ApplyResources(this.groupBox17, "groupBox17");
this.groupBox17.Name = "groupBox17";
this.groupBox17.TabStop = false;
//
// label26
// ACRO_PIT_IMAX
//
resources.ApplyResources(this.label26, "label26");
this.label26.Name = "label26";
resources.ApplyResources(this.ACRO_PIT_IMAX, "ACRO_PIT_IMAX");
this.ACRO_PIT_IMAX.Name = "ACRO_PIT_IMAX";
//
// label27
//
resources.ApplyResources(this.label27, "label27");
this.label27.Name = "label27";
//
// ACRO_PIT_I
//
resources.ApplyResources(this.ACRO_PIT_I, "ACRO_PIT_I");
this.ACRO_PIT_I.Name = "ACRO_PIT_I";
//
// label29
//
resources.ApplyResources(this.label29, "label29");
this.label29.Name = "label29";
//
// ACRO_PIT_P
//
resources.ApplyResources(this.ACRO_PIT_P, "ACRO_PIT_P");
this.ACRO_PIT_P.Name = "ACRO_PIT_P";
//
// label33
//
resources.ApplyResources(this.label33, "label33");
this.label33.Name = "label33";
//
// groupBox18
//
this.groupBox18.Controls.Add(this.ACRO_RLL_IMAX);
this.groupBox18.Controls.Add(this.label40);
this.groupBox18.Controls.Add(this.ACRO_RLL_I);
this.groupBox18.Controls.Add(this.label44);
this.groupBox18.Controls.Add(this.ACRO_RLL_P);
this.groupBox18.Controls.Add(this.label48);
resources.ApplyResources(this.groupBox18, "groupBox18");
this.groupBox18.Name = "groupBox18";
this.groupBox18.TabStop = false;
//
// ACRO_RLL_IMAX
//
resources.ApplyResources(this.ACRO_RLL_IMAX, "ACRO_RLL_IMAX");
this.ACRO_RLL_IMAX.Name = "ACRO_RLL_IMAX";
//
// label40
//
resources.ApplyResources(this.label40, "label40");
this.label40.Name = "label40";
//
// ACRO_RLL_I
//
resources.ApplyResources(this.ACRO_RLL_I, "ACRO_RLL_I");
this.ACRO_RLL_I.Name = "ACRO_RLL_I";
//
// label44
//
resources.ApplyResources(this.label44, "label44");
this.label44.Name = "label44";
//
// ACRO_RLL_P
//
resources.ApplyResources(this.ACRO_RLL_P, "ACRO_RLL_P");
this.ACRO_RLL_P.Name = "ACRO_RLL_P";
//
// label48
//
resources.ApplyResources(this.label48, "label48");
this.label48.Name = "label48";
//
// Configuration
//
@ -1965,6 +2070,8 @@
this.groupBox25.ResumeLayout(false);
this.TabPlanner.ResumeLayout(false);
((System.ComponentModel.ISupportInitialize)(this.NUM_tracklength)).EndInit();
this.groupBox17.ResumeLayout(false);
this.groupBox18.ResumeLayout(false);
this.ResumeLayout(false);
}
@ -2214,5 +2321,19 @@
private System.Windows.Forms.Label label109;
private System.Windows.Forms.Label label14;
private System.Windows.Forms.Label label26;
private System.Windows.Forms.GroupBox groupBox17;
private System.Windows.Forms.DomainUpDown ACRO_PIT_IMAX;
private System.Windows.Forms.Label label27;
private System.Windows.Forms.DomainUpDown ACRO_PIT_I;
private System.Windows.Forms.Label label29;
private System.Windows.Forms.DomainUpDown ACRO_PIT_P;
private System.Windows.Forms.Label label33;
private System.Windows.Forms.GroupBox groupBox18;
private System.Windows.Forms.DomainUpDown ACRO_RLL_IMAX;
private System.Windows.Forms.Label label40;
private System.Windows.Forms.DomainUpDown ACRO_RLL_I;
private System.Windows.Forms.Label label44;
private System.Windows.Forms.DomainUpDown ACRO_RLL_P;
private System.Windows.Forms.Label label48;
}
}

43
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs
View File

@ -61,24 +61,6 @@ namespace ArdupilotMega.GCSViews
{
InitializeComponent();
// fix for dup name
XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD";
}
private void Configuration_Load(object sender, EventArgs e)
{
// read tooltips
if (tooltips.Count == 0)
readToolTips();
this.SuspendLayout();
// prefill all fields
param = MainV2.comPort.param;
processToScreen();
this.ResumeLayout();
// enable disable relevbant hardware tabs
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
{
@ -93,6 +75,20 @@ namespace ArdupilotMega.GCSViews
TabAC2.Enabled = true;
}
// read tooltips
if (tooltips.Count == 0)
readToolTips();
// prefill all fields
param = MainV2.comPort.param;
processToScreen();
// fix for dup name
XTRK_ANGLE_CD1.Name = "XTRK_ANGLE_CD";
}
private void Configuration_Load(object sender, EventArgs e)
{
// setup up camera button states
if (MainV2.cam != null)
{
@ -386,7 +382,7 @@ namespace ArdupilotMega.GCSViews
// enable roll and pitch pairing for ac2
if (CHK_lockrollpitch.Checked)
{
if (name.StartsWith("RATE_") || name.StartsWith("STB_"))
if (name.StartsWith("RATE_") || name.StartsWith("STB_") || name.StartsWith("ACRO_"))
{
if (name.Contains("_RLL_"))
{
@ -534,6 +530,8 @@ namespace ArdupilotMega.GCSViews
continue;
if (name == "WP_TOTAL")
continue;
if (name == "CMD_TOTAL")
continue;
if (row.Cells[0].Value.ToString() == name)
{
if (row.Cells[1].Value.ToString() != value.ToString())
@ -1021,6 +1019,8 @@ namespace ArdupilotMega.GCSViews
continue;
if (name == "WP_TOTAL")
continue;
if (name == "CMD_TOTAL")
continue;
param2[name] = value;
}
@ -1039,5 +1039,10 @@ namespace ArdupilotMega.GCSViews
MessageBox.Show("You need to restart the planner for this to take effect");
MainV2.config["CHK_GDIPlus"] = CHK_GDIPlus.Checked.ToString();
}
private void CHK_lockrollpitch_CheckedChanged(object sender, EventArgs e)
{
}
}
}

2088
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx
View File

File diff suppressed because it is too large Load Diff

10
Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
View File

@ -488,7 +488,13 @@ namespace ArdupilotMega.GCSViews
return;
}
int apmformat_version = ArduinoDetect.decodeApVar(MainV2.comportname, board);
int apmformat_version = -1; // fail continue
try
{
apmformat_version = ArduinoDetect.decodeApVar(MainV2.comportname, board);
}
catch { }
if (apmformat_version != -1 && apmformat_version != temp.k_format_version)
{
@ -678,7 +684,7 @@ namespace ArdupilotMega.GCSViews
Application.DoEvents();
System.Threading.Thread.Sleep(5000); // 5 seconds - new apvar erases eeprom on new format version, this should buy us some time.
System.Threading.Thread.Sleep(10000); // 10 seconds - new apvar erases eeprom on new format version, this should buy us some time.
}
catch (Exception ex) { lbl_status.Text = "Failed upload"; MessageBox.Show("Check port settings or Port in use? " + ex.ToString()); port.Close(); }

6
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -364,7 +364,7 @@ namespace ArdupilotMega.GCSViews
if (CB_tuning.Checked == false) // draw if in view
// if (CB_tuning.Checked == false) // draw if in view
{
if (MainV2.comPort.logreadmode && MainV2.comPort.logplaybackfile != null)
@ -992,9 +992,9 @@ namespace ArdupilotMega.GCSViews
CMB_setwp.Items.Add("0 (Home)");
if (MainV2.comPort.param["WP_TOTAL"] != null)
if (MainV2.comPort.param["CMD_TOTAL"] != null)
{
int wps = int.Parse(MainV2.comPort.param["WP_TOTAL"].ToString());
int wps = int.Parse(MainV2.comPort.param["CMD_TOTAL"].ToString());
for (int z = 1; z <= wps; z++)
{
CMB_setwp.Items.Add(z.ToString());

18
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -1573,16 +1573,14 @@ namespace ArdupilotMega.GCSViews
xScale.Max = time + xScale.MajorStep;
xScale.Min = xScale.Max - 30.0;
}
// Make sure the Y axis is rescaled to accommodate actual data
try
{
zg1.AxisChange();
}
catch { }
// Force a redraw
zg1.Invalidate();
// Make sure the Y axis is rescaled to accommodate actual data
try
{
zg1.AxisChange();
}
catch { }
// Force a redraw
zg1.Invalidate();
}
private void SaveSettings_Click(object sender, EventArgs e)

7
Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs
View File

@ -172,6 +172,13 @@ namespace ArdupilotMega.GCSViews
comPort.Open();
comPort.WriteLine("");
comPort.WriteLine("");
comPort.WriteLine("");
comPort.WriteLine("");
comPort.WriteLine("");
comPort.WriteLine("");
System.Threading.Thread t11 = new System.Threading.Thread(delegate()
{
threadrun = true;

14
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -505,9 +505,17 @@ namespace ArdupilotMega
if (MainV2.comPort.param.Count > 0)
{
min = (int)(float)(MainV2.comPort.param["RC" + chan + "_MIN"]);
max = (int)(float)(MainV2.comPort.param["RC" + chan + "_MAX"]);
trim = (int)(float)(MainV2.comPort.param["RC" + chan + "_TRIM"]);
try
{
min = (int)(float)(MainV2.comPort.param["RC" + chan + "_MIN"]);
max = (int)(float)(MainV2.comPort.param["RC" + chan + "_MAX"]);
trim = (int)(float)(MainV2.comPort.param["RC" + chan + "_TRIM"]);
}
catch {
min = 1000;
max = 2000;
trim = 1500;
}
}
else
{

3
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -1275,6 +1275,7 @@ namespace ArdupilotMega
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0)
{
param["WP_TOTAL"] = (float)wp_total - 1;
param["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
}
@ -1647,7 +1648,7 @@ namespace ArdupilotMega
if (bpstime.Second != DateTime.Now.Second && !logreadmode)
{
Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead);
Console.WriteLine("bps {0} loss {1} left {2} mem {3}", bps1, synclost, BaseStream.BytesToRead,System.GC.GetTotalMemory(false));
bps2 = bps1; // prev sec
bps1 = 0; // current sec
bpstime = DateTime.Now;

25
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -468,6 +468,7 @@ namespace ArdupilotMega
private void MenuConfiguration_Click(object sender, EventArgs e)
{
MyView.SuspendLayout();
MyView.Controls.Clear();
GCSViews.Terminal.threadrun = false;
@ -486,13 +487,11 @@ namespace ArdupilotMega
UserControl temp = Configuration;
temp.SuspendLayout();
fixtheme(temp);
temp.Anchor = AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right | AnchorStyles.Top;
//temp.Anchor = AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right | AnchorStyles.Top;
temp.Location = new Point(0, MainMenu.Height);
temp.Location = new Point(0, 0);
temp.Dock = DockStyle.Fill;
@ -500,9 +499,11 @@ namespace ArdupilotMega
temp.BackColor = Color.FromArgb(0x26, 0x27, 0x28);
temp.ResumeLayout();
MyView.Controls.Add(temp);
temp.ResumeLayout();
MyView.ResumeLayout();
}
private void MenuSimulation_Click(object sender, EventArgs e)
@ -562,7 +563,19 @@ namespace ArdupilotMega
this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect;
UserControl temp = new GCSViews.Terminal();
// dispose of old else memory leak
if (Terminal != null)
{
try
{
Terminal.Dispose();
}
catch { }
}
Terminal = new GCSViews.Terminal();
UserControl temp = Terminal;
fixtheme(temp);

3
Tools/ArdupilotMegaPlanner/MavlinkLog.cs
View File

@ -332,7 +332,7 @@ namespace ArdupilotMega
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true);
cs.UpdateCurrentSettings(null, true, mine);
try
{
@ -360,6 +360,7 @@ namespace ArdupilotMega
writeKML(logfile + ".kml");
progressBar1.Value = 100;
}
}

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.0.86")]
[assembly: AssemblyFileVersion("1.0.87")]
[assembly: NeutralResourcesLanguageAttribute("")]

4
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -135,7 +135,7 @@ namespace ArdupilotMega.Setup
System.Threading.Thread.Sleep(5);
MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true);
MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true, MainV2.comPort);
if (MainV2.cs.ch1in > 800 && MainV2.cs.ch1in < 2200)
{
@ -192,7 +192,7 @@ namespace ArdupilotMega.Setup
MessageBox.Show("Ensure all your sticks are centered, and click ok to continue");
MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true);
MainV2.cs.UpdateCurrentSettings(currentStateBindingSource, true, MainV2.comPort);
rctrim[0] = MainV2.cs.ch1in;
rctrim[1] = MainV2.cs.ch2in;

2
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
View File

@ -11,7 +11,7 @@
<dsig:Transform Algorithm="urn:schemas-microsoft-com:HashTransforms.Identity" />
</dsig:Transforms>
<dsig:DigestMethod Algorithm="http://www.w3.org/2000/09/xmldsig#sha1" />
<dsig:DigestValue>FgyYFBDKA+EmX+ZazsEdbI/ME7o=</dsig:DigestValue>
<dsig:DigestValue>S+dMQOC9TeJyQiYvhw37LpJxZU0=</dsig:DigestValue>
</hash>
</dependentAssembly>
</dependency>

2088
Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx
View File

File diff suppressed because it is too large Load Diff

4
Tools/scripts/format.sh
View File

@ -2,10 +2,12 @@
function format {
DIR=$1
find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec astyle {} \;
find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec rm {}.orig \;
find $DIR -regex ".*\.\(h\|cpp\|pde\)" -exec rm -f {}.orig \;
}
format apo
format ArduRover
format ArduBoat
format libraries/APO
format libraries/AP_Common
format libraries/AP_GPS

20
libraries/AP_Common/AP_Common.h
View File

@ -26,7 +26,7 @@
// auto-detect at compile time if a call to a string function is using
// a flash-stored string or not
typedef struct {
char c;
char c;
} prog_char_t;
#include <stdint.h>
@ -97,17 +97,17 @@ typedef struct {
static inline int strcasecmp_P(const char *str1, const prog_char_t *pstr)
{
return strcasecmp_P(str1, (const prog_char *)pstr);
return strcasecmp_P(str1, (const prog_char *)pstr);
}
static inline int strcmp_P(const char *str1, const prog_char_t *pstr)
{
return strcmp_P(str1, (const prog_char *)pstr);
return strcmp_P(str1, (const prog_char *)pstr);
}
static inline size_t strlcat_P(char *buffer, const prog_char_t *pstr, size_t buffer_size)
{
return strlcat_P(buffer, (const prog_char *)pstr, buffer_size);
return strlcat_P(buffer, (const prog_char *)pstr, buffer_size);
}
//@}
@ -145,12 +145,12 @@ static inline size_t strlcat_P(char *buffer, const prog_char_t *pstr, size_t buf
//@{
struct Location {
uint8_t id; ///< command id
uint8_t options; ///< options bitmask (1<<0 = relative altitude)
uint8_t p1; ///< param 1
int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100)
int32_t lat; ///< param 3 - Lattitude * 10**7
int32_t lng; ///< param 4 - Longitude * 10**7
uint8_t id; ///< command id
uint8_t options; ///< options bitmask (1<<0 = relative altitude)
uint8_t p1; ///< param 1
int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100)
int32_t lat; ///< param 3 - Lattitude * 10**7
int32_t lng; ///< param 4 - Longitude * 10**7
};
//@}

4
libraries/AP_Common/AP_Test.h
View File

@ -68,8 +68,8 @@ private:
/// Constructor
///
Test::Test(const char *name) :
_name(name),
_fail(false)
_name(name),
_fail(false)
{
}

80
libraries/AP_Common/AP_Var.cpp
View File

@ -40,10 +40,10 @@ uint16_t AP_Var::_bytes_in_use;
// Constructor for standalone variables
//
AP_Var::AP_Var(Key p_key, const prog_char_t *name, Flags flags) :
_group(NULL),
_key(p_key | k_key_not_located),
_name(name),
_flags(flags)
_group(NULL),
_key(p_key | k_key_not_located),
_name(name),
_flags(flags)
{
// Insert the variable or group into the list of known variables, unless
// it wants to be unlisted.
@ -57,10 +57,10 @@ AP_Var::AP_Var(Key p_key, const prog_char_t *name, Flags flags) :
// Constructor for variables in a group
//
AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags flags) :
_group(pGroup),
_key(index),
_name(name),
_flags(flags)
_group(pGroup),
_key(index),
_name(name),
_flags(flags)
{
AP_Var **vp;
@ -76,10 +76,10 @@ AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags f
size_t loopCount = 0;
while (*vp != NULL) {
if (loopCount++>k_num_max) return;
if (loopCount++>k_num_max) return;
if ((*vp)->_key >= _key) {
break;
break;
}
vp = &((*vp)->_link);
}
@ -106,17 +106,17 @@ AP_Var::~AP_Var(void)
// Scan the list and remove this if we find it
{
size_t loopCount = 0;
while (*vp) {
size_t loopCount = 0;
while (*vp) {
if (loopCount++>k_num_max) return;
if (loopCount++>k_num_max) return;
if (*vp == this) {
*vp = _link;
break;
}
vp = &((*vp)->_link);
}
if (*vp == this) {
*vp = _link;
break;
}
vp = &((*vp)->_link);
}
}
// If we are destroying a group, remove all its variables from the list
@ -129,7 +129,7 @@ AP_Var::~AP_Var(void)
while (*vp) {
if (loopCount++>k_num_max) return;
if (loopCount++>k_num_max) return;
// Does the variable claim us as its group?
if ((*vp)->_group == this) {
@ -166,7 +166,7 @@ AP_Var::find(const char *name)
for (vp = first(); vp; vp = vp->next()) {
if (loopCount++>k_num_max) return NULL;
if (loopCount++>k_num_max) return NULL;
char name_buffer[32];
@ -189,7 +189,7 @@ AP_Var::find(Key key)
AP_Var *vp;
size_t loopCount = 0;
for (vp = first(); vp; vp = vp->next()) {
if (loopCount++>k_num_max) return NULL;
if (loopCount++>k_num_max) return NULL;
if (key == vp->key()) {
return vp;
}
@ -242,7 +242,7 @@ bool AP_Var::save(void)
newv = eeprom_read_byte(ep);
if (newv != vbuf[i]) {
serialDebug("readback failed at offset %p: got %u, expected %u",
ep, newv, vbuf[i]);
ep, newv, vbuf[i]);
return false;
}
}
@ -304,10 +304,10 @@ bool AP_Var::save_all(void)
while (vp) {
if (loopCount++>k_num_max) return false;
if (loopCount++>k_num_max) return false;
if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autosave
(vp->_key != k_key_none)) { // has a key
(vp->_key != k_key_none)) { // has a key
if (!vp->save()) {
result = false;
@ -329,10 +329,10 @@ bool AP_Var::load_all(void)
while (vp) {
if (loopCount++>k_num_max) return false;
if (loopCount++>k_num_max) return false;
if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autoload
(vp->_key != k_key_none)) { // has a key
(vp->_key != k_key_none)) { // has a key
if (!vp->load()) {
result = false;
@ -365,7 +365,7 @@ AP_Var::erase_all()
while (vp) {
if (loopCount++>k_num_max) return;
if (loopCount++>k_num_max) return;
vp->_key = vp->key() | k_key_not_located;
vp = vp->_link;
@ -449,7 +449,7 @@ AP_Var::first_member(AP_Var_group *group)
while (*vp) {
if (loopCount++>k_num_max) return NULL;
if (loopCount++>k_num_max) return NULL;
serialDebug("consider %p with %p", *vp, (*vp)->_group);
if ((*vp)->_group == group) {
@ -471,7 +471,7 @@ AP_Var::next_member()
while (vp) {
if (loopCount++>k_num_max) return NULL;
if (loopCount++>k_num_max) return NULL;
if (vp->_group == _group) {
return vp;
@ -498,7 +498,7 @@ bool AP_Var::_EEPROM_scan(void)
eeprom_address = 0;
eeprom_read_block(&ee_header, (void *)eeprom_address, sizeof(ee_header));
if ((ee_header.magic != k_EEPROM_magic) ||
(ee_header.revision != k_EEPROM_revision)) {
(ee_header.revision != k_EEPROM_revision)) {
serialDebug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision);
return false;
@ -514,7 +514,7 @@ bool AP_Var::_EEPROM_scan(void)
while (eeprom_address < (k_EEPROM_size - sizeof(var_header) - 1)) {
if (loopCount++>k_num_max) return NULL;
if (loopCount++>k_num_max) return NULL;
// Read a variable header
//
@ -531,10 +531,10 @@ bool AP_Var::_EEPROM_scan(void)
// Sanity-check the variable header and abort if it looks bad
//
if (k_EEPROM_size <= (
eeprom_address + // current position
sizeof(var_header) + // header for this variable
var_header.size + 1 + // data for this variable
sizeof(var_header))) { // header for sentinel
eeprom_address + // current position
sizeof(var_header) + // header for this variable
var_header.size + 1 + // data for this variable
sizeof(var_header))) { // header for sentinel
serialDebug("header overruns EEPROM");
return false;
@ -544,7 +544,7 @@ bool AP_Var::_EEPROM_scan(void)
vp = _variables;
size_t loopCount2 = 0;
while(vp) {
if (loopCount2++>k_num_max) return false;
if (loopCount2++>k_num_max) return false;
if (vp->key() == var_header.key) {
// adjust the variable's key to point to this entry
vp->_key = eeprom_address + sizeof(var_header);
@ -572,7 +572,7 @@ bool AP_Var::_EEPROM_scan(void)
vp = _variables;
size_t loopCount3 = 0;
while(vp) {
if (loopCount3++>k_num_max) return false;
if (loopCount3++>k_num_max) return false;
if (vp->_key & k_key_not_located) {
vp->_key |= k_key_not_allocated;
serialDebug("key %u not allocated", vp->key());
@ -661,7 +661,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
ee_header.spare = 0;
eeprom_write_block(&ee_header, (void *)0, sizeof(ee_header));
_tail_sentinel = sizeof(ee_header);
// Write a variable-sized pad header with a reserved key value
@ -737,7 +737,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali
while (vp) {
if (loopCount++>k_num_max) return false;
if (loopCount++>k_num_max) return false;
// (un)serialise the group member
if (do_serialize) {

12
libraries/AP_Common/AP_Var.h
View File

@ -302,14 +302,18 @@ public:
///
/// @return The parent group, or NULL if the variable is not grouped.
///
AP_Var_group *group(void) { return _group; }
AP_Var_group *group(void) {
return _group;
}
/// Returns the first variable in the global list.
///
/// @return The first variable in the global list, or NULL if
/// there are none.
///
static AP_Var *first(void) { return _variables; }
static AP_Var *first(void) {
return _variables;
}
/// Returns the next variable in the global list.
///
@ -367,7 +371,9 @@ public:
///
/// @return The sum of sizeof(*this) for all constructed AP_Var subclass instances.
///
static uint16_t get_memory_use() { return _bytes_in_use; }
static uint16_t get_memory_use() {
return _bytes_in_use;
}
protected:
// Memory statistics

22
libraries/AP_Common/c++.cpp
View File

@ -14,29 +14,29 @@
void * operator new(size_t size)
{
#ifdef AP_DISPLAYMEM
displayMemory();
displayMemory();
#endif
return(calloc(size, 1));
return(calloc(size, 1));
}
void operator delete(void *p)
{
if (p) free(p);
if (p) free(p);
}
extern "C" void __cxa_pure_virtual()
{
while (1)
{
Serial.println("Error: pure virtual call");
delay(1000);
}
while (1)
{
Serial.println("Error: pure virtual call");
delay(1000);
}
}
void * operator new[](size_t size)
{
#ifdef AP_DISPLAYMEM
displayMemory();
displayMemory();
#endif
return(calloc(size, 1));
}
@ -50,12 +50,12 @@ __extension__ typedef int __guard __attribute__((mode (__DI__)));
int __cxa_guard_acquire(__guard *g)
{
return !*(char *)(g);
return !*(char *)(g);
};
void __cxa_guard_release (__guard *g)
{
*(char *)g = 1;
*(char *)g = 1;
};
void __cxa_guard_abort (__guard *) {};

22
libraries/AP_Common/examples/menu/menu.pde
View File

@ -7,24 +7,24 @@ FastSerialPort0(Serial);
int8_t
menu_test(uint8_t argc, const Menu::arg *argv)
{
int i;
int i;
Serial.printf("This is a test with %d arguments\n", argc);
for (i = 1; i < argc; i++) {
Serial.printf("%d: int %ld float ", i, argv[i].i);
Serial.println(argv[i].f, 6); // gross
}
Serial.printf("This is a test with %d arguments\n", argc);
for (i = 1; i < argc; i++) {
Serial.printf("%d: int %ld float ", i, argv[i].i);
Serial.println(argv[i].f, 6); // gross
}
}
int8_t
menu_auto(uint8_t argc, const Menu::arg *argv)
{
Serial.println("auto text");
Serial.println("auto text");
}
const struct Menu::command top_menu_commands[] PROGMEM = {
{"*", menu_auto},
{"test", menu_test},
{"*", menu_auto},
{"test", menu_test},
};
MENU(top, "menu", top_menu_commands);
@ -32,8 +32,8 @@ MENU(top, "menu", top_menu_commands);
void
setup(void)
{
Serial.begin(38400);
top.run();
Serial.begin(38400);
top.run();
}
void

168
libraries/AP_Common/include/menu.h
View File

@ -23,101 +23,101 @@
/// Class defining and handling one menu tree
class Menu {
public:
/// argument passed to a menu function
///
/// Space-delimited arguments are parsed from the commandline and
/// separated into these structures.
///
/// If the argument cannot be parsed as a float or a long, the value
/// of f or i respectively is undefined. You should range-check
/// the inputs to your function.
///
struct arg {
const char *str; ///< string form of the argument
long i; ///< integer form of the argument (if a number)
float f; ///< floating point form of the argument (if a number)
};
/// argument passed to a menu function
///
/// Space-delimited arguments are parsed from the commandline and
/// separated into these structures.
///
/// If the argument cannot be parsed as a float or a long, the value
/// of f or i respectively is undefined. You should range-check
/// the inputs to your function.
///
struct arg {
const char *str; ///< string form of the argument
long i; ///< integer form of the argument (if a number)
float f; ///< floating point form of the argument (if a number)
};
/// menu command function
///
/// Functions called by menu array entries are expected to be of this
/// type.
///
/// @param argc The number of valid arguments, including the
/// name of the command in argv[0]. Will never be
/// more than MENU_ARGS_MAX.
/// @param argv Pointer to an array of Menu::arg structures
/// detailing any optional arguments given to the
/// command. argv[0] is always the name of the
/// command, so that the same function can be used
/// to handle more than one command.
///
typedef int8_t (*func)(uint8_t argc, const struct arg *argv);
/// menu command function
///
/// Functions called by menu array entries are expected to be of this
/// type.
///
/// @param argc The number of valid arguments, including the
/// name of the command in argv[0]. Will never be
/// more than MENU_ARGS_MAX.
/// @param argv Pointer to an array of Menu::arg structures
/// detailing any optional arguments given to the
/// command. argv[0] is always the name of the
/// command, so that the same function can be used
/// to handle more than one command.
///
typedef int8_t (*func)(uint8_t argc, const struct arg *argv);
/// menu pre-prompt function
///
/// Called immediately before waiting for the user to type a command; can be
/// used to display help text or status, for example.
///
/// If this function returns false, the menu exits.
///
typedef bool (*preprompt)(void);
/// menu pre-prompt function
///
/// Called immediately before waiting for the user to type a command; can be
/// used to display help text or status, for example.
///
/// If this function returns false, the menu exits.
///
typedef bool (*preprompt)(void);
/// menu command description
///
struct command {
/// Name of the command, as typed or received.
/// Command names are limited in size to keep this structure compact.
///
const char command[MENU_COMMAND_MAX];
/// menu command description
///
struct command {
/// Name of the command, as typed or received.
/// Command names are limited in size to keep this structure compact.
///
const char command[MENU_COMMAND_MAX];
/// The function to call when the command is received.
///
/// The argc argument will be at least 1, and no more than
/// MENU_ARGS_MAX. The argv array will be populated with
/// arguments typed/received up to MENU_ARGS_MAX. The command
/// name will always be in argv[0].
///
/// Commands may return -2 to cause the menu itself to exit.
/// The "?", "help" and "exit" commands are always defined, but
/// can be overridden by explicit entries in the command array.
///
int8_t (*func)(uint8_t argc, const struct arg *argv); ///< callback function
};
/// The function to call when the command is received.
///
/// The argc argument will be at least 1, and no more than
/// MENU_ARGS_MAX. The argv array will be populated with
/// arguments typed/received up to MENU_ARGS_MAX. The command
/// name will always be in argv[0].
///
/// Commands may return -2 to cause the menu itself to exit.
/// The "?", "help" and "exit" commands are always defined, but
/// can be overridden by explicit entries in the command array.
///
int8_t (*func)(uint8_t argc, const struct arg *argv); ///< callback function
};
/// constructor
///
/// Note that you should normally not call the constructor directly. Use
/// the MENU and MENU2 macros defined below.
///
/// @param prompt The prompt to be displayed with this menu.
/// @param commands An array of ::command structures in program memory (PROGMEM).
/// @param entries The number of entries in the menu.
///
Menu(const char *prompt, const struct command *commands, uint8_t entries, preprompt ppfunc = 0);
/// constructor
///
/// Note that you should normally not call the constructor directly. Use
/// the MENU and MENU2 macros defined below.
///
/// @param prompt The prompt to be displayed with this menu.
/// @param commands An array of ::command structures in program memory (PROGMEM).
/// @param entries The number of entries in the menu.
///
Menu(const char *prompt, const struct command *commands, uint8_t entries, preprompt ppfunc = 0);
/// menu runner
void run(void);
/// menu runner
void run(void);
private:
/// Implements the default 'help' command.
///
void _help(void); ///< implements the 'help' command
/// Implements the default 'help' command.
///
void _help(void); ///< implements the 'help' command
/// calls the function for the n'th menu item
///
/// @param n Index for the menu item to call
/// @param argc Number of arguments prepared for the menu item
///
int8_t _call(uint8_t n, uint8_t argc);
/// calls the function for the n'th menu item
///
/// @param n Index for the menu item to call
/// @param argc Number of arguments prepared for the menu item
///
int8_t _call(uint8_t n, uint8_t argc);
const char *_prompt; ///< prompt to display
const command *_commands; ///< array of commands
const uint8_t _entries; ///< size of the menu
const preprompt _ppfunc; ///< optional pre-prompt action
const char *_prompt; ///< prompt to display
const command *_commands; ///< array of commands
const uint8_t _entries; ///< size of the menu
const preprompt _ppfunc; ///< optional pre-prompt action
static char _inbuf[MENU_COMMANDLINE_MAX]; ///< input buffer
static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments
static char _inbuf[MENU_COMMANDLINE_MAX]; ///< input buffer
static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments
};
/// Macros used to define a menu.

190
libraries/AP_Common/menu.cpp
View File

@ -19,10 +19,10 @@ Menu::arg Menu::_argv[MENU_ARGS_MAX + 1];
// constructor
Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entries, preprompt ppfunc) :
_prompt(prompt),
_commands(commands),
_entries(entries),
_ppfunc(ppfunc)
_prompt(prompt),
_commands(commands),
_entries(entries),
_ppfunc(ppfunc)
{
}
@ -30,112 +30,112 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri
void
Menu::run(void)
{
uint8_t len, i, ret;
uint8_t argc;
int c;
char *s;
// loop performing commands
for (;;) {
uint8_t len, i, ret;
uint8_t argc;
int c;
char *s;
// run the pre-prompt function, if one is defined
if ((NULL != _ppfunc) && !_ppfunc())
return;
// loop performing commands
for (;;) {
// loop reading characters from the input
len = 0;
Serial.printf("%S] ", FPSTR(_prompt));
for (;;) {
c = Serial.read();
if (-1 == c)
continue;
// carriage return -> process command
if ('\r' == c) {
_inbuf[len] = '\0';
Serial.write('\r');
Serial.write('\n');
break;
}
// backspace
if ('\b' == c) {
if (len > 0) {
len--;
Serial.write('\b');
Serial.write(' ');
Serial.write('\b');
continue;
}
}
// printable character
if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) {
_inbuf[len++] = c;
Serial.write((char)c);
continue;
}
}
// split the input line into tokens
argc = 0;
_argv[argc++].str = strtok_r(_inbuf, " ", &s);
// XXX should an empty line by itself back out of the current menu?
while (argc <= MENU_ARGS_MAX) {
_argv[argc].str = strtok_r(NULL, " ", &s);
if ('\0' == _argv[argc].str)
break;
_argv[argc].i = atol(_argv[argc].str);
_argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B !
argc++;
}
// run the pre-prompt function, if one is defined
if ((NULL != _ppfunc) && !_ppfunc())
return;
// populate arguments that have not been specified with "" and 0
// this is safer than NULL in the case where commands may look
// without testing argc
i = argc;
while (i <= MENU_ARGS_MAX) {
_argv[i].str = "";
_argv[i].i = 0;
_argv[i].f = 0;
i++;
}
// look for a command matching the first word (note that it may be empty)
for (i = 0; i < _entries; i++) {
if (!strcasecmp_P(_argv[0].str, _commands[i].command)) {
ret = _call(i, argc);
if (-2 == ret)
return;
break;
}
}
// loop reading characters from the input
len = 0;
Serial.printf("%S] ", FPSTR(_prompt));
for (;;) {
c = Serial.read();
if (-1 == c)
continue;
// carriage return -> process command
if ('\r' == c) {
_inbuf[len] = '\0';
Serial.write('\r');
Serial.write('\n');
break;
}
// backspace
if ('\b' == c) {
if (len > 0) {
len--;
Serial.write('\b');
Serial.write(' ');
Serial.write('\b');
continue;
}
}
// printable character
if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) {
_inbuf[len++] = c;
Serial.write((char)c);
continue;
}
}
// implicit commands
if (i == _entries) {
if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) {
_help();
} else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) {
return;
}
}
}
// split the input line into tokens
argc = 0;
_argv[argc++].str = strtok_r(_inbuf, " ", &s);
// XXX should an empty line by itself back out of the current menu?
while (argc <= MENU_ARGS_MAX) {
_argv[argc].str = strtok_r(NULL, " ", &s);
if ('\0' == _argv[argc].str)
break;
_argv[argc].i = atol(_argv[argc].str);
_argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B !
argc++;
}
// populate arguments that have not been specified with "" and 0
// this is safer than NULL in the case where commands may look
// without testing argc
i = argc;
while (i <= MENU_ARGS_MAX) {
_argv[i].str = "";
_argv[i].i = 0;
_argv[i].f = 0;
i++;
}
// look for a command matching the first word (note that it may be empty)
for (i = 0; i < _entries; i++) {
if (!strcasecmp_P(_argv[0].str, _commands[i].command)) {
ret = _call(i, argc);
if (-2 == ret)
return;
break;
}
}
// implicit commands
if (i == _entries) {
if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) {
_help();
} else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) {
return;
}
}
}
}
// display the list of commands in response to the 'help' command
void
Menu::_help(void)
{
int i;
Serial.println("Commands:");
for (i = 0; i < _entries; i++)
Serial.printf(" %S\n", FPSTR(_commands[i].command));
int i;
Serial.println("Commands:");
for (i = 0; i < _entries; i++)
Serial.printf(" %S\n", FPSTR(_commands[i].command));
}
// run the n'th command in the menu
int8_t
Menu::_call(uint8_t n, uint8_t argc)
{
func fn;
func fn;
fn = (func)pgm_read_word(&_commands[n].func);
return(fn(argc, &_argv[0]));
fn = (func)pgm_read_word(&_commands[n].func);
return(fn(argc, &_argv[0]));
}

12
libraries/AP_DCM/AP_DCM_HIL.cpp
View File

@ -42,12 +42,12 @@ AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*cYaw;
_dcm_matrix.a.y = cPitch*sYaw;
_dcm_matrix.a.z = -sPitch;
_dcm_matrix.b.x = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.b.x = cPitch*sYaw;
_dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
_dcm_matrix.b.z = sRoll*cPitch;
_dcm_matrix.c.x = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.c.y = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
}

72
libraries/AP_GPS/AP_GPS_406.cpp
View File

@ -23,35 +23,35 @@ AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s)
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void)
{
_change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
_configure_gps(); // Function to configure GPS, to output only the desired msg's
_change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
_configure_gps(); // Function to configure GPS, to output only the desired msg's
AP_GPS_SIRF::init(); // let the superclass do anything it might need here
idleTimeout = 1200;
AP_GPS_SIRF::init(); // let the superclass do anything it might need here
idleTimeout = 1200;
}
// Private Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_406::_configure_gps(void)
{
const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){
for(int x = 0; x < 5; x++){
_port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg..
_port->write(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0);
_port->write(gps_checksum[x]); // Print the Checksum
_port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
_port->write(gps_ender[1]); // ender
}
}
const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++) {
for(int x = 0; x < 5; x++) {
_port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg..
_port->write(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0);
_port->write(gps_checksum[x]); // Print the Checksum
_port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
_port->write(gps_ender[1]); // ender
}
}
}
// The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary
@ -60,24 +60,24 @@ AP_GPS_406::_configure_gps(void)
// The change is sticky, but only for as long as the internal supercap holds
// settings (usually less than a week).
//
void
void
AP_GPS_406::_change_to_sirf_protocol(void)
{
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(4800);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(9600);
delay(300);
_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);
fs->begin(GPS_406_BITRATE);
delay(300);
_port->print(init_str);
delay(300);
}

8
libraries/AP_GPS/AP_GPS_406.h
View File

@ -20,12 +20,12 @@ class AP_GPS_406 : public AP_GPS_SIRF
{
public:
// Methods
AP_GPS_406(Stream *port);
virtual void init(void);
AP_GPS_406(Stream *port);
virtual void init(void);
private:
void _change_to_sirf_protocol(void);
void _configure_gps(void);
void _change_to_sirf_protocol(void);
void _configure_gps(void);
};
#endif

288
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -25,9 +25,9 @@ const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY;
AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
GPS(s),
_fs(s),
_gps(gps)
GPS(s),
_fs(s),
_gps(gps)
{
}
@ -36,8 +36,8 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
void
AP_GPS_Auto::init(void)
{
idleTimeout = 1200;
if (callback == NULL) callback = delay;
idleTimeout = 1200;
if (callback == NULL) callback = delay;
}
// Called the first time that a client tries to kick the GPS to update.
@ -53,41 +53,41 @@ AP_GPS_Auto::init(void)
bool
AP_GPS_Auto::read(void)
{
GPS *gps;
uint8_t i;
unsigned long then;
GPS *gps;
uint8_t i;
unsigned long then;
// Loop through possible baudrates trying to detect a GPS at one of them.
//
// Note that we need to have a FastSerial rather than a Stream here because
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
// Loop through possible baudrates trying to detect a GPS at one of them.
//
// Note that we need to have a FastSerial rather than a Stream here because
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
_fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) {
_fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device
gps->init();
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
}
return false;
// configure the detected GPS and give it a chance to listen to its device
gps->init();
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
}
return false;
}
//
@ -96,126 +96,126 @@ AP_GPS_Auto::read(void)
GPS *
AP_GPS_Auto::_detect(void)
{
unsigned long then;
int fingerprint[4];
int tries;
GPS *gps;
unsigned long then;
int fingerprint[4];
int tries;
GPS *gps;
//
// Loop attempting to detect a recognized GPS
//
Serial.print('G');
gps = NULL;
for (tries = 0; tries < 2; tries++) {
//
// Loop attempting to detect a recognized GPS
//
Serial.print('G');
gps = NULL;
for (tries = 0; tries < 2; tries++) {
//
// Empty the serial buffer and wait for 50ms of quiet.
//
// XXX We can detect babble by counting incoming characters, but
// what would we do about it?
//
_port->flush();
then = millis();
do {
callback(1);
if (_port->available()) {
then = millis();
_port->read();
}
} while ((millis() - then) < 50);
//
// Empty the serial buffer and wait for 50ms of quiet.
//
// XXX We can detect babble by counting incoming characters, but
// what would we do about it?
//
_port->flush();
then = millis();
do {
callback(1);
if (_port->available()) {
then = millis();
_port->read();
}
} while ((millis() - then) < 50);
//
// Collect four characters to fingerprint a device
//
// If we take more than 1200ms to receive four characters, abort.
// This will normally only be the case where there is no GPS attached.
//
while (_port->available() < 4) {
callback(1);
if ((millis() - then) > 1200) {
Serial.print('!');
return NULL;
}
}
fingerprint[0] = _port->read();
fingerprint[1] = _port->read();
fingerprint[2] = _port->read();
fingerprint[3] = _port->read();
//
// Collect four characters to fingerprint a device
//
// If we take more than 1200ms to receive four characters, abort.
// This will normally only be the case where there is no GPS attached.
//
while (_port->available() < 4) {
callback(1);
if ((millis() - then) > 1200) {
Serial.print('!');
return NULL;
}
}
fingerprint[0] = _port->read();
fingerprint[1] = _port->read();
fingerprint[2] = _port->read();
fingerprint[3] = _port->read();
//
// ublox or MTK in DIYD binary mode (whose smart idea was
// it to make the MTK look sort-of like it was talking UBX?)
//
if ((0xb5 == fingerprint[0]) &&
(0x62 == fingerprint[1]) &&
(0x01 == fingerprint[2])) {
//
// ublox or MTK in DIYD binary mode (whose smart idea was
// it to make the MTK look sort-of like it was talking UBX?)
//
if ((0xb5 == fingerprint[0]) &&
(0x62 == fingerprint[1]) &&
(0x01 == fingerprint[2])) {
// message 5 is MTK pretending to talk UBX
if (0x05 == fingerprint[3]) {
gps = new AP_GPS_MTK(_port);
Serial.print_P(PSTR(" MTK1.4 "));
break;
}
// message 5 is MTK pretending to talk UBX
if (0x05 == fingerprint[3]) {
gps = new AP_GPS_MTK(_port);
Serial.print_P(PSTR(" MTK1.4 "));
break;
}
// any other message is ublox
gps = new AP_GPS_UBLOX(_port);
Serial.print_P(PSTR(" ublox "));
break;
}
// any other message is ublox
gps = new AP_GPS_UBLOX(_port);
Serial.print_P(PSTR(" ublox "));
break;
}
//
// MTK v1.6
//
if ((0xd0 == fingerprint[0]) &&
(0xdd == fingerprint[1]) &&
(0x20 == fingerprint[2])) {
gps = new AP_GPS_MTK16(_port);
Serial.print_P(PSTR(" MTK1.6 "));
break;
}
//
// MTK v1.6
//
if ((0xd0 == fingerprint[0]) &&
(0xdd == fingerprint[1]) &&
(0x20 == fingerprint[2])) {
gps = new AP_GPS_MTK16(_port);
Serial.print_P(PSTR(" MTK1.6 "));
break;
}
//
// SIRF in binary mode
//
if ((0xa0 == fingerprint[0]) &&
(0xa2 == fingerprint[1])) {
gps = new AP_GPS_SIRF(_port);
Serial.print_P(PSTR(" SiRF "));
break;
}
//
// SIRF in binary mode
//
if ((0xa0 == fingerprint[0]) &&
(0xa2 == fingerprint[1])) {
gps = new AP_GPS_SIRF(_port);
Serial.print_P(PSTR(" SiRF "));
break;
}
//
// If we haven't spammed the various init strings, send them now
// and retry to avoid a false-positive on the NMEA detector.
//
if (0 == tries) {
Serial.print('*');
// use the FastSerial port handle so that we can use PROGMEM strings
_fs->println_P((const prog_char_t *)_mtk_set_binary);
_fs->println_P((const prog_char_t *)_ublox_set_binary);
_fs->println_P((const prog_char_t *)_sirf_set_binary);
//
// If we haven't spammed the various init strings, send them now
// and retry to avoid a false-positive on the NMEA detector.
//
if (0 == tries) {
Serial.print('*');
// use the FastSerial port handle so that we can use PROGMEM strings
_fs->println_P((const prog_char_t *)_mtk_set_binary);
_fs->println_P((const prog_char_t *)_ublox_set_binary);
_fs->println_P((const prog_char_t *)_sirf_set_binary);
// give the GPS time to react to the settings
callback(100);
continue;
} else {
Serial.print('?');
}
// give the GPS time to react to the settings
callback(100);
continue;
} else {
Serial.print('?');
}
#if WITH_NMEA_MODE
//
// Something talking NMEA
//
if (('$' == fingerprint[0]) &&
(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
//
// Something talking NMEA
//
if (('$' == fingerprint[0]) &&
(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
// XXX this may be a bit presumptive, might want to give the GPS a couple of
// iterations around the loop to react to init strings?
gps = new AP_GPS_NMEA(_port);
break;
}
// XXX this may be a bit presumptive, might want to give the GPS a couple of
// iterations around the loop to react to init strings?
gps = new AP_GPS_NMEA(_port);
break;
}
#endif
}
return(gps);
}
return(gps);
}

54
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -12,40 +12,40 @@
class AP_GPS_Auto : public GPS
{
public:
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port Stream connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port Stream connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Dummy init routine, does nothing
virtual void init(void);
/// Dummy init routine, does nothing
virtual void init(void);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.
///
virtual bool read(void);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.
///
virtual bool read(void);
private:
/// Copy of the port, known at construction time to be a real FastSerial port.
FastSerial *_fs;
/// Copy of the port, known at construction time to be a real FastSerial port.
FastSerial *_fs;
/// global GPS driver pointer, updated by auto-detection
///
GPS **_gps;
/// global GPS driver pointer, updated by auto-detection
///
GPS **_gps;
/// low-level auto-detect routine
///
GPS *_detect(void);
/// low-level auto-detect routine
///
GPS *_detect(void);
static const prog_char _mtk_set_binary[];
static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[];
static const prog_char _mtk_set_binary[];
static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[];
};
#endif

18
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -21,21 +21,21 @@ AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s)
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(void)
{
idleTimeout = 1200;
{
idleTimeout = 1200;
}
bool AP_GPS_HIL::read(void)
{
bool result = _updated;
bool result = _updated;
// return true once for each update pushed in
_updated = false;
return result;
// return true once for each update pushed in
_updated = false;
return result;
}
void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{
time = _time;
latitude = _latitude*1.0e7;
@ -46,7 +46,7 @@ void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _al
speed_3d = _speed_3d*1.0e2;
num_sats = _num_sats;
fix = true;
new_data = true;
_updated = true;
new_data = true;
_updated = true;
}

10
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -16,9 +16,9 @@
class AP_GPS_HIL : public GPS {
public:
AP_GPS_HIL(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_HIL(Stream *s);
virtual void init(void);
virtual bool read(void);
/**
* Hardware in the loop set function
@ -31,10 +31,10 @@ public:
* @param altitude - altitude in meters
*/
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
private:
bool _updated;
bool _updated;
};
#endif // AP_GPS_HIL_H

282
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -15,7 +15,7 @@
Methods:
init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
lattitude : lattitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
@ -25,7 +25,7 @@
new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data
fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
*/
#include "AP_GPS_IMU.h"
#include "WProgram.h"
@ -38,192 +38,192 @@ AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_IMU::init(void)
{
// we expect the stream to already be open at the corret bitrate
idleTimeout = 1200;
// we expect the stream to already be open at the corret bitrate
idleTimeout = 1200;
}
// optimization : This code doesn't wait for data. It only proccess the data available.
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
bool
bool
AP_GPS_IMU::read(void)
{
byte data;
int numc = 0;
byte data;
int numc = 0;
numc = _port->available();
numc = _port->available();
if (numc > 0){
for (int i=0;i<numc;i++){ // Process bytes received
if (numc > 0) {
for (int i=0; i<numc; i++) { // Process bytes received
data = _port->read();
switch(step){ //Normally we start from zero. This is a state machine
case 0:
if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 3:
if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 4:
payload_length = data;
checksum(payload_length);
step++;
if (payload_length > 28){
step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0;
ck_a = 0;
ck_b = 0;
//payload_error_count++;
}
break;
case 5:
message_num = data;
checksum(data);
step++;
break;
case 6: // Payload data read...
// We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter >= payload_length) {
step++;
}
break;
case 7:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 8:
GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read...
// Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) {
join_data();
} else if (message_num == 0x03) {
GPS_join_data();
} else if (message_num == 0x04) {
join_data_xplane();
} else if (message_num == 0x0a) {
//PERF_join_data();
} else {
data = _port->read();
switch(step) { //Normally we start from zero. This is a state machine
case 0:
if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 3:
if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 4:
payload_length = data;
checksum(payload_length);
step++;
if (payload_length > 28) {
step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0;
ck_a = 0;
ck_b = 0;
//payload_error_count++;
}
break;
case 5:
message_num = data;
checksum(data);
step++;
break;
case 6: // Payload data read...
// We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter >= payload_length) {
step++;
}
break;
case 7:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 8:
GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read...
// Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) {
join_data();
} else if (message_num == 0x03) {
GPS_join_data();
} else if (message_num == 0x04) {
join_data_xplane();
} else if (message_num == 0x0a) {
//PERF_join_data();
} else {
// _error("Invalid message number = %d\n", (int)message_num);
}
} else {
}
} else {
// _error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++;
}
// Variable initialization
step = 0;
payload_counter = 0;
ck_a = 0;
ck_b = 0;
break;
}
}// End for...
}
return true;
//imu_checksum_error_count++;
}
// Variable initialization
step = 0;
payload_counter = 0;
ck_a = 0;
ck_b = 0;
break;
}
}// End for...
}
return true;
}
/****************************************************************
*
*
****************************************************************/
void AP_GPS_IMU::join_data(void)
{
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4];
imu_ok = true;
//Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4];
imu_ok = true;
}
void AP_GPS_IMU::join_data_xplane()
{
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
ground_course = *(unsigned int *)&buffer[4];
//Storing airspeed
airspeed = *(int *)&buffer[6];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
imu_ok = true;
//Storing IMU heading (yaw)
ground_course = *(unsigned int *)&buffer[4];
//Storing airspeed
airspeed = *(int *)&buffer[6];
imu_ok = true;
}
void AP_GPS_IMU::GPS_join_data(void)
{
longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4];
longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4];
//Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10;
//Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10;
//Storing Speed
speed_3d = ground_speed = (float)*(int *)&buffer[10];
//We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(long *)&buffer[14];
//Storing Speed
speed_3d = ground_speed = (float)*(int *)&buffer[10];
imu_health = buffer[15];
new_data = true;
fix = true;
//We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(long *)&buffer[14];
imu_health = buffer[15];
new_data = true;
fix = true;
}
/****************************************************************
*
*
****************************************************************/
// checksum algorithm
void AP_GPS_IMU::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
ck_a += data;
ck_b += ck_a;
}
@ -231,4 +231,4 @@ void AP_GPS_IMU::checksum(byte data)
* Unused
****************************************************************/
void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {};
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {};

66
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -7,42 +7,42 @@
#define MAXPAYLOAD 32
class AP_GPS_IMU : public GPS {
public:
public:
// Methods
AP_GPS_IMU(Stream *s);
virtual void init(void);
virtual bool read(void);
// Properties
long roll_sensor; // how much we're turning in deg * 100
long pitch_sensor; // our angle of attack in deg * 100
int airspeed;
float imu_health;
uint8_t imu_ok;
// Unused
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
AP_GPS_IMU(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// Packet checksums
uint8_t ck_a;
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
uint8_t step;
uint8_t msg_class;
uint8_t message_num;
uint8_t payload_length;
uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD];
void join_data();
void join_data_xplane();
void GPS_join_data();
void checksum(unsigned char data);
// Properties
long roll_sensor; // how much we're turning in deg * 100
long pitch_sensor; // our angle of attack in deg * 100
int airspeed;
float imu_health;
uint8_t imu_ok;
// Unused
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
private:
// Packet checksums
uint8_t ck_a;
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
uint8_t step;
uint8_t msg_class;
uint8_t message_num;
uint8_t payload_length;
uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD];
void join_data();
void join_data_xplane();
void GPS_join_data();
void checksum(unsigned char data);
};
#endif

208
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -20,21 +20,21 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
}
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_MTK::init(void)
{
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
{
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
idleTimeout = 1200;
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -51,103 +51,103 @@ AP_GPS_MTK::init(void)
bool
AP_GPS_MTK::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
restart:
switch(_step){
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
_step++;
_ck_b += (_ck_a += data);
_payload_counter = 0;
} else {
_step = 0;
goto restart;
}
break;
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
_step++;
_ck_b += (_ck_a += data);
_payload_counter = 0;
} else {
_step = 0;
goto restart;
}
break;
// Receive message data
//
case 4:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Receive message data
//
case 4:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 5:
_step++;
if (_ck_a != data) {
_error("GPS_MTK: checksum error\n");
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
_error("GPS_MTK: checksum error\n");
break;
}
// Checksum and message processing
//
case 5:
_step++;
if (_ck_a != data) {
_error("GPS_MTK: checksum error\n");
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
_error("GPS_MTK: checksum error\n");
break;
}
fix = (_buffer.msg.fix_type == FIX_3D);
latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10;
altitude = _swapl(&_buffer.msg.altitude);
ground_speed = _swapl(&_buffer.msg.ground_speed);
ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites;
// time from gps is UTC, but convert here to msToD
long time_utc = _swapl(&_buffer.msg.utc_time);
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
fix = (_buffer.msg.fix_type == FIX_3D);
latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10;
altitude = _swapl(&_buffer.msg.altitude);
ground_speed = _swapl(&_buffer.msg.ground_speed);
ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites;
parsed = true;
}
}
return parsed;
// time from gps is UTC, but convert here to msToD
long time_utc = _swapl(&_buffer.msg.utc_time);
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true;
}
}
return parsed;
}

74
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -20,52 +20,52 @@
class AP_GPS_MTK : public GPS {
public:
AP_GPS_MTK(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_MTK(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
};
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
};
// #pragma pack(pop)
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1,
MESSAGE_ID = 5
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1,
MESSAGE_ID = 5
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Buffer parse & GPS state update
void _parse_gps();
// Buffer parse & GPS state update
void _parse_gps();
};
#endif // AP_GPS_MTK_H

220
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -20,23 +20,23 @@ AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
}
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_MTK16::init(void)
{
_port->flush();
{
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
_time_offset = 0;
_offset_calculated = false;
idleTimeout = 1200;
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
_time_offset = 0;
_offset_calculated = false;
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -53,109 +53,109 @@ AP_GPS_MTK16::init(void)
bool
AP_GPS_MTK16::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
restart:
switch(_step){
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (sizeof(_buffer) == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0;
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (sizeof(_buffer) == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0;
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
// Receive message data
//
case 3:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Receive message data
//
case 3:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 4:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 5:
_step = 0;
if (_ck_b != data) {
break;
}
// Checksum and message processing
//
case 4:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 5:
_step = 0;
if (_ck_b != data) {
break;
}
fix = _buffer.msg.fix_type == FIX_3D;
latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise
longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise
altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date;
// time from gps is UTC, but convert here to msToD
long time_utc = _buffer.msg.utc_time;
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
fix = _buffer.msg.fix_type == FIX_3D;
latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise
longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise
altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date;
parsed = true;
/* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) {
long tempd1 = date;
long day = tempd1/10000;
tempd1 -= day * 10000;
long month = tempd1/100;
long year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year);
_epoch = UNIX_EPOCH;
_offset_calculated = TRUE;
}
*/
}
}
return parsed;
// time from gps is UTC, but convert here to msToD
long time_utc = _buffer.msg.utc_time;
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true;
/* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) {
long tempd1 = date;
long day = tempd1/10000;
tempd1 -= day * 10000;
long month = tempd1/100;
long year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year);
_epoch = UNIX_EPOCH;
_offset_calculated = TRUE;
}
*/
}
}
return parsed;
}

78
libraries/AP_GPS/AP_GPS_MTK16.h
View File

@ -18,53 +18,53 @@
class AP_GPS_MTK16 : public GPS {
public:
AP_GPS_MTK16(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_MTK16(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
};
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
};
// #pragma pack(pop)
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xd0,
PREAMBLE2 = 0xdd,
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xd0,
PREAMBLE2 = 0xdd,
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
};
#endif // AP_GPS_MTK16_H

498
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -41,17 +41,17 @@
// the autodetection process.
//
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84
"";
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84
"";
// MediaTek init messages //////////////////////////////////////////////////////
//
@ -59,11 +59,11 @@ const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
// MediaTek-based GPS.
//
const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
"$PMTK330,0*2E\r\n" // datum = WGS84
"$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
"";
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
"$PMTK330,0*2E\r\n" // datum = WGS84
"$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
"";
// ublox init messages /////////////////////////////////////////////////////////
//
@ -75,10 +75,10 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
// and we don't know the baudrate.
//
const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
"";
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
"";
// NMEA message identifiers ////////////////////////////////////////////////////
//
@ -92,83 +92,83 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) :
GPS(s)
GPS(s)
{
FastSerial *fs = (FastSerial *)_port;
FastSerial *fs = (FastSerial *)_port;
// Re-open the port with enough receive buffering for the messages we expect
// and very little tx buffering, since we don't care about sending.
// Leave the port speed alone as we don't actually know at what rate we're running...
//
fs->begin(0, 200, 16);
// Re-open the port with enough receive buffering for the messages we expect
// and very little tx buffering, since we don't care about sending.
// Leave the port speed alone as we don't actually know at what rate we're running...
//
fs->begin(0, 200, 16);
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(void)
{
BetterStream *bs = (BetterStream *)_port;
BetterStream *bs = (BetterStream *)_port;
// send the SiRF init strings
bs->print_P((const prog_char_t *)_SiRF_init_string);
// send the SiRF init strings
bs->print_P((const prog_char_t *)_SiRF_init_string);
// send the MediaTek init strings
bs->print_P((const prog_char_t *)_MTK_init_string);
// send the MediaTek init strings
bs->print_P((const prog_char_t *)_MTK_init_string);
// send the ublox init strings
bs->print_P((const prog_char_t *)_ublox_init_string);
idleTimeout = 1200;
// send the ublox init strings
bs->print_P((const prog_char_t *)_ublox_init_string);
idleTimeout = 1200;
}
bool AP_GPS_NMEA::read(void)
{
int numc;
bool parsed = false;
int numc;
bool parsed = false;
numc = _port->available();
while (numc--) {
if (_decode(_port->read())) {
parsed = true;
}
}
return parsed;
numc = _port->available();
while (numc--) {
if (_decode(_port->read())) {
parsed = true;
}
}
return parsed;
}
bool AP_GPS_NMEA::_decode(char c)
{
bool valid_sentence = false;
bool valid_sentence = false;
switch (c) {
case ',': // term terminators
_parity ^= c;
case '\r':
case '\n':
case '*':
if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0;
valid_sentence = _term_complete();
}
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
return valid_sentence;
switch (c) {
case ',': // term terminators
_parity ^= c;
case '\r':
case '\n':
case '*':
if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0;
valid_sentence = _term_complete();
}
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
return valid_sentence;
case '$': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
return valid_sentence;
}
case '$': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
return valid_sentence;
}
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
_parity ^= c;
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
_parity ^= c;
return valid_sentence;
return valid_sentence;
}
//
@ -176,203 +176,203 @@ bool AP_GPS_NMEA::_decode(char c)
//
int AP_GPS_NMEA::_from_hex(char a)
{
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
}
unsigned long AP_GPS_NMEA::_parse_decimal()
{
char *p = _term;
unsigned long ret = 100UL * atol(p);
while (isdigit(*p))
++p;
if (*p == '.') {
if (isdigit(p[1])) {
ret += 10 * (p[1] - '0');
if (isdigit(p[2]))
ret += p[2] - '0';
}
}
return ret;
char *p = _term;
unsigned long ret = 100UL * atol(p);
while (isdigit(*p))
++p;
if (*p == '.') {
if (isdigit(p[1])) {
ret += 10 * (p[1] - '0');
if (isdigit(p[2]))
ret += p[2] - '0';
}
}
return ret;
}
unsigned long AP_GPS_NMEA::_parse_degrees()
{
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
// scan for decimal point or end of field
for (p = _term; isdigit(*p); p++)
;
q = _term;
// scan for decimal point or end of field
for (p = _term; isdigit(*p); p++)
;
q = _term;
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (int i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 100000UL + (min * 10000UL + frac_min) / 6;
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (int i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 100000UL + (min * 10000UL + frac_min) / 6;
}
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{
// handle the last term in a message
if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) {
if (_gps_data_good) {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
time = _new_time;
date = _new_date;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
ground_speed = _new_speed;
ground_course = _new_course;
fix = true;
break;
case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude;
time = _new_time;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count;
hdop = _new_hdop;
fix = true;
break;
case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed;
ground_course = _new_course;
// VTG has no fix indicator, can't change fix status
break;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about
// fix status.
fix = false;
}
}
// we got a good message
return true;
}
// we got a bad message, ignore it
return false;
}
// handle the last term in a message
if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) {
if (_gps_data_good) {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
time = _new_time;
date = _new_date;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
ground_speed = _new_speed;
ground_course = _new_course;
fix = true;
break;
case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude;
time = _new_time;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count;
hdop = _new_hdop;
fix = true;
break;
case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed;
ground_course = _new_course;
// VTG has no fix indicator, can't change fix status
break;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about
// fix status.
fix = false;
}
}
// we got a good message
return true;
}
// we got a bad message, ignore it
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
if (!strcmp_P(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC;
} else if (!strcmp_P(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA;
} else if (!strcmp_P(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
if (!strcmp_P(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC;
} else if (!strcmp_P(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA;
} else if (!strcmp_P(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal();
break;
// 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal();
break;
// time and date
//
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
_new_time = _parse_decimal();
break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// time and date
//
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
_new_time = _parse_decimal();
break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// location
//
case _GPS_SENTENCE_GPRMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal();
break;
// location
//
case _GPS_SENTENCE_GPRMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal();
break;
// course and speed
//
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
_new_course = _parse_decimal();
break;
}
}
// course and speed
//
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
_new_course = _parse_decimal();
break;
}
}
return false;
return false;
}

110
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -51,72 +51,72 @@
class AP_GPS_NMEA : public GPS
{
public:
/// Constructor
///
AP_GPS_NMEA(Stream *s);
/// Constructor
///
AP_GPS_NMEA(Stream *s);
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init();
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init();
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
virtual bool read();
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
virtual bool read();
private:
/// Coding for the GPS sentences that the parser handles
/// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.
_GPS_SENTENCE_GPRMC = 32,
_GPS_SENTENCE_GPGGA = 64,
_GPS_SENTENCE_GPVTG = 96,
_GPS_SENTENCE_OTHER = 0
_GPS_SENTENCE_GPRMC = 32,
_GPS_SENTENCE_GPGGA = 64,
_GPS_SENTENCE_GPVTG = 96,
_GPS_SENTENCE_OTHER = 0
};
/// Update the decode state machine with a new character
///
/// @param c The next character in the NMEA input stream
/// @returns True if processing the character has resulted in
/// an update to the GPS state
///
bool _decode(char c);
/// Update the decode state machine with a new character
///
/// @param c The next character in the NMEA input stream
/// @returns True if processing the character has resulted in
/// an update to the GPS state
///
bool _decode(char c);
/// Return the numeric value of an ascii hex character
///
/// @param a The character to be converted
/// @returns The value of the character as a hex digit
///
int _from_hex(char a);
/// Return the numeric value of an ascii hex character
///
/// @param a The character to be converted
/// @returns The value of the character as a hex digit
///
int _from_hex(char a);
/// Parses the current term as a NMEA-style decimal number with
/// up to two decimal digits.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 100.
///
unsigned long _parse_decimal();
/// Parses the current term as a NMEA-style decimal number with
/// up to two decimal digits.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 100.
///
unsigned long _parse_decimal();
/// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits.
///
/// This gives a theoretical resolution limit of around 18cm.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 10000.
///
unsigned long _parse_degrees();
/// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits.
///
/// This gives a theoretical resolution limit of around 18cm.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 10000.
///
unsigned long _parse_degrees();
/// Processes the current term when it has been deemed to be
/// complete.
///
/// Each GPS message is broken up into terms separated by commas.
/// Each term is then processed by this function as it is received.
///
/// @returns True if completing the term has resulted in
/// an update to the GPS state.
bool _term_complete();
/// Processes the current term when it has been deemed to be
/// complete.
///
/// Each GPS message is broken up into terms separated by commas.
/// Each term is then processed by this function as it is received.
///
/// @returns True if completing the term has resulted in
/// an update to the GPS state.
bool _term_complete();
uint8_t _parity; ///< NMEA message checksum accumulator
bool _is_checksum_term; ///< current term is the checksum

8
libraries/AP_GPS/AP_GPS_None.h
View File

@ -8,8 +8,10 @@
class AP_GPS_None : public GPS
{
public:
AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {};
virtual bool read(void) { return false; };
AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {};
virtual bool read(void) {
return false;
};
};
#endif

262
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -19,8 +19,8 @@
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
//
static uint8_t init_messages[] = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
};
// Constructors ////////////////////////////////////////////////////////////////
@ -29,18 +29,18 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
}
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_SIRF::init(void)
{
_port->flush();
{
_port->flush();
// For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode
// before calling us.
// For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode
// before calling us.
// send SiRF binary setup messages
_port->write(init_messages, sizeof(init_messages));
idleTimeout = 1200;
// send SiRF binary setup messages
_port->write(init_messages, sizeof(init_messages));
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -55,143 +55,143 @@ AP_GPS_SIRF::init(void)
bool
AP_GPS_SIRF::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
while(numc--) {
numc = _port->available();
while(numc--) {
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
switch(_step){
switch(_step) {
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message length
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_payload_length = (uint16_t)data << 8;
break;
case 3:
_step++;
_payload_length |= data;
_payload_counter = 0;
_checksum = 0;
break;
// Message length
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_payload_length = (uint16_t)data << 8;
break;
case 3:
_step++;
_payload_length |= data;
_payload_counter = 0;
_checksum = 0;
break;
// Message header processing
//
// We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them.
//
case 4:
_step++;
_accumulate(data);
_payload_length--;
_gather = false;
switch(data) {
case MSG_GEONAV:
if (_payload_length == sizeof(sirf_geonav)) {
_gather = true;
_msg_id = data;
}
break;
}
break;
// Message header processing
//
// We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them.
//
case 4:
_step++;
_accumulate(data);
_payload_length--;
_gather = false;
switch(data) {
case MSG_GEONAV:
if (_payload_length == sizeof(sirf_geonav)) {
_gather = true;
_msg_id = data;
}
break;
}
break;
// Receive message data
//
// Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly
// to the preamble detector rather than bothering with
// the checksum logic.
//
case 5:
if (_gather) { // gather data if requested
_accumulate(data);
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
} else {
if (++_payload_counter == _payload_length)
_step = 0;
}
break;
// Receive message data
//
// Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly
// to the preamble detector rather than bothering with
// the checksum logic.
//
case 5:
if (_gather) { // gather data if requested
_accumulate(data);
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
} else {
if (++_payload_counter == _payload_length)
_step = 0;
}
break;
// Checksum and message processing
//
case 6:
_step++;
if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n");
_step = 0;
}
break;
case 7:
_step = 0;
if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n");
break;
}
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return(parsed);
// Checksum and message processing
//
case 6:
_step++;
if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n");
_step = 0;
}
break;
case 7:
_step = 0;
if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n");
break;
}
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return(parsed);
}
bool
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);
latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl);
ground_speed = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed > 50)
ground_course = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites;
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);
latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl);
ground_speed = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed > 50)
ground_course = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites;
return true;
}
return false;
return true;
}
return false;
}
void
AP_GPS_SIRF::_accumulate(uint8_t val)
{
_checksum = (_checksum + val) & 0x7fff;
_checksum = (_checksum + val) & 0x7fff;
}

130
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -17,80 +17,80 @@
class AP_GPS_SIRF : public GPS {
public:
AP_GPS_SIRF(Stream *s);
AP_GPS_SIRF(Stream *s);
virtual void init();
virtual bool read();
virtual void init();
virtual bool read();
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct sirf_geonav {
uint16_t fix_invalid;
uint16_t fix_type;
uint16_t week;
uint32_t time;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint16_t second;
uint32_t satellites_used;
int32_t latitude;
int32_t longitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
int8_t map_datum;
int16_t ground_speed;
int16_t ground_course;
int16_t res1;
int16_t climb_rate;
uint16_t heading_rate;
uint32_t horizontal_position_error;
uint32_t vertical_position_error;
uint32_t time_error;
int16_t horizontal_velocity_error;
int32_t clock_bias;
uint32_t clock_bias_error;
int32_t clock_drift;
uint32_t clock_drift_error;
uint32_t distance;
uint16_t distance_error;
uint16_t heading_error;
uint8_t satellites;
uint8_t hdop;
uint8_t mode_info;
};
struct sirf_geonav {
uint16_t fix_invalid;
uint16_t fix_type;
uint16_t week;
uint32_t time;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint16_t second;
uint32_t satellites_used;
int32_t latitude;
int32_t longitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
int8_t map_datum;
int16_t ground_speed;
int16_t ground_course;
int16_t res1;
int16_t climb_rate;
uint16_t heading_rate;
uint32_t horizontal_position_error;
uint32_t vertical_position_error;
uint32_t time_error;
int16_t horizontal_velocity_error;
int32_t clock_bias;
uint32_t clock_bias_error;
int32_t clock_drift;
uint32_t clock_drift_error;
uint32_t distance;
uint16_t distance_error;
uint16_t heading_error;
uint8_t satellites;
uint8_t hdop;
uint8_t mode_info;
};
// #pragma pack(pop)
enum sirf_protocol_bytes {
PREAMBLE1 = 0xa0,
PREAMBLE2 = 0xa2,
POSTAMBLE1 = 0xb0,
POSTAMBLE2 = 0xb3,
MSG_GEONAV = 0x29
};
enum sirf_fix_type {
FIX_3D = 0x6,
FIX_MASK = 0x7
};
enum sirf_protocol_bytes {
PREAMBLE1 = 0xa0,
PREAMBLE2 = 0xa2,
POSTAMBLE1 = 0xb0,
POSTAMBLE2 = 0xb3,
MSG_GEONAV = 0x29
};
enum sirf_fix_type {
FIX_3D = 0x6,
FIX_MASK = 0x7
};
// State machine state
uint8_t _step;
uint16_t _checksum;
bool _gather;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _msg_id;
// State machine state
uint8_t _step;
uint16_t _checksum;
bool _gather;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _msg_id;
// Message buffer
union {
sirf_geonav nav;
uint8_t bytes[];
} _buffer;
// Message buffer
union {
sirf_geonav nav;
uint8_t bytes[];
} _buffer;
bool _parse_gps(void);
void _accumulate(uint8_t val);
bool _parse_gps(void);
void _accumulate(uint8_t val);
};
#endif // AP_GPS_SIRF_h

38
libraries/AP_GPS/AP_GPS_Shim.h
View File

@ -17,31 +17,31 @@
class AP_GPS_Shim : public GPS
{
public:
AP_GPS_Shim() : GPS(NULL) {}
AP_GPS_Shim() : GPS(NULL) {}
virtual void init(void) {};
virtual bool read(void) {
bool updated = _updated;
_updated = false;
return updated;
}
virtual void init(void) {};
virtual bool read(void) {
bool updated = _updated;
_updated = false;
return updated;
}
/// Set-and-mark-updated macro for the public member variables; each instance
/// defines a member function set_<variable>(<type>)
///
/// Set-and-mark-updated macro for the public member variables; each instance
/// defines a member function set_<variable>(<type>)
///
#define __GPS_SHIM_SET(__type, __name) void set_##__name(__type v) { __name = v; _updated = true; }
__GPS_SHIM_SET(long, time);
__GPS_SHIM_SET(long, latitude);
__GPS_SHIM_SET(long, longitude);
__GPS_SHIM_SET(long, altitude);
__GPS_SHIM_SET(long, ground_speed);
__GPS_SHIM_SET(long, ground_course);
__GPS_SHIM_SET(long, speed_3d);
__GPS_SHIM_SET(int, hdop);
__GPS_SHIM_SET(long, time);
__GPS_SHIM_SET(long, latitude);
__GPS_SHIM_SET(long, longitude);
__GPS_SHIM_SET(long, altitude);
__GPS_SHIM_SET(long, ground_speed);
__GPS_SHIM_SET(long, ground_course);
__GPS_SHIM_SET(long, speed_3d);
__GPS_SHIM_SET(int, hdop);
#undef __GPS_SHIM_SET
private:
bool _updated; ///< set anytime a member is updated, cleared when read() returns true
bool _updated; ///< set anytime a member is updated, cleared when read() returns true
};
#endif // AP_GPS_HIL_H

276
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -23,13 +23,13 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
void
AP_GPS_UBLOX::init(void)
{
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
_port->flush();
_epoch = TIME_OF_WEEK;
idleTimeout = 1200;
_port->flush();
_epoch = TIME_OF_WEEK;
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -44,122 +44,122 @@ AP_GPS_UBLOX::init(void)
bool
AP_GPS_UBLOX::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
switch(_step){
switch(_step) {
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_gather = false; // class is not interesting, discard
}
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
if (_gather) { // if class was interesting
switch(data) {
case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh);
break;
case MSG_STATUS:
_expect = sizeof(ubx_nav_status);
break;
case MSG_SOL:
_expect = sizeof(ubx_nav_solution);
break;
case MSG_VELNED:
_expect = sizeof(ubx_nav_velned);
break;
default:
_gather = false; // message is not interesting
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect)
_gather = false;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_gather = false; // class is not interesting, discard
}
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
if (_gather) { // if class was interesting
switch(data) {
case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh);
break;
case MSG_STATUS:
_expect = sizeof(ubx_nav_status);
break;
case MSG_SOL:
_expect = sizeof(ubx_nav_solution);
break;
case MSG_VELNED:
_expect = sizeof(ubx_nav_velned);
break;
default:
_gather = false; // message is not interesting
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect)
_gather = false;
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data)
_step = 0; // bad checksum
break;
case 8:
_step = 0;
if (_ck_b != data)
break; // bad checksum
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data)
_step = 0; // bad checksum
break;
case 8:
_step = 0;
if (_ck_b != data)
break; // bad checksum
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return parsed;
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return parsed;
}
// Private Methods /////////////////////////////////////////////////////////////
@ -167,28 +167,28 @@ AP_GPS_UBLOX::read(void)
bool
AP_GPS_UBLOX::_parse_gps(void)
{
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
default:
return false;
}
return true;
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
default:
return false;
}
return true;
}

184
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -19,106 +19,106 @@ class AP_GPS_UBLOX : public GPS
{
public:
// Methods
AP_GPS_UBLOX(Stream *s);
virtual void init();
virtual bool read();
AP_GPS_UBLOX(Stream *s);
virtual void init();
virtual bool read();
private:
// u-blox UBX protocol essentials
// u-blox UBX protocol essentials
// XXX this is being ignored by the compiler #pragma pack(1)
struct ubx_nav_posllh {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
};
struct ubx_nav_status {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
};
struct ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
};
struct ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
struct ubx_nav_posllh {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
};
struct ubx_nav_status {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
};
struct ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
};
struct ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
// // #pragma pack(pop)
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x1,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
};
enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1
};
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x1,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
};
enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _msg_id;
bool _gather;
uint16_t _expect;
uint16_t _payload_length;
uint16_t _payload_counter;
// State machine state
uint8_t _step;
uint8_t _msg_id;
bool _gather;
uint16_t _expect;
uint16_t _payload_length;
uint16_t _payload_counter;
// Receive buffer
union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
uint8_t bytes[];
} _buffer;
// Receive buffer
union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
uint8_t bytes[];
} _buffer;
// Buffer parse & GPS state update
bool _parse_gps();
// Buffer parse & GPS state update
bool _parse_gps();
};
#endif

42
libraries/AP_GPS/GPS.cpp
View File

@ -6,30 +6,30 @@
void
GPS::update(void)
{
bool result;
bool result;
// call the GPS driver to process incoming data
result = read();
// call the GPS driver to process incoming data
result = read();
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((millis() - _idleTimer) > idleTimeout) {
_status = NO_GPS;
init();
// reset the idle timer
_idleTimer = millis();
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((millis() - _idleTimer) > idleTimeout) {
_status = NO_GPS;
valid_read = true;
new_data = true;
init();
// reset the idle timer
_idleTimer = millis();
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
// reset the idle timer
_idleTimer = millis();
}
valid_read = true;
new_data = true;
// reset the idle timer
_idleTimer = millis();
}
}
void
@ -42,5 +42,5 @@ GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude,
void
GPS::_error(const char *msg)
{
Serial.println(msg);
Serial.println(msg);
}

288
libraries/AP_GPS/GPS.h
View File

@ -15,189 +15,193 @@ class GPS
{
public:
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
void (*callback)(unsigned long t);
void (*callback)(unsigned long t);
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked
};
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked
};
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) { return _status; }
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) {
return _status;
}
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) { return _epoch; }
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) {
return _epoch;
}
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(void) = 0;
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(void) = 0;
// Properties
long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
// Properties
long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
// Deprecated properties
bool fix; ///< true if we have a position fix (use ::status instead)
bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
// Deprecated properties
bool fix; ///< true if we have a position fix (use ::status instead)
bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
// Debug support
bool print_errors; ///< deprecated
// Debug support
bool print_errors; ///< deprecated
// HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
// HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
/// Time in milliseconds after which we will assume the GPS is no longer
/// sending us updates and attempt a re-init.
///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate.
///
unsigned long idleTimeout;
/// Time in milliseconds after which we will assume the GPS is no longer
/// sending us updates and attempt a re-init.
///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate.
///
unsigned long idleTimeout;
protected:
Stream *_port; ///< port the GPS is attached to
Stream *_port; ///< port the GPS is attached to
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param s Stream connected to the GPS module.
///
GPS(Stream *s) : _port(s) {};
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param s Stream connected to the GPS module.
///
GPS(Stream *s) : _port(s) {};
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes);
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
private:
/// Last time that the GPS driver got a good packet from the GPS
///
unsigned long _idleTimer;
/// Last time that the GPS driver got a good packet from the GPS
///
unsigned long _idleTimer;
/// Our current status
GPS_Status _status;
/// Our current status
GPS_Status _status;
};
inline long
GPS::_swapl(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return(u.v);
return(u.v);
}
inline int16_t
GPS::_swapi(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
u.b[0] = b[1];
u.b[1] = b[0];
return(u.v);
return(u.v);
}
#endif

62
libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde
View File

@ -19,41 +19,41 @@ AP_GPS_406 gps(&Serial1);
void setup()
{
tone(A6, 1000, 200);
tone(A6, 1000, 200);
Serial.begin(38400, 16, 128);
Serial1.begin(57600, 128, 16);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400, 16, 128);
Serial1.begin(57600, 128, 16);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}

42
libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
View File

@ -18,31 +18,31 @@ AP_GPS_Auto GPS(&Serial1, &gps);
void setup()
{
Serial.begin(115200);
Serial1.begin(38400);
Serial.begin(115200);
Serial1.begin(38400);
Serial.println("GPS AUTO library test");
gps = &GPS;
gps->init();
Serial.println("GPS AUTO library test");
gps = &GPS;
gps->init();
}
void loop()
{
gps->update();
if (gps->new_data){
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
gps->update();
if (gps->new_data) {
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
}

60
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -19,39 +19,39 @@ AP_GPS_MTK gps(&Serial1);
void setup()
{
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}

81
libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde
View File

@ -18,54 +18,55 @@ GPS *gps = &NMEA_gps;
#define T7 10000000
const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble
0x00, 0x18, // message length
0x81, 0x02, // switch to NMEA
0x01, 0x01, // GGA on with checksum
0x00, 0x01, // GLL off
0x00, 0x01, // GSA off
0x00, 0x01, // GSV off
0x01, 0x01, // RMC on with checksum
0x01, 0x01, // VTG on with checksum
0x00, 0x01, // MSS off
0x00, 0x01, // EPE off
0x00, 0x01, // ZPA off
0x00, 0x00, // pad
0x96, 0x00, // 38400
0x01, 0x25, // checksum TBD
0xb0, 0xb3 }; // postamble
0x00, 0x18, // message length
0x81, 0x02, // switch to NMEA
0x01, 0x01, // GGA on with checksum
0x00, 0x01, // GLL off
0x00, 0x01, // GSA off
0x00, 0x01, // GSV off
0x01, 0x01, // RMC on with checksum
0x01, 0x01, // VTG on with checksum
0x00, 0x01, // MSS off
0x00, 0x01, // EPE off
0x00, 0x01, // ZPA off
0x00, 0x00, // pad
0x96, 0x00, // 38400
0x01, 0x25, // checksum TBD
0xb0, 0xb3
}; // postamble
void setup()
{
Serial.begin(38400);
Serial1.begin(38400);
Serial.begin(38400);
Serial1.begin(38400);
// try to coerce a SiRF unit that's been traumatized by
// AP_GPS_AUTO back into NMEA mode so that we can test
// it.
for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++)
Serial1.write(sirf_to_nmea[i]);
// try to coerce a SiRF unit that's been traumatized by
// AP_GPS_AUTO back into NMEA mode so that we can test
// it.
for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++)
Serial1.write(sirf_to_nmea[i]);
Serial.println("GPS NMEA library test");
gps->init();
Serial.println("GPS NMEA library test");
gps->init();
}
void loop()
{
gps->update();
if (gps->new_data){
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
gps->update();
if (gps->new_data) {
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
}

60
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -19,39 +19,39 @@ AP_GPS_UBLOX gps(&Serial1);
void setup()
{
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}