Paul,
Thanks for the reply!
Overall what I’m trying to do is get the RP2040 and GPS to guide a finless rocket straight up, so locating the launch lat/lon is critical. I can wait on getting a good fix: it’s worth it. However a 3.5 second period lacking guidance could be disastrous on a 20 second burn.
I’ll include the code, though it’s a total hack job in its current state.
Thanks again, and best regards,
Roger
/*
Configuring the GNSS to automatically send position reports over I2C
By: Nathan Seidle and Thorsten von Eicken
SparkFun Electronics
License: MIT. See license file for more information but you can
basically do whatever you want with this code.
This example shows how to configure the U-Blox GNSS the send navigation reports automatically
and retrieving the latest one via getPVT. This eliminates the blocking in getPVT while the GNSS
produces a fresh navigation solution at the expense of returning a slighly old solution.
This can be used over serial or over I2C, this example shows the I2C use. With serial the GNSS
simply outputs the UBX_NAV_PVT packet. With I2C it queues it into its internal I2C buffer (4KB in
size?) where it can be retrieved in the next I2C poll.
Feel like supporting open source hardware?
Buy a board from SparkFun!
ZED-F9P RTK2: [https://www.sparkfun.com/products/15136](https://www.sparkfun.com/products/15136)
NEO-M8P RTK: [https://www.sparkfun.com/products/15005](https://www.sparkfun.com/products/15005)
SAM-M8Q: [https://www.sparkfun.com/products/15106](https://www.sparkfun.com/products/15106)
Hardware Connections:
Plug a Qwiic cable into the GNSS and a BlackBoard
If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper ([https://www.sparkfun.com/products/14425](https://www.sparkfun.com/products/14425))
Open the serial monitor at 115200 baud to see the output
*/
#include <Wire.h> //Needed for I2C to GNSS
#include <Arduino_LSM6DSOX.h>
// #include <WiFiNINA.h>
#include "SparkFun_u-blox_GNSS_Arduino_Library.h" //[http://librarymanager/All#SparkFun_u-blox_GNSS](http://librarymanager/All#SparkFun_u-blox_GNSS)
SFE_UBLOX_GNSS myGNSS;
#include <SD.h>
//We always need to set the CS Pin
int CS_pin = 4; // 10;
// int pow_pin = 8;
int record = 0;
long milly;
// first KF
long lat = 449242185;
long lon = -934849672;
float p = 100;
float r1 = 100;
float K = 0.5;
// Const acc, const vel KF
long PadLat = 449241984; // Mtka dddddddd
long PadLong = -934849672;
float pl = float(PadLat);
float cosLat = cos(pl / 10000000.0 * PI / 180);
long etime;
float xN = 0; // Initial postion guess - on the pad
float xE = 0;
float vN = 0.0;
float vE = 0.0; // Initial velocity - still
float accN = 0.0;
float accE = 0.0; // Initial acceleration - still
float PxN = 6.01 * 6.01; // Initial position uncertainty
float PxE = PxN;
float PvN = 1; //0.5 * 0.5; // also m or m^2
float PvE = 1;
float rx = 2.0; // * 2.0; // m! GPS position uncertainty - 2m
float rv = 0.05 * 0.05; // GPS velocity uncertainty - 0.05 m/s
float ra = (0.061 / 1000 * 9.8) * (0.061 / 1000 * 9.8); // 2040 accel variance - 0.061mg
float KvN; // Kalman gain velocity North
float KvE;
float KxN; // Kalman gain North position
float KxE;
float dT; // time increment
float x, y, z; // 2040 accels - body frame
float xcorr, ycorr, zcorr; // accelerometer corrections
File dataKFile;
#include <Adafruit_BNO055.h> // Thanks, Adafruit!
#include <Adafruit_Sensor.h>
// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
// id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);
// Quaternion data
imu::Quaternion quat, padquat, nowquat;
void setup() {
Serial.begin(19200);
while (!Serial)
; //Wait for user to open terminal
// Serial.println("SparkFun u-blox Example");
pinMode(4, OUTPUT);
digitalWrite(4, HIGH);
delay(1);
// now it's safe to use SD.begin(4) and Ethernet.begin()
// Serial.begin(9600);
// delay(1000);
Serial.println("Initializing Card");
//CS Pin is an output
if (!SD.begin(CS_pin)) {
Serial.println("Card Failure");
return;
}
Serial.println("Card Ready");
Wire.begin();
if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
{
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
while (1)
;
}
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
myGNSS.setNavigationFrequency(20); // 2); //Produce two solutions per second max 25
myGNSS.setAutoPVT(true); //Tell the GNSS to "send" each solution
//myGNSS.saveConfiguration(); //Optional: Save the current settings to flash and BBR
/* Initialise the sensor */
if (!bno.begin()) {
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while (1)
;
}
delay(1000);
/* Use external crystal for better accuracy */
bno.setExtCrystalUse(true);
Serial.println("Getting data points");
File dataFile = SD.open("LLH.csv", FILE_WRITE);
if (dataFile) {
dataFile.println("Record, Millis, Lat, Lon, Alt, VelN, VelE, accY, accZ"); // -x is flight direction
dataFile.close();
}
dataKFile = SD.open("KF.csv", FILE_WRITE);
if (dataKFile) {
dataKFile.println("Record, Millis, y-ycorr, accN, velN, PvN, ra, KvelN, xN, PxN, KxN, z-zcorr, accE, velE, PvE, ra, KvelE, xE, PxE, KxE"); // -x is flight direction
dataKFile.close();
}
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1)
;
}
// Wait for GPS lock
long latitude = 0;
long longitude = 0;
int PDOP = 1000;
while (PDOP > 250) { // latitude == 0 | longitude == 0) {
Serial.print("Waiting for GPS lock ");
Serial.println(millis());
if (myGNSS.getPVT() && (myGNSS.getInvalidLlh() == false)) {
// latitude = myGNSS.getLatitude();
// longitude = myGNSS.getLongitude();
PDOP = myGNSS.getPDOP();
}
// delay(1000);
}
// delay(10000); // give it some more time
int count = 0;
/*
PadLat = 0;
PadLong = 0;
*/
Serial.println("Finding myself");
while (K > 0.01) {
if (myGNSS.getPVT() && (myGNSS.getInvalidLlh() == false)) {
/* if (abs(PadLat - 449241057) < 10000 & abs(PadLong - -934849672) < 15000) { // specific to tonka ...
PadLat += myGNSS.getLatitude();
PadLong += myGNSS.getLongitude();
count++;
*/
latitude = myGNSS.getLatitude();
// old KF
// Serial.println();
K = p / (p + r1);
lat = lat + K * (latitude - lat);
p = (1 - K) * p;
/*
Serial.print("Lat ");
Serial.print(lat);
Serial.print(" K ");
Serial.println(K, 4);
*/
}
PadLat = lat;
}
K = 0.5;
p = 100; // reset for longitude KF
while (K > 0.01) {
if (myGNSS.getPVT() && (myGNSS.getInvalidLlh() == false)) {
longitude = myGNSS.getLongitude();
// old KF
// Serial.println();
K = p / (p + r1);
lon = lon + K * (longitude - lon);
p = (1 - K) * p;
/*
Serial.print("Lat ");
Serial.print(lat);
Serial.print(" K ");
Serial.println(K, 4);
*/
}
}
PadLat = lat;
PadLong = lon;
pl = float(PadLat);
cosLat = cos(pl / 10000000 * PI / 180);
Serial.print("cosLat: ");
Serial.println(cosLat, 5);
/*PadLat /= count;
PadLong /= count; */
Serial.print("Pad Lat: ");
Serial.print(PadLat);
Serial.print(" Pad Long: ");
Serial.println(PadLong);
/*
Serial.print(" count: ");
Serial.println(count);
*/
for (int i = 0; i < 200; i++) {
if (myGNSS.getPVT() && (myGNSS.getInvalidLlh() == false)) {
File dataFile = SD.open("LLH.csv", FILE_WRITE);
delay(10);
if (dataFile) {
milly = millis();
dataFile.print(record);
dataFile.print(",");
dataFile.print(milly);
dataFile.print(",");
latitude = myGNSS.getLatitude();
dataFile.print(latitude);
dataFile.print(",");
longitude = myGNSS.getLongitude();
dataFile.print(longitude);
dataFile.print(",");
long altitude = myGNSS.getAltitude();
dataFile.print(altitude);
dataFile.print(",");
int nedNorthVel = myGNSS.getNedNorthVel(); // mm/s
dataFile.print(nedNorthVel);
dataFile.print(",");
int nedEastVel = myGNSS.getNedEastVel();
dataFile.print(nedEastVel);
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(x, y, z);
// dataFile.print(x);
dataFile.print(",");
dataFile.print(y, 5);
dataFile.print(",");
dataFile.print(z, 5);
}
dataFile.println();
delay(10);
dataFile.close();
// Serial.println(record);
record++;
delay(10);
} else {
Serial.println("Couldn't open log file");
}
} else {
delay(10);
}
}
// Calibrate accelerometers
xcorr = 0.0;
ycorr = 0.0;
zcorr = 0.0;
count = 0;
for (int i = 0; i < 2000; i++) {
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(x, y, z);
xcorr += x;
ycorr = ycorr + y;
zcorr += z;
count++;
}
}
xcorr /= count;
ycorr = ycorr / count;
zcorr /= count;
Serial.print("ycorr ");
Serial.println(ycorr, 5);
etime = millis();
// Quaternion data
padquat = bno.getQuat();
printquat("pad", padquat);
}
boolean once = true;
void loop() {
// TBD
}