DressOrc/sppControlSystem
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

zaestghbaethb

Browse source
This commit is contained in:
Rosia E Evans 2023-05-06 17:03:21 +01:00
parent 43c63acd4d
commit f6307e8cec
4 changed files with 46445 additions and 222 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
debug.cfg Normal file
View file

@ -0,0 +1,14 @@
# SPDX-License-Identifier: GPL-2.0-or-later
#
# Example OpenOCD configuration file for ESP32-WROVER-KIT board.
#
# For example, OpenOCD can be started for ESP32 debugging on
#
# openocd -f board/esp32-wrover-kit-3.3v.cfg
#
# Source the JTAG interface configuration file
source [find interface/ftdi/esp32_devkitj_v1.cfg]
set ESP32_FLASH_VOLTAGE 3.3
# Source the ESP32 configuration file
source [find target/esp32.cfg]

19
debug_custom.json Normal file
View file

@ -0,0 +1,19 @@
{
"name":"Arduino on ESP32",
"toolchainPrefix":"xtensa-esp32-elf",
"svdFile":"esp32.svd",
"request":"attach",
"postAttachCommands":[
"set remote hardware-watchpoint-limit 2",
"monitor reset halt",
"monitor gdb_sync",
"thb setup",
"c"
],
"overrideRestartCommands":[
"monitor reset halt",
"monitor gdb_sync",
"thb setup",
"c"
]
}

46087
esp32.svd Normal file
View file

File diff suppressed because it is too large Load diff

513
sppControlSystem.ino
View file

@ -14,28 +14,46 @@
Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
Copyright Colin Sauze Copyright Colin Sauze
libraries required:
timelib
esp32servo
tinygps
*/ */
#include <stdio.h> #include <stdio.h>
#include <Servo.h> //#include <Servo.h>
#include <ESP32Servo.h>
#include <Wire.h> #include <Wire.h>
#include <SoftwareSerial.h> #include <HardwareSerial.h>
#include <math.h> #include <math.h>
#include "Time.h"
#include <WiFi.h>
#include <WiFiAP.h>
#include <AsyncUDP.h>
//Time.h became TimeLib.h as of version 1.6.1
#include "TimeLib.h"
#include "TinyGPS.h" #include "TinyGPS.h"
#define HMC6343_ADDRESS 0x19 #define HMC6343_ADDRESS 0x19
#define HMC6343_HEADING_REG 0x50 #define HMC6343_HEADING_REG 0x50
#define GPS_ENABLE_PIN 12 #define GPS_ENABLE_PIN 12
#define CMPS12_ADDRESS 0x60
#define GPS_READ_INTERVAL 15 //how many seconds to leave between GPS reads #define GPS_READ_INTERVAL 15 //how many seconds to leave between GPS reads
#define WP_THRESHOLD 15 //how close (in metres) should we get before we change waypoint? #define WP_THRESHOLD 15 //how close (in metres) should we get before we change waypoint?
#define rad2deg(x) (180 / M_PI) * x #define rad2deg(x) (180/M_PI) * x
#define deg2rad(x) x *M_PI / 180 #define deg2rad(x) x * M_PI/180
Servo rudderServo; // create servo object to control a servo Servo rudderServo; // create servo object to control a servo
SoftwareSerial myDebug(7, 8);
//SoftwareSerial myDebug(7, 8);
HardwareSerial myDebug(1);
TinyGPS gps; TinyGPS gps;
@ -49,39 +67,45 @@ TinyGPS gps;
#define LON 7 #define LON 7
#define TIME 8 #define TIME 8
#define DEBUG_CRITICAL 1 //really important messages that we don't want to ignore and are prepared to sacrifice execution speed to see #define DEBUG_CRITICAL 1 //really important messages that we don't want to ignore and are prepared to sacrifice execution speed to see
#define DEBUG_IMPORTANT 2 //fairly important messages that we probably want to see, but might cause issues with execution speed #define DEBUG_IMPORTANT 2 //fairly important messages that we probably want to see, but might cause issues with execution speed
#define DEBUG_MINOR 3 //less important messages that we can safely turn off to improve execution speed #define DEBUG_MINOR 3 //less important messages that we can safely turn off to improve execution speed
#define DEBUG_THRESHOLD DEBUG_IMPORTANT //set to 0 to show no debugging messages #define DEBUG_THRESHOLD DEBUG_MINOR //set to 0 to show no debugging messages
byte ledState = 0; const char *ssid = "boat";
AsyncUDP udp;
byte ledState=0;
struct Data { struct Data{
uint16_t heading; uint16_t heading;
uint16_t wind_dir; uint16_t wind_dir;
int8_t roll; int8_t roll;
int8_t pitch; int8_t pitch;
int8_t rudder; int8_t rudder;
byte sail; byte sail;
float lat; float lat;
float lon; float lon;
long unixtime; long unixtime;
} state; }
state;
//make printf work //make printf work
static FILE uartout = { static FILE uartout = {
0 0}
}; ;
static int uart_putchar(char c, FILE *stream) { static int uart_putchar (char c, FILE *stream)
myDebug.write(c); {
return 0; myDebug.write(c) ;
return 0 ;
} }
static void say(byte level, char *msg) { static void say(byte level, char* msg)
if (level <= DEBUG_THRESHOLD) { {
if(level<=DEBUG_THRESHOLD)
{
myDebug.print("Debug"); myDebug.print("Debug");
myDebug.print(level); myDebug.print(level);
myDebug.print(": [Ctrl] "); myDebug.print(": [Ctrl] ");
@ -90,62 +114,56 @@ static void say(byte level, char *msg) {
} }
//debugging printf that prepends "Debug:" to everything and can be easily turned off //debugging printf that prepends "Debug:" to everything and can be easily turned off
void dprintf(byte level, const char *fmt, ...) { void dprintf(byte level, const char *fmt, ...)
{
char outbuf[1024];
if (level <= DEBUG_THRESHOLD) { if(level<=DEBUG_THRESHOLD)
printf("Debug%d: [Ctrl] ", level); {
printf("Debug%d: [Ctrl] ",level);
va_list ap; va_list ap;
va_start(ap, fmt); va_start(ap, fmt);
vsnprintf(outbuf,1024,fmt, ap);
vprintf(fmt, ap); myDebug.print(outbuf);
va_end(ap); va_end(ap);
} }
} }
void dprintf2(const char *fmt, ...) {
//printf("Debug%d: [Ctrl] ",level); void setup()
va_list ap; {
va_start(ap, fmt);
printf(fmt, ap);
va_end(ap);
}
void setup() {
//Serial.begin(9600); //baud rate makes no difference on 32u4 //Serial.begin(9600); //baud rate makes no difference on 32u4
//for GPS, we communicate with the GPS through the Serial Port Serial.begin(4800, SERIAL_8N1, 16, 17); //for GPS // 17 tx, 16 rx
//so, using Serial.println() will actually send messages to the GPS
Serial.begin(4800);
myDebug.begin(4800); //debug UART myDebug.begin(4800, SERIAL_8N1, 3, 1); //debug UART on GPIO 6 and 7
say(DEBUG_MINOR, "Debug serial system set up");
say(DEBUG_CRITICAL, "Control system start up"); say(DEBUG_CRITICAL,"Control system start up");
//required for printf //required for printf
fdev_setup_stream(&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE); //but not on ESP32
stdout = &uartout; //fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
dprintf(DEBUG_IMPORTANT, "Printf configured \r\n"); //stdout = &uartout ;
dprintf(DEBUG_IMPORTANT,"Printf configured \r\n");
delay(5000); delay(5000);
say(DEBUG_IMPORTANT, "Setting up servos..."); say(DEBUG_IMPORTANT,"Setting up servos...");
//Use .attach for setting up connection to the servo //Use .attach for setting up connection to the servo
rudderServo.attach(5, 1060, 1920); // Attach, with the output limited rudderServo.attach(5, 1060, 1920); // Attach, with the output limited
// between 1000 and 2000 ms // between 1000 and 2000 ms
rudderServo.writeMicroseconds(1500); rudderServo.writeMicroseconds(1500);
say(DEBUG_IMPORTANT, "Done"); say(DEBUG_IMPORTANT,"Done");
say(DEBUG_IMPORTANT, "Setting up I2C..."); say(DEBUG_IMPORTANT,"Setting up I2C...");
Wire.begin(); // Initialise i2c for compass Wire.begin(); // Initialise i2c for compass
say(DEBUG_IMPORTANT, "Done"); say(DEBUG_IMPORTANT,"Done");
say(DEBUG_IMPORTANT, "Setting up GPS..."); say(DEBUG_IMPORTANT,"Setting up GPS...");
pinMode(GPS_ENABLE_PIN, OUTPUT); //GPS on/off line pinMode(GPS_ENABLE_PIN, OUTPUT); //GPS on/off line
//setup GPS //setup GPS
digitalWrite(GPS_ENABLE_PIN, 1); digitalWrite(GPS_ENABLE_PIN,1);
delay(1000); delay(1000);
//turn off VTG //turn off VTG
Serial.println("$PSRF103,05,00,00,01*21\r"); Serial.println("$PSRF103,05,00,00,01*21\r");
@ -167,17 +185,25 @@ void setup() {
//leave GPS on to get its initial fix //leave GPS on to get its initial fix
//digitalWrite(GPS_ENABLE_PIN,0); //digitalWrite(GPS_ENABLE_PIN,0);
say(DEBUG_IMPORTANT, "Done");
say(DEBUG_IMPORTANT, "Setup Complete\n"); //setup WiFi
say(DEBUG_IMPORTANT,"setting up WiFi");
WiFi.softAP(ssid);
IPAddress myIP = WiFi.softAPIP();
say(DEBUG_IMPORTANT,"Done");
say(DEBUG_IMPORTANT,"Setup Complete\n");
} }
//computes an NMEA checksum //computes an NMEA checksum
byte compute_checksum(byte *data, byte length) { byte compute_checksum(char *data,byte length)
byte computed_checksum = 0; {
byte computed_checksum=0;
for (byte i = 0; i < length; i++) { for (byte i = 0; i < length; i++)
computed_checksum = (byte)computed_checksum ^ data[i]; {
computed_checksum = (byte)computed_checksum ^ (byte)data[i];
} }
return computed_checksum; return computed_checksum;
@ -185,264 +211,341 @@ byte compute_checksum(byte *data, byte length) {
//reads heading from HMC6343 compass //reads heading from HMC6343 compass
int readCompass() { int readCompass() {
byte buf[6]; unsigned char high_byte, low_byte, angle8;
char pitch, roll;
unsigned int heading;
Wire.beginTransmission(HMC6343_ADDRESS); // Start communicating with the HMC6343 compasss Wire.beginTransmission(CMPS12_ADDRESS); //starts communication with CMPS12
Wire.write(HMC6343_HEADING_REG); // Send the address of the register that we want to read Wire.write(1); //Sends the register we wish to start reading from
//Wire.write(0x55); // Send the address of the register that we want to read
Wire.endTransmission(); Wire.endTransmission();
Wire.requestFrom(HMC6343_ADDRESS, 6); // Request six bytes of data from the HMC6343 compasss // Request 5 bytes from the CMPS12
for (int i = 0; i < 6; i++) { // this will give us the 8 bit bearing,
while (Wire.available() < 1) // both bytes of the 16 bit bearing, pitch and roll
; // Busy wait while there is no byte to receive Wire.requestFrom(CMPS12_ADDRESS, 5);
buf[i] = Wire.read();
//printf("buf[%d]=%d\r\n",i,buf[i]);
}
int heading = ((buf[0] << 8) + buf[1]); // the heading in degrees
int pitch = ((buf[2] << 8) + buf[3]); // the pitch in degrees
int roll = ((buf[4] << 8) + buf[5]); // the roll in degrees*/
heading = heading / 10; while(Wire.available() < 5); // Wait for all bytes to come back
roll = roll / 10;
pitch = pitch / 10;
//myDebug.print("Heading = "); angle8 = Wire.read(); // Read back the 5 bytes
//myDebug.print(heading); high_byte = Wire.read();
low_byte = Wire.read();
pitch = Wire.read();
roll = Wire.read();
state.roll = (int8_t)roll; // heading is angle16
state.pitch = (int8_t)pitch; heading = high_byte; // Calculate 16 bit angle
state.heading = (uint16_t)heading; heading <<= 8;
heading += low_byte;
//dprintf(DEBUG_IMPORTANT,"Heading: %d Roll: %d Pitch: %d\r\n",state.heading,state.roll,state.pitch); // may need to reimplement this later
//printf("heading=%d\r\n",heading); // heading=heading/10;
//dprintf2("heading=%d\r\n",heading); // roll=roll/10;
// pitch=pitch/10;
myDebug.print("Heading = ");
myDebug.print(heading);
// rewrite this to set state to our values
state.roll=(int8_t)roll;
state.pitch=(int8_t)pitch;
state.heading=(uint16_t)heading;
dprintf(DEBUG_IMPORTANT,"Heading: %d Roll: %d Pitch: %d\r\n",state.heading,state.roll,state.pitch);
printf("heading=%d\r\n",heading);
delay(100); delay(100);
return (int)heading; // Print the sensor readings to the serial port. return (int)heading; // Print the sensor readings to the serial port.
} }
void readGPS() { void readGPS() {
unsigned long fix_age = 9999, time, date; unsigned long fix_age=9999,time,date;
say(DEBUG_MINOR, "About to read GPS"); say(DEBUG_MINOR,"About to read GPS");
digitalWrite(GPS_ENABLE_PIN, 1); //turn the GPS on digitalWrite(GPS_ENABLE_PIN,1); //turn the GPS on
delay(1000); delay(1000);
while (fix_age == TinyGPS::GPS_INVALID_AGE || fix_age > 3000) //make sure the GPS has a fix, this might cause a wait the first time, but it should be quick any subsequent time while(fix_age == TinyGPS::GPS_INVALID_AGE||fix_age>3000) //make sure the GPS has a fix, this might cause a wait the first time, but it should be quick any subsequent time
{ {
Serial.println("$PSRF103,04,01,00,01*21\r"); Serial.println("$PSRF103,04,01,00,01*21\r");
dprintf(DEBUG_MINOR, "NMEA string: "); dprintf(DEBUG_MINOR,"NMEA string: ");
unsigned long start = millis(); unsigned long start = millis();
while (millis() < start + 2000) { while(millis()<start+2000)
if (Serial.available()) { {
if(Serial.available())
{
int c = Serial.read(); int c = Serial.read();
gps.encode(c); gps.encode(c);
if (DEBUG_THRESHOLD >= DEBUG_MINOR) { if(DEBUG_THRESHOLD>=DEBUG_MINOR)
{
myDebug.write(c); myDebug.write(c);
} }
if (c == '\n') { if(c=='\n')
{
break; break;
} }
} }
} }
gps.get_datetime(&date, &time, &fix_age); gps.get_datetime(&date,&time,&fix_age);
dprintf(DEBUG_MINOR, "fix age = %ld\r\n", fix_age); dprintf(DEBUG_MINOR,"fix age = %ld\r\n",fix_age);
if (fix_age == TinyGPS::GPS_INVALID_AGE) { if(fix_age == TinyGPS::GPS_INVALID_AGE)
dprintf(DEBUG_IMPORTANT, "Invalid fix, fix_age=%ld\r\n", fix_age); {
say(DEBUG_IMPORTANT, "No GPS fix"); dprintf(DEBUG_IMPORTANT,"Invalid fix, fix_age=%ld\r\n",fix_age);
say(DEBUG_IMPORTANT,"No GPS fix");
} }
} }
digitalWrite(GPS_ENABLE_PIN, 0); //turn the GPS off digitalWrite(GPS_ENABLE_PIN,0); //turn the GPS off
gps.get_datetime(&date, &time, &fix_age); gps.get_datetime(&date,&time,&fix_age);
gps.f_get_position(&state.lat, &state.lon, &fix_age); gps.f_get_position(&state.lat,&state.lon,&fix_age);
if (fix_age == TinyGPS::GPS_INVALID_AGE) { if(fix_age == TinyGPS::GPS_INVALID_AGE)
say(DEBUG_IMPORTANT, "Invalid fix"); {
say(DEBUG_IMPORTANT,"Invalid fix");
} }
else { else
{
say(DEBUG_IMPORTANT, "Fix Valid"); say(DEBUG_IMPORTANT,"Fix Valid");
dprintf(DEBUG_IMPORTANT, "lat=%ld lon=%ld\r\n", (long)(state.lat * 1000), (long)(state.lon * 1000)); dprintf(DEBUG_IMPORTANT,"lat=%ld lon=%ld\r\n",(long)(state.lat*1000),(long)(state.lon*1000));
int year; int year;
byte month, day, hour, min, sec; byte month,day,hour,min,sec;
unsigned long age; unsigned long age;
gps.crack_datetime(&year, &month, &day, &hour, &min, &sec, NULL, &age); gps.crack_datetime(&year,&month,&day,&hour,&min,&sec,NULL,&age);
setTime(hour,min,sec,day,month,year); //sets the time in the time library, lets us get unix time
setTime(hour, min, sec, day, month, year); //sets the time in the time library, lets us get unix time
} }
} }
int mod(int value) { //keeps angles betweeen 0 and 360 int mod(int value){ //keeps angles betweeen 0 and 360
int newValue; int newValue;
if (value < 0) { if(value < 0){
newValue = value + 360; newValue = value + 360;
} else if (value >= 360) { }
else if(value >= 360){
newValue = value - 360; newValue = value - 360;
} else { }
else{
newValue = value; newValue = value;
} }
return newValue; return newValue;
} }
//calculates difference between two headings taking wrap around into account //calculates difference between two headings taking wrap around into account
int get_hdg_diff(int heading1, int heading2) { int get_hdg_diff(int heading1,int heading2)
{
int result; int result;
result = heading1 - heading2; result = heading1-heading2;
if (result < -180) { if(result<-180)
{
result = 360 + result; result = 360 + result;
return result; return result;
} }
if (result > 180) { if(result>180)
result = 0 - (360 - result); {
result = 0 - (360-result);
} }
return result; return result;
} }
void loop() { void loop()
unsigned long last_gps_read = 0; {
unsigned long last_time = 0, time_now = 0; say(DEBUG_MINOR, "Entered loop");
int wp_hdg = 0; // heading to the next waypoint, in degrees unsigned long last_gps_read=0;
float wp_dist = 0.0; // distance to next waypoint unsigned long last_time=0,time_now=0;
int wp_num = 0; // number of waypoints int wp_hdg=0;
float wp_dist=0.0;
int wp_num=0;
float igain = 0.01; // i and p gain are multipliers for the rudder servo, they fix issues float igain=0.01;
float pgain = 0.1; float pgain=0.1;
float running_err = 0.0; float running_err=0.0;
int hdg_err = 0; int hdg_err=0;
int relwind; // wind direction relative to us int relwind;
long last_telemetry = 0; long last_telemetry=0;
#define TELEMETRY_INTERVAL 10 #define TELEMETRY_INTERVAL 10
#define TARGET_LOOP_INTERVAL 100 //number of milliseconds between loop intervals #define TARGET_LOOP_INTERVAL 100 //number of milliseconds between loop intervals
#define NUM_OF_WAYPOINTS 1 #define NUM_OF_WAYPOINTS 1
float wp_lats[NUM_OF_WAYPOINTS]; float wp_lats[NUM_OF_WAYPOINTS];
float wp_lons[NUM_OF_WAYPOINTS]; float wp_lons[NUM_OF_WAYPOINTS];
wp_lats[0] = 52.4; wp_lats[0]=52.4;
wp_lons[0] = -4.4; wp_lons[0]=-4.4;
while (1) { while(1)
{
//make loop execute at constant speed //make loop execute at constant speed
time_now = millis(); time_now=millis();
if (time_now - last_time > 0 && time_now - last_time < TARGET_LOOP_INTERVAL) { if(time_now-last_time>0&&time_now-last_time<TARGET_LOOP_INTERVAL)
delay(TARGET_LOOP_INTERVAL - (time_now - last_time)); {
delay(TARGET_LOOP_INTERVAL-(time_now-last_time));
} }
last_time = millis(); last_time=millis();
//get external values say(DEBUG_MINOR, "reading compass");
/*
* Grab compass value, work out the wind direction in relation to our heading, then grab out gps location and print some debug
* When we read the gps value we also check how close we are to our target waypoint, if we're close enough we move on to the next one.
* after doing this we do some stuff to make sure we arnt going to waypoints that dont exist then set our waypoint heading (wp_heading)
* and some other data that will be used to steer towards the waypoint
*/
readCompass(); readCompass();
//state.wind_dir=getTrueWind(); say(DEBUG_MINOR, "finished reading compass");
//state.wind_dir=getTrueWind(); {
//no wind sensor, so just use a fixed wind direction //no wind sensor, so just use a fixed wind direction
state.wind_dir = 270; state.wind_dir=270;
relwind = mod(state.wind_dir - state.heading); relwind=mod(state.wind_dir - state.heading);
if (millis() - last_gps_read > (GPS_READ_INTERVAL * 1000) || millis() < last_gps_read) //read the GPS at the specified interval or whenever the millis count wraps around say(DEBUG_MINOR, "may read gps");
if(millis()-last_gps_read>(GPS_READ_INTERVAL*1000)||millis()<last_gps_read) //read the GPS at the specified interval or whenever the millis count wraps around
{ {
say(DEBUG_MINOR, "Reading GPS"); say(DEBUG_MINOR,"Reading GPS");
readGPS(); readGPS();
wp_hdg = (int)TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]); wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]); wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
if (wp_dist < WP_THRESHOLD) { if(wp_dist<WP_THRESHOLD)
{
wp_num++; wp_num++;
if (wp_num == NUM_OF_WAYPOINTS) //reached last waypoint already if(wp_num==NUM_OF_WAYPOINTS) //reached last waypoint already
{ {
wp_num--; wp_num--;
} else //reached new waypoint }
else //reached new waypoint
{ {
wp_hdg = (int)TinyGPS::course_to(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]); wp_hdg = (int) TinyGPS::course_to(state.lat, state.lon,wp_lats[wp_num],wp_lons[wp_num]);
wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num], wp_lons[wp_num]); wp_dist = TinyGPS::distance_between(state.lat, state.lon, wp_lats[wp_num],wp_lons[wp_num]);
} }
} }
last_gps_read = millis(); last_gps_read=millis();
} }
say(DEBUG_MINOR, "Finished possible gps read");
//sail logic //sail logic
//sailLogic(relwind); //sailLogic(relwind);
//no sail as we have a fixed one
//rudder logic //rudder logic
/* hdg_err = get_hdg_diff(wp_hdg,state.heading);
* First we use get_hdg_diff to get work out how many degrees off the correct direction we are, this is hdg_err (heading error)
* next we increase the running error and clamp it, this variaby slowly increases over time so that we can reach the heading (withough this we just get closer and closer but never reach it)
* then we use the heading and the error to set what position the rudder should be in, clamp it then also store that value in state for reference later (state holds all the data the arduino needs, see its definition at like 60~)
*/
hdg_err = get_hdg_diff(wp_hdg, state.heading);
running_err = running_err + (float)hdg_err; running_err = running_err + (float)hdg_err;
if (abs(running_err > 4000)) { if (abs(running_err > 4000))
running_err = 4000; // limit integral component {
running_err = 4000; // limit integral component
} }
running_err = running_err * 0.9; running_err = running_err * 0.9;
/*dprintf("hdg_err = %d running_err = ",hdg_err); /*dprintf("hdg_err = %d running_err = ",hdg_err);
myDebug.println(running_err);*/ myDebug.println(running_err);*/
state.rudder = (int)round((pgain * (float)hdg_err) + (igain * running_err)); state.rudder = (int) round((pgain * (float)hdg_err) + (igain * running_err));
if (state.rudder < -5) { if(state.rudder<-5)
state.rudder = -5; {
} else if (state.rudder > 5) { state.rudder=-5;
state.rudder = 5; }
else if(state.rudder>5)
{
state.rudder=5;
} }
rudderServo.writeMicroseconds(1500 + (state.rudder * 100)); say(DEBUG_MINOR, "correcting rudder");
rudderServo.writeMicroseconds(1500+(state.rudder*100));
// print debug info say(DEBUG_MINOR, "possible telemetry incoming:");
/* if(last_telemetry+(TELEMETRY_INTERVAL*1000)<millis())
* We print debug info every certain amount of seconds (10 by default). {
* we mostly print info about where we are and where the waypoint is char msgbuf[255];
*/ char nmeadata[80];
char checksum;
if (last_telemetry + (TELEMETRY_INTERVAL * 1000) < millis()) { //generate key value telemetry packet
dprintf(DEBUG_CRITICAL, "time=%ld hdg=%d hdg_err=%d roll=%d pitch=%d truewind=%d relwind=%d sail=%d rudder=%d wp_num=%d wp_hdg=%d wp_dist=%ld ", now(), state.heading, hdg_err, state.roll, state.pitch, state.wind_dir, relwind, state.sail, state.rudder, wp_num, wp_hdg, (long)wp_dist); snprintf(msgbuf,254,"time=%ld lat=%.4f lon=%.4f hdg=%d hdg_err=%d roll=%d pitch=%d rudder=%d wp_num=%d wp_hdg=%d wp_dist=%ld ",now(),state.lat,state.lon,state.heading,hdg_err,state.roll,state.pitch,state.rudder,wp_num,wp_hdg,(long)wp_dist);
say(DEBUG_CRITICAL,msgbuf);
udp.broadcastTo(msgbuf,1234);
//time=181734082 hdg=-14836 hdg_err=9217 roll=-22526 pitch=24182 truewind=-25261 relwind=27648 sail=-6656 rudder=-7937 wp_hdg=2768 wp_dist=-284423467 lat=52.41648 lon=-4.06522 wplat=52.40000 wplon=-4.40000 running_err=966.57 //generate compass NMEA string
snprintf(nmeadata,79,"HDM,%d.0,M",state.heading);
checksum=compute_checksum(nmeadata,strlen(nmeadata));
snprintf(msgbuf,254,"$%s*%X\n",nmeadata,checksum);
say(DEBUG_CRITICAL, msgbuf);
udp.broadcastTo(msgbuf,10000);
// time=1398700112 hdg=158 hdg_err=107 roll=-26 pitch=-32 truewind=302 relwind=144 sail=4 rudder=5 Debug1: [Ctrl] wp_hdg=265 wp_dist=22842 lat=52.41666 lon=-4.06445 wplat=52.40000 wplon=-4.40000 running_err=963.96 //generate GPS NMEA string
char timestr[7];
char hemNS;
char hemEW;
char latstr[10]; //DDMM.MMMM0
char lonstr[11]; //DDDMM.MMMM0
int degrees;
float minutes;
//generate time
snprintf(timestr,6,"%2d%2d%2d",hour(),minute(),second());
if (DEBUG_THRESHOLD >= DEBUG_CRITICAL) { //generate lat/lon
degrees = abs((int) state.lat);
minutes = (fabs(state.lat) - (float)degrees) * 60.0;
snprintf(latstr,9,"%2d%2.4f",degrees,minutes);
//get the hemisphere north/south
if (state.lat>0) {
hemNS = 'N';
}
else {
hemNS = 'S';
}
degrees = abs((int) state.lon);
minutes = (fabs(state.lon) - (float)degrees) * 60.0;
snprintf(lonstr,10,"%3d%2.4f",degrees,minutes);
//get the hemisphere east/east
if (state.lon>0) {
hemEW = 'E';
}
else {
hemEW = 'W';
}
//$GPGLL,LATMM.MMMM,N/S,LONMM.MMMM,E/W,HHMMSS,A,*SUM
snprintf(nmeadata,79,"GPGLL,%s,%c,%s,%c,%s,A%",latstr,hemNS,lonstr,hemEW,timestr);
checksum=compute_checksum(nmeadata,strlen(nmeadata));
snprintf(msgbuf,254,"$%s*%X\n",nmeadata,checksum);
say(DEBUG_CRITICAL, msgbuf);
//send to UDP port 10000
udp.broadcastTo(msgbuf,10000);
if(DEBUG_THRESHOLD>=DEBUG_CRITICAL)
{
myDebug.print("lat="); myDebug.print("lat=");
myDebug.print(state.lat, 5); myDebug.print(state.lat,5);
myDebug.print(" lon="); myDebug.print(" lon=");
myDebug.print(state.lon, 5); myDebug.print(state.lon,5);
myDebug.print(" wplat="); myDebug.print(" wplat=");
myDebug.print(wp_lats[wp_num], 5); myDebug.print(wp_lats[wp_num],5);
myDebug.print(" wplon="); myDebug.print(" wplon=");
myDebug.print(wp_lons[wp_num], 5); myDebug.print(wp_lons[wp_num],5);
myDebug.print(" running_err="); myDebug.print(" running_err=");
myDebug.println(running_err); myDebug.println(running_err);
} }
//transmit_data(); last_telemetry=millis();
last_telemetry = millis();
} }
} }
} }