From c80143b96ea57488d53175212d92e768e58ccfdd Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 1 Oct 2011 21:51:14 +0900 Subject: [PATCH 1/6] AP_Optflow - small fix to lat/lon calculation and made example sketch work again --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 6 +++--- .../AP_OpticalFlow_test/AP_OpticalFlow_test.pde | 17 +++++++++++++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index da47395701..25dbec3fec 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -97,9 +97,9 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less - - vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; - vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; + // convert x/y movements into lon/lat movement + vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x; + vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; } _last_altitude = altitude; diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index 057e9d5353..ef8c604a2c 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -3,10 +3,23 @@ Code by Randy Mackay. DIYDrones.com */ +#include +#include #include // ArduPilot Mega Vector/Matrix math Library -#include // Arduino SPI library +#include // Arduino SPI library +#include // ArduCopter DCM Library #include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +FastSerialPort0(Serial); // FTDI/console + AP_OpticalFlow_ADNS3080 flowSensor; void setup() @@ -297,7 +310,7 @@ void display_motion() while( !Serial.available() ) { flowSensor.read(); - flowSensor.get_position(0,0,0,100); + flowSensor.update_position(0,0,0,1,100); // x,y,squal //if( flowSensor.motion() || first_time ) { From 6cd90471d18724d532c2b23869990d532d2338e8 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sat, 1 Oct 2011 16:18:39 +0200 Subject: [PATCH 2/6] ArduPPM : modified library path --- Tools/ArduPPM/ATMega328p/Encoder-PPM.c | 2 +- Tools/ArduPPM/Libraries/readme.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c index e46890def3..fa4aefc454 100644 --- a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c +++ b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c @@ -37,7 +37,7 @@ // PREPROCESSOR DIRECTIVES // ------------------------------------------------------------------------------------------------------------------------------------------------------------ -#include "ppm_encoder.h" +#include "..\Libraries\ppm_encoder.h" #include diff --git a/Tools/ArduPPM/Libraries/readme.txt b/Tools/ArduPPM/Libraries/readme.txt index f52de71854..0bf068e5a4 100644 --- a/Tools/ArduPPM/Libraries/readme.txt +++ b/Tools/ArduPPM/Libraries/readme.txt @@ -1,2 +1,2 @@ -libraries used by all code bases \ No newline at end of file +libraries used by all ArduPPM code bases \ No newline at end of file From 1ca4936fc729f9c41697bf4420c0d4a073d017ba Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sat, 1 Oct 2011 16:36:35 +0200 Subject: [PATCH 3/6] Added Jeti and Spektrum base code to ArduPPM/WorkBasket folder --- .../Jeti Duplex/Jetibox/JetiBox.cpp | 166 +++ .../WorkBasket/Jeti Duplex/Jetibox/JetiBox.h | 51 + .../X-DIY_Jeti_V2/X-DIY/APM_Config.h | 925 ++++++++++++ .../X-DIY/APM_Config.h.reference | 914 ++++++++++++ .../X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h | 19 + .../X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde | 845 +++++++++++ .../X-DIY_Jeti_V2/X-DIY/Attitude.pde | 343 +++++ .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde | 387 +++++ .../X-DIY_Jeti_V2/X-DIY/EEPROM map.txt | 389 +++++ .../X-DIY_Jeti_V2/X-DIY/EEPROM.pde | 286 ++++ .../X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde | 176 +++ .../X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde | 1258 ++++++++++++++++ .../X-DIY_Jeti_V2/X-DIY/GCS_IMU_ouput.pde | 192 +++ .../X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde | 238 +++ .../X-DIY_Jeti_V2/X-DIY/GCS_Standard.pde | 371 +++++ .../X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde | 9 + .../X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde | 127 ++ .../X-DIY_Jeti_V2/X-DIY/HIL_output.pde | 78 + .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde | 264 ++++ .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde | 596 ++++++++ .../X-DIY/command description.txt | 75 + .../X-DIY_Jeti_V2/X-DIY/commands.pde | 228 +++ .../X-DIY_Jeti_V2/X-DIY/commands_process.pde | 480 ++++++ .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h | 569 +++++++ .../X-DIY_Jeti_V2/X-DIY/control_modes.pde | 58 + .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde | 70 + .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h | 345 +++++ .../X-DIY_Jeti_V2/X-DIY/events.pde | 226 +++ .../X-DIY_Jeti_V2/X-DIY/navigation.pde | 230 +++ .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde | 180 +++ .../X-DIY_Jeti_V2/X-DIY/sensors.pde | 65 + .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde | 410 ++++++ .../X-DIY_Jeti_V2/X-DIY/system.pde | 491 +++++++ .../Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde | 433 ++++++ .../libraries/APM_ADC/APM_ADC.cpp | 149 ++ .../X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h | 24 + .../examples/APM_ADC_test/APM_ADC_test.pde | 33 + .../libraries/APM_ADC/keywords.txt | 3 + .../libraries/APM_BMP085/APM_BMP085.cpp | 243 +++ .../libraries/APM_BMP085/APM_BMP085.h | 34 + .../APM_BMP085_test/APM_BMP085_test.pde | 41 + .../libraries/APM_BMP085/keywords.txt | 5 + .../libraries/APM_BinComm/APM_BinComm.cpp | 206 +++ .../libraries/APM_BinComm/APM_BinComm.h | 267 ++++ .../libraries/APM_BinComm/keywords.txt | 6 + .../APM_BinComm/protocol/protocol.def | 224 +++ .../libraries/APM_BinComm/protocol/protocol.h | 1301 +++++++++++++++++ .../APM_BinComm/protocol/protogen.awk | 174 +++ .../libraries/APM_BinComm/test/Makefile | 14 + .../libraries/APM_BinComm/test/WProgram.h | 15 + .../libraries/APM_BinComm/test/test.cpp | 110 ++ .../libraries/APM_Compass/APM_Compass.cpp | 235 +++ .../libraries/APM_Compass/APM_Compass.h | 69 + .../APM_Compass_test/APM_Compass_test.pde | 87 ++ .../libraries/APM_Compass/keywords.txt | 19 + .../APM_FastSerial/APM_FastSerial.cpp | 150 ++ .../libraries/APM_FastSerial/APM_FastSerial.h | 25 + .../APM_FastSerial/APM_FastSerial.pde | 91 ++ .../libraries/APM_FastSerial/keywords.txt | 2 + .../X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp | 193 +++ .../X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h | 24 + .../APM_RC/examples/APM_radio/APM_radio.pde | 31 + .../libraries/APM_RC/keywords.txt | 8 + .../libraries/APM_RC_QUAD/APM_RC_QUAD.cpp | 169 +++ .../libraries/APM_RC_QUAD/APM_RC_QUAD.h | 21 + .../APM_radio_quad/APM_radio_quad.pde | 31 + .../libraries/APM_RC_QUAD/keywords.txt | 5 + .../libraries/AP_Common/AP_Common.cpp | 17 + .../libraries/AP_Common/AP_Common.h | 53 + .../X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp | 21 + .../AP_Common/examples/menu/menu.pde | 45 + .../libraries/AP_Common/include/menu.h | 138 ++ .../libraries/AP_Common/keywords.txt | 4 + .../libraries/AP_Common/menu.cpp | 129 ++ .../libraries/AP_Compass/AP_Compass.cpp | 115 ++ .../libraries/AP_Compass/AP_Compass.h | 17 + .../libraries/AP_Compass/Compass.h | 23 + .../AP_Compass_test/AP_Compass_test.pde | 40 + .../libraries/AP_Compass/keywords.txt | 15 + .../X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h | 13 + .../libraries/AP_GPS/AP_GPS_406.cpp | 81 + .../libraries/AP_GPS/AP_GPS_406.h | 32 + .../libraries/AP_GPS/AP_GPS_Auto.cpp | 162 ++ .../libraries/AP_GPS/AP_GPS_Auto.h | 52 + .../libraries/AP_GPS/AP_GPS_IMU.cpp | 224 +++ .../libraries/AP_GPS/AP_GPS_IMU.h | 44 + .../libraries/AP_GPS/AP_GPS_MTK.cpp | 146 ++ .../libraries/AP_GPS/AP_GPS_MTK.h | 87 ++ .../libraries/AP_GPS/AP_GPS_NMEA.cpp | 246 ++++ .../libraries/AP_GPS/AP_GPS_NMEA.h | 60 + .../libraries/AP_GPS/AP_GPS_None.h | 13 + .../libraries/AP_GPS/AP_GPS_SIRF.cpp | 193 +++ .../libraries/AP_GPS/AP_GPS_SIRF.h | 96 ++ .../libraries/AP_GPS/AP_GPS_UBLOX.cpp | 192 +++ .../libraries/AP_GPS/AP_GPS_UBLOX.h | 124 ++ .../X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp | 35 + .../X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h | 150 ++ .../examples/GPS_406_test/GPS_406_test.pde | 58 + .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 59 + .../examples/GPS_MTK_test/GPS_MTK_test.pde | 56 + .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 57 + .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 56 + .../X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h | 7 + .../libraries/AP_Math/keywords.txt | 29 + .../X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h | 134 ++ .../X-DIY_Jeti_V2/libraries/AP_Math/vector2.h | 158 ++ .../X-DIY_Jeti_V2/libraries/AP_Math/vector3.h | 183 +++ .../libraries/AP_Navigation/Navigation.cpp | 249 ++++ .../libraries/AP_Navigation/Navigation.h | 76 + .../examples/Navigation/Navigation.pde | 152 ++ .../Navigation_simple/Navigation_simple.pde | 28 + .../X-DIY_Jeti_V2/libraries/DCM/DCM.cpp | 472 ++++++ .../X-DIY_Jeti_V2/libraries/DCM/DCM.h | 147 ++ .../DCM/examples/DCM_test/DCM_test.pde | 38 + .../libraries/DataFlash/DataFlash.cpp | 342 +++++ .../libraries/DataFlash/DataFlash.h | 86 ++ .../DataFlash_test/DataFlash_test.pde | 99 ++ .../libraries/DataFlash/keywords.txt | 14 + .../libraries/FastSerial/FastSerial.cpp | 348 +++++ .../libraries/FastSerial/FastSerial.h | 261 ++++ .../examples/FastSerial/FastSerial.pde | 55 + .../libraries/FastSerial/keywords.txt | 7 + .../X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store | Bin 0 -> 6148 bytes .../libraries/GPS_IMU/GPS_IMU.cpp | 279 ++++ .../X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h | 69 + .../libraries/GPS_IMU/examples/.DS_Store | Bin 0 -> 6148 bytes .../examples/GPS_IMU_test/GPS_IMU_test.pde | 44 + .../libraries/GPS_IMU/keywords.txt | 16 + .../libraries/GPS_MTK/GPS_MTK.cpp | 225 +++ .../X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h | 49 + .../examples/GPS_MTK_test/GPS_MTK_test.pde | 47 + .../libraries/GPS_MTK/keywords.txt | 16 + .../libraries/GPS_NMEA/GPS_NMEA.cpp | 272 ++++ .../libraries/GPS_NMEA/GPS_NMEA.h | 46 + .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 42 + .../libraries/GPS_NMEA/keywords.txt | 18 + .../libraries/GPS_UBLOX/GPS_UBLOX.cpp | 274 ++++ .../libraries/GPS_UBLOX/GPS_UBLOX.h | 50 + .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 42 + .../libraries/GPS_UBLOX/keywords.txt | 16 + .../libraries/JETI_Box/JETI_Box.cpp | 202 +++ .../libraries/JETI_Box/JETI_Box.cpp.bak | 188 +++ .../libraries/JETI_Box/JETI_Box.h | 36 + .../libraries/JETI_Box/keywords.txt | 8 + .../X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp | 327 +++++ .../X-DIY_Jeti_V2/libraries/RC/APM2_RC.h | 38 + .../X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp | 329 +++++ .../X-DIY_Jeti_V2/libraries/RC/AP_RC.h | 32 + .../X-DIY_Jeti_V2/libraries/RC/RC.cpp | 84 ++ .../X-DIY_Jeti_V2/libraries/RC/RC.h | 46 + .../APM_RC_elevons/APM_RC_elevons.pde | 59 + .../RC/examples/APM_RC_test/APM_RC_test.pde | 59 + .../examples/AP_RC_elevons/AP_RC_elevons.pde | 48 + .../RC/examples/AP_RC_test/AP_RC_test.pde | 50 + .../X-DIY_Jeti_V2/libraries/RC/keywords.txt | 4 + .../libraries/Waypoints/Waypoints.cpp | 120 ++ .../libraries/Waypoints/Waypoints.h | 46 + .../ArduPPM/WorkBasket/Jeti Duplex/readme.txt | 29 + Tools/ArduPPM/WorkBasket/readme.txt | 9 + .../spektrum_to_ppm_encoder.pde | 309 ++++ 160 files changed, 24769 insertions(+) create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_IMU_ouput.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Standard.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/HIL_output.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/events.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/system.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp.bak create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h create mode 100644 Tools/ArduPPM/WorkBasket/Jeti Duplex/readme.txt create mode 100644 Tools/ArduPPM/WorkBasket/readme.txt create mode 100644 Tools/ArduPPM/WorkBasket/spektrum_to_ppm_encoder/spektrum_to_ppm_encoder.pde diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.cpp new file mode 100644 index 0000000000..6669416048 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.cpp @@ -0,0 +1,166 @@ +/* + JetiBox.cpp, Version 1.0 beta + July 2010, by Uwe Gartmann + + Library acts as a Sensor when connected to a Jeti Receiver + written for Arduino Mega / ArduPilot Mega + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include +#include +#include "wiring.h" +#include "mySerial0.h" +#include "JetiBox.h" + +#define LCDLine1 1 +#define LCDLine2 17 +#define LCDMaxChar 32 + +//define Jeti-Box Display +struct jeti_box { + unsigned char lcd[32]; + volatile unsigned char lcdpos; + volatile unsigned char keys; + volatile unsigned char sendpos; +}; + +// create jbox +jeti_box jb = {{0},0,0,0}; +unsigned char dummy; +unsigned char lastkey = 0; + +ISR(USART3_RX_vect) //serial data available +{ + // save response data to keys + dummy = UDR3; + if (dummy != 0xFF) + { + jb.keys = dummy; + // disable this interrupt + UCSR3B &= ~(1< interrupt is enabled (function start() ), send startbyte with 9.bit=0 + * jbox.sendpos = 1-32 -> send display data with 9.bit=1 + * jbox.sendpos = 33 -> send endbyte with 9.bit=0 + * jbox.sendpos = 34 -> reset sendpos=0 -> disable this interrupt + */ +{ + switch (jb.sendpos) + { + case 0: + // send start byte with 9.bit=0 + UCSR3B &= ~(1< 0 + jb.sendpos = 0; + // enable receiver interrupt for reading key byte + UCSR3B |= (1<>8); //high byte + UBRR3L=_UBRR3; //low byte + + // Set frame format: 9data, odd parity, 2stop bit + UCSR3C = (0<>4) xor 0x0F; + if (lastkey==c) + { + return 0; + }else{ + return lastkey = c; + } + +} + +void JetiBox::write(uint8_t c) +{ + jb.lcd[jb.lcdpos] = c; + if (jb.lcdpos < LCDMaxChar) + { + jb.lcdpos++; + } +} + +void JetiBox::line1() +{ + jb.lcdpos = LCDLine1; +} + +void JetiBox::line2() +{ + jb.lcdpos = LCDLine2; +} + +// Preinstantiate Objects ////////////////////////////////////////////////////// +JetiBox JBox; + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.h new file mode 100644 index 0000000000..ee39e5a2e8 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.h @@ -0,0 +1,51 @@ +/* + JetiBox.h, Version 1.0 beta + July 2010, by Uwe Gartmann + + Library acts as a Sensor when connected to a Jeti Receiver + written for Arduino Mega / ArduPilot Mega + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef JetiBox_h +#define JetiBox_h + +#include +#include "Print.h" + +#define jbox_key_up 0b0010 +#define jbox_key_right 0b0001 +#define jbox_key_down 0b0100 +#define jbox_key_left 0b1000 + +struct jeti_box; + +class JetiBox : public Print +{ + public: + JetiBox(); + void begin(); + void refresh(); + uint8_t keys(void); + virtual void write(uint8_t); + using Print::write; // pull in write(str) and write(buf, size) from Print + void line1(); + void line2(); +}; + +extern JetiBox JBox; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h new file mode 100644 index 0000000000..e75fb175cf --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h @@ -0,0 +1,925 @@ +// +// Example and reference ArduPilot Mega configuration file +// ======================================================= +// +// This file contains documentation and examples for configuration options +// supported by the ArduPilot Mega software. +// +// Most of these options are just that - optional. You should create +// the APM_Config.h file and use this file as a reference for options +// that you want to change. Don't copy this file directly; the options +// described here and their default values may change over time. +// +// Each item is marked with a keyword describing when you should set it: +// +// REQUIRED +// You must configure this in your APM_Config.h file. The +// software will not compile if the option is not set. +// +// OPTIONAL +// The option has a sensible default (which will be described +// here), but you may wish to override it. +// +// EXPERIMENTAL +// You should configure this option unless you are prepared +// to deal with potential problems. It may relate to a feature +// still in development, or which is not yet adequately tested. +// +// DEBUG +// The option should only be set if you are debugging the +// software, or if you are gathering information for a bug +// report. +// +// NOTE: +// Many of these settings are considered 'factory defaults', and the +// live value is stored and managed in the ArduPilot Mega EEPROM. +// Use the setup 'factoryreset' command after changing options in +// your APM_Config.h file. +// +// Units +// ----- +// +// Unless indicated otherwise, numeric quantities use the following units: +// +// Measurement | Unit +// ------------+------------------------------------- +// angle | degrees +// distance | metres +// speed | metres per second +// servo angle | microseconds +// voltage | volts +// times | seconds +// throttle | percent +// + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// HARDWARE CONFIGURATION AND CONNECTIONS +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// GPS_PROTOCOL REQUIRED +// +// GPS configuration, must be one of: +// +// GPS_PROTOCOL_NONE No GPS attached +// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. +// GPS_PROTOCOL_MTK MediaTek-based GPS. +// GPS_PROTOCOL_UBLOX UBLOX GPS +// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED +// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?) +// +#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX +// + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_SENSOR OPTIONAL +// AIRSPEED_RATIO OPTIONAL +// +// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. +// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed +// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s +// +// The default assumes that an airspeed sensor is connected. +// +#define AIRSPEED_SENSOR ENABLED +#define AIRSPEED_RATIO 1.9936 +// +#define MAGNETOMETER 1 +#define USE_MAGNETOMETER ENABLED + +////////////////////////////////////////////////////////////////////////////// +// GCS_PROTOCOL OPTIONAL +// GCS_PORT OPTIONAL +// +// The GCS_PROTOCOL option determines which (if any) ground control station +// protocol will be used. Must be one of: +// +// GCS_PROTOCOL_NONE No GCS output +// GCS_PROTOCOL_STANDARD standard APM protocol +// GCS_PROTOCOL_SPECIAL special test protocol (?) +// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol +// GCS_PROTOCOL_XPLANE HIL simulation ground station +// GCS_PROTOCOL_IMU ArdiPilot IMU output +// GCS_PROTOCOL_JASON Jason's special secret GCS protocol +// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental) +// +// The GCS_PORT option determines which serial port will be used by the +// GCS protocol. The usual values are 0 for the console/USB port, +// or 3 for the telemetry port on the oilpan. Note that some protocols +// will ignore this value and always use the console port. +// +// The default GCS protocol is the standard ArduPilot Mega protocol. +// +// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD +// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial +// port is the FTDI/console port. GCS_PORT normally should not be set +// in your configuration. +// +#define GCS_PROTOCOL GCS_PROTOCOL_XDIY +#define GCS_PORT 0 +// + +////////////////////////////////////////////////////////////////////////////// +// Serial port speeds. +// +// SERIAL0_BAUD OPTIONAL +// +// Baudrate for the console port. Default is 38400bps. +// +// SERIAL3_BAUD OPTIONAL +// +// Baudrate for the telemetry port. Default is 115200bps. +// +//#define SERIAL0_BAUD 38400 +//#define SERIAL3_BAUD 115200 +// + +////////////////////////////////////////////////////////////////////////////// +// Battery monitoring OPTIONAL +// +// See the manual for details on selecting divider resistors for battery +// monitoring via the oilpan. +// +// BATTERY_EVENT OPTIONAL +// +// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is +// DISABLED. +// +// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set +// +// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s +// battery. +// +// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. +// +// Normally derived from BATTERY_TYPE, the automatic value can be overridden +// here. Value in volts at which ArduPilot Mega should consider the +// battery to be "low". +// +// VOLT_DIV_RATIO OPTIONAL +// +// See the manual for details. The default value corresponds to the resistors +// recommended by the manual. +// +//#define BATTERY_EVENT DISABLED +//#define BATTERY_TYPE 0 +//#define LOW_VOLTAGE 11.4 +//#define VOLT_DIV_RATIO 3.0 +// + +////////////////////////////////////////////////////////////////////////////// +// INPUT_VOLTAGE OPTIONAL +// +// In order to have accurate pressure and battery voltage readings, this +// value should be set to the voltage measured on the 5V rail on the oilpan. +// +// See the manual for more details. The default value should be close. +// +#define INPUT_VOLTAGE 5.0 +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// RADIO CONFIGURATION +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// FLIGHT_MODE OPTIONAL +// FLIGHT_MODE_CHANNEL OPTIONAL +// +// Flight modes assigned to the control channel, and the input channel that +// is read for the control mode. +// +// Use a servo tester, or the ArduPilotMega_demo test program to check your +// switch settings. +// +// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and +// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option +// uses channel numbers 1-8 (and defaults to 8). +// +// If you only have a three-position switch or just want three modes, set your +// switch to produce 1165, 1425, and 1815 microseconds and configure +// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. +// +// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control +// channel connected to input channel 8, the hardware failsafe mode will +// activate for any control input over 1750ms. +// +// For more modes (up to six), set your switch(es) to produce any of 1165, +// 1295, 1425, 1555, 1685, and 1815 microseconds. +// +// Flight mode | Switch Setting (ms) +// ------------+--------------------- +// 1 | 1165 +// 2 | 1295 +// 3 | 1425 +// 4 | 1555 +// 5 | 1685 +// 6 | 1815 (FAILSAFE if using channel 8) +// +// The following standard flight modes are available: +// +// Name | Description +// -----------------+-------------------------------------------- +// | +// MANUAL | Full manual control via the hardware multiplexer. +// | +// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. +// | +// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. +// | +// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. +// | +// RTL | Returns to the Home location and then LOITERs at a safe altitude. +// | +// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter +// | application or your Ground Control System to edit and upload +// | waypoints and other commands. +// | +// +// +// The following non-standard modes are EXPERIMENTAL: +// +// Name | Description +// -----------------+-------------------------------------------- +// | +// LOITER | Flies in a circle around the current location. +// | +// CIRCLE | Flies in a stabilized 'dumb' circle. +// | +// +// +// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and +// FLIGHT_MODE_6 should be MANUAL. +// +// +#define FLIGHT_MODE_CHANNEL 8 +// +//#define FLIGHT_MODE_1 RTL +//#define FLIGHT_MODE_2 RTL +//#define FLIGHT_MODE_3 FLY_BY_WIRE_A +//#define FLIGHT_MODE_4 FLY_BY_WIRE_A +//#define FLIGHT_MODE_5 MANUAL +//#define FLIGHT_MODE_6 MANUAL + +// Define the pulse width when to switch to next higher FLIGHT_MODE +#define FLIGHT_MODE_1_BOUNDARY 1125 +#define FLIGHT_MODE_2_BOUNDARY 1335 +#define FLIGHT_MODE_3_BOUNDARY 1550 +#define FLIGHT_MODE_4_BOUNDARY 1690 +#define FLIGHT_MODE_5_BOUNDARY 1880 + + + + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_FAILSAFE OPTIONAL +// THROTTLE_FS_VALUE OPTIONAL +// THROTTLE_FAILSAFE_ACTION OPTIONAL +// +// The throttle failsafe allows you to configure a software failsafe activated +// by a setting on the throttle input channel (channel 3). +// +// This can be used to achieve a failsafe override on loss of radio control +// without having to sacrifice one of your FLIGHT_MODE settings, as the +// throttle failsafe overrides the switch-selected mode. +// +// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default +// is for it to be disabled. +// +// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value +// below which the failsafe engages. The default is 975ms, which is a very low +// throttle setting. Most transmitters will let you trim the manual throttle +// position up so that you cannot engage the failsafe with a regular stick movement. +// +// Configure your receiver's failsafe setting for the throttle channel to the +// absolute minimum, and use the ArduPilotMega_demo program to check that +// you cannot reach that value with the throttle control. Leave a margin of +// at least 50 microseconds between the lowest throttle setting and +// THROTTLE_FS_VALUE. +// +// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe +// mode is entered while flying in AUTO mode. This is important in order to avoid +// accidental failsafe behaviour when flying waypoints that take the aircraft +// temporarily out of radio range. +// +// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, +// the aircraft will head for home in RTL mode. If the throttle channel moves +// back up, it will return to AUTO or LOITER mode. +// +// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes. +// +//#define THROTTLE_FAILSAFE DISABLED +//#define THROTTLE_FS_VALUE 975 +//#define THROTTLE_FAILSAFE_ACTION 2 +// + +////////////////////////////////////////////////////////////////////////////// +// AUTO_TRIM OPTIONAL +// +// ArduPilot Mega can update its trim settings by looking at the +// radio inputs when switching out of MANUAL mode. This allows you to +// manually trim your aircraft before switching to an assisted mode, but it +// also means that you should avoid switching out of MANUAL while you have +// any control stick deflection. +// +// The default is to enable AUTO_TRIM. +// +#define AUTO_TRIM ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_REVERSE OPTIONAL +// +// A few speed controls require the throttle servo signal be reversed. Setting +// this to ENABLED will reverse the throttle output signal. Ensure that your +// throttle needs to be reversed by using the hardware failsafe and the +// ArduPilotMega_demo program before setting this option. +// +// The default is to not reverse the signal. +// +//#define THROTTLE_REVERSE DISABLED +// + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_STICK_MIXING OPTIONAL +// +// If this option is set to ENABLED, manual control inputs are are respected +// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) +// +// The default is to enable stick mixing, allowing the pilot to take +// emergency action without switching modes. +// +#define ENABLE_STICK_MIXING ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_OUT DEBUG +// +// When debugging, it can be useful to disable the throttle output. Set +// this option to DISABLED to disable throttle output signals. +// +// The default is to not disable throttle output. +// +#define THROTTLE_OUT DISABLED +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// STARTUP BEHAVIOUR +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// GROUND_START_DELAY OPTIONAL +// +// If configured, inserts a delay between power-up and the beginning of IMU +// calibration during a ground start. +// +// Use this setting to give you time to position the aircraft horizontally +// for the IMU calibration. +// +// The default is to begin IMU calibration immediately at startup. +// +//#define GROUND_START_DELAY 0 +// + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_AIR_START OPTIONAL +// +// If air start is disabled then you will get a ground start (including IMU +// calibration) every time the AP is powered up. This means that if you get +// a power glitch or reboot for some reason in the air, you will probably +// crash, but it prevents a lot of problems on the ground like unintentional +// motor start-ups, etc. +// +// If air start is enabled then you will get an air start at power up and a +// ground start will be performed if the speed is near zero when we get gps +// lock. +// +// The default is to disable air start. +// +#define ENABLE_AIR_START 0 +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// FLIGHT AND NAVIGATION CONTROL +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// Altitude measurement and control. +// +// AOA OPTIONAL +// +// The angle in 100ths of a degree that the nose of the aircraft should be +// raised from horizontal in level flight. The default is 1 degree. +// +//#define AOA 100 // note, 100ths of a degree +// +// ALT_EST_GAIN OPTIONAL +// +// The gain of the altitude estimation function; a lower number results +// in slower error correction and smoother output. The default is a +// reasonable starting point. +// +//#define ALT_EST_GAIN 0.01 +// +// ALTITUDE_MIX OPTIONAL +// +// Configures the blend between GPS and pressure altitude. +// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. +// +// The default is to use only GPS altitude. +// +//#define ALTITUDE_MIX 0 +// + + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_HIL OPTIONAL +// +// This will output a binary control string to for use in HIL sims +// such as Xplane 9 or FlightGear. +// +//#define ENABLE_HIL ENABLED +// + + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_CRUISE OPTIONAL +// +// The speed in metres per second to maintain during cruise. The default +// is 10m/s, which is a conservative value suitable for relatively small, +// light aircraft. +// +#define AIRSPEED_CRUISE 15 +// + +////////////////////////////////////////////////////////////////////////////// +// FLY_BY_WIRE_B airspeed control +// +// AIRSPEED_FBW_MIN OPTIONAL +// AIRSPEED_FBW_MAX OPTIONAL +// +// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode. +// The defaults are 6 and 30 metres per second. +// +// THROTTLE_ALT_P OPTIONAL +// THROTTLE_ALT_I OPTIONAL +// THROTTLE_ALT_D OPTIONAL +// +// P, I and D terms for the throttle control loop. Defaults are 0.5, 0, 0. +// +// THROTTLE_ALT_INT_MAX OPTIONAL +// +// Maximum throttle input due to the integral. Limits the throttle input +// due to persistent inability to maintain the commanded speed. Helps +// prevent the throttle from staying wide open when the control is reduced +// after a period at maxium speed. +// Default is 20 (20%). +// +//#define AIRSPEED_FBW_MIN 6 +//#define AIRSPEED_FBW_MAX 30 +//#define THROTTLE_ALT_P 0.32 +//#define THROTTLE_ALT_I 0.04 +//#define THROTTLE_ALT_D 0.0 +//#define THROTTLE_ALT_INT_MAX 20 +// + +////////////////////////////////////////////////////////////////////////////// +// Throttle control +// +// THROTTLE_MIN OPTIONAL +// +// The minimum throttle setting to which the autopilot will reduce the +// throttle while descending. The default is zero, which is +// suitable for aircraft with a steady power-off glide. Increase this +// value if your aircraft needs throttle to maintain a stable descent in +// level flight. +// +// THROTTLE_CRUISE OPTIONAL +// +// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. +// The default is 45%, which is reasonable for a modestly powered aircraft. +// +// THROTTLE_MAX OPTIONAL +// +// The maximum throttle setting the autopilot will apply. The default is 75%. +// Reduce this value if your aicraft is overpowered, or has complex flight +// characteristics at high throttle settings. +// +//#define THROTTLE_MIN 0 +//#define THROTTLE_CRUISE 45 +//#define THROTTLE_MAX 75 +// + +////////////////////////////////////////////////////////////////////////////// +// Autopilot control limits +// +// HEAD_MAX OPTIONAL +// +// The maximum commanded bank angle in either direction. +// The default is 45 degrees. Decrease this value if your aircraft is not +// stable or has difficulty maintaining altitude in a steep bank. +// +// PITCH_MAX OPTIONAL +// +// The maximum commanded pitch up angle. +// The default is 15 degrees. Care should be taken not to set this value too +// large, as the aircraft may stall. +// +// PITCH_MIN +// +// The maximum commanded pitch down angle. Note that this value must be +// negative. The default is -25 degrees. Care should be taken not to set +// this value too large as it may result in overspeeding the aircraft. +// +//#define HEAD_MAX 45 +//#define PITCH_MAX 15 +//#define PITCH_MIN -25 + +////////////////////////////////////////////////////////////////////////////// +// Attitude control gains +// +// Tuning values for the attitude control PID loops. +// +// The P term is the primary tuning value. This determines how the control +// deflection varies in proportion to the required correction. +// +// The I term is used to help control surfaces settle. This value should +// normally be kept low. +// +// The D term is used to control overshoot. Avoid using or adjusting this +// term if you are not familiar with tuning PID loops. It should normally +// be zero for most aircraft. +// +// Note: When tuning these values, start with changes of no more than 25% at +// a time. +// +// SERVO_ROLL_P OPTIONAL +// SERVO_ROLL_I OPTIONAL +// SERVO_ROLL_D OPTIONAL +// +// P, I and D terms for roll control. Defaults are 0.4, 0, 0. +// +// SERVO_ROLL_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// ROLL_SLEW_LIMIT EXPERIMENTAL +// +// Limits the slew rate of the roll control in degrees per second. If zero, +// slew rate is not limited. Default is to not limit the roll control slew rate. +// (This feature is currently not implemented.) +// +// SERVO_PITCH_P OPTIONAL +// SERVO_PITCH_I OPTIONAL +// SERVO_PITCH_D OPTIONAL +// +// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. +// +// SERVO_PITCH_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. native flight +// AoA). If you find this value is insufficient, consider adjusting the AOA +// parameter. +// Default is 5 degrees. +// +// PITCH_COMP OPTIONAL +// +// Adds pitch input to compensate for the loss of lift due to roll control. +// Default is 0.20 (20% of roll control also applied to pitch control). +// +// SERVO_YAW_P OPTIONAL +// SERVO_YAW_I OPTIONAL +// SERVO_YAW_D OPTIONAL +// +// P, I and D terms for the YAW control. Defaults are 0.5, 0, 0. +// +// SERVO_YAW_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// RUDDER_MIX OPTIONAL +// +// Roll to yaw mixing. This allows for co-ordinated turns. +// Default is 0.50 (50% of roll control also applied to yaw control.) +// +//#define SERVO_ROLL_P 0.4 +//#define SERVO_ROLL_I 0.0 +//#define SERVO_ROLL_D 0.0 +//#define SERVO_ROLL_INT_MAX 5 +//#define ROLL_SLEW_LIMIT 0 +//#define SERVO_PITCH_P 0.6 +//#define SERVO_PITCH_I 0.0 +//#define SERVO_PITCH_D 0.0 +//#define SERVO_PITCH_INT_MAX 5 +//#define PITCH_COMP 0.2 +//#define SERVO_YAW_P 0.5 +//#define SERVO_YAW_I 0.0 +//#define SERVO_YAW_D 0.0 +//#define SERVO_YAW_INT_MAX 5 +//#define RUDDER_MIX 0.5 +// + +////////////////////////////////////////////////////////////////////////////// +// Navigation control gains +// +// Tuning values for the navigation control PID loops. +// +// The P term is the primary tuning value. This determines how the control +// deflection varies in proportion to the required correction. +// +// The I term is used to control drift. +// +// The D term is used to control overshoot. Avoid adjusting this term if +// you are not familiar with tuning PID loops. +// +// Note: When tuning these values, start with changes of no more than 25% at +// a time. +// +// NAV_ROLL_P OPTIONAL +// NAV_ROLL_I OPTIONAL +// NAV_ROLL_D OPTIONAL +// +// P, I and D terms for navigation control over roll, normally used for +// controlling the aircraft's course. The P term controls how aggressively +// the aircraft will bank to change or hold course. +// Defaults are 0.7, 0.01, 0.02. +// +// NAV_ROLL_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// NAV_PITCH_ASP_P OPTIONAL +// NAV_PITCH_ASP_I OPTIONAL +// NAV_PITCH_ASP_D OPTIONAL +// +// P, I and D terms for pitch adjustments made to maintain airspeed. +// Defaults are 0.65, 0, 0. +// +// NAV_PITCH_ASP_INT_MAX OPTIONAL +// +// Maximum pitch offset due to the integral. This limits the control +// output from being overdriven due to a persistent offset (eg. inability +// to maintain the programmed airspeed). +// Default is 5 degrees. +// +// NAV_PITCH_ALT_P OPTIONAL +// NAV_PITCH_ALT_I OPTIONAL +// NAV_PITCH_ALT_D OPTIONAL +// +// P, I and D terms for pitch adjustments made to maintain altitude. +// Defaults are 0.65, 0, 0. +// +// NAV_PITCH_ALT_INT_MAX OPTIONAL +// +// Maximum pitch offset due to the integral. This limits the control +// output from being overdriven due to a persistent offset (eg. inability +// to maintain the programmed altitude). +// Default is 5 degrees. +// +//#define NAV_ROLL_P 0.7 +//#define NAV_ROLL_I 0.01 +//#define NAV_ROLL_D 0.02 +//#define NAV_ROLL_INT_MAX 5 +//#define NAV_PITCH_ASP_P 0.65 +//#define NAV_PITCH_ASP_I 0.0 +//#define NAV_PITCH_ASP_D 0.0 +//#define NAV_PITCH_ASP_INT_MAX 5 +//#define NAV_PITCH_ALT_P 0.65 +//#define NAV_PITCH_ALT_I 0.0 +//#define NAV_PITCH_ALT_D 0.0 +//#define NAV_PITCH_ALT_INT_MAX 5 +// + +////////////////////////////////////////////////////////////////////////////// +// Energy/Altitude control gains +// +// The Energy/altitude control system uses throttle input to control aircraft +// altitude. +// +// The P term is the primary tuning value. This determines how the throttle +// setting varies in proportion to the required correction. +// +// The I term is used to compensate for small offsets. +// +// The D term is used to control overshoot. Avoid adjusting this term if +// you are not familiar with tuning PID loops. +// +// THROTTLE_TE_P OPTIONAL +// THROTTLE_TE_I OPTIONAL +// THROTTLE_TE_D OPTIONAL +// +// P, I and D terms for throttle adjustments made to control altitude. +// Defaults are 0.5, 0, 0. +// +// THROTTLE_TE_INT_MAX OPTIONAL +// +// Maximum throttle input due to the integral term. This limits the +// throttle from being overdriven due to a persistent offset (e.g. +// inability to maintain the programmed altitude). +// Default is 20%. +// +// THROTTLE_SLEW_LIMIT OPTIONAL +// +// Limits the slew rate of the throttle, in percent per second. Helps +// avoid sudden throttle changes, which can destabilise the aircraft. +// A setting of zero disables the feature. +// Default is zero (disabled). +// +// P_TO_T OPTIONAL +// +// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR +// is DISABLED. Default is 2.5. +// +//#define THROTTLE_TE_P 0.50 +//#define THROTTLE_TE_I 0.0 +//#define THROTTLE_TE_D 0.0 +//#define THROTTLE_TE_INT_MAX 20 +//#define THROTTLE_SLEW_LIMIT 0 +//#define P_TO_T 2.5 +// + +////////////////////////////////////////////////////////////////////////////// +// Crosstrack compensation +// +// XTRACK_GAIN OPTIONAL +// +// Crosstrack compensation in degrees per metre off track. +// Default value is 0.01 degrees per metre. Values lower than 0.001 will +// disable crosstrack compensation. +// +// XTRACK_ENTRY_ANGLE OPTIONAL +// +// Maximum angle used to correct for track following. +// Default value is 30 degrees. +// +//#define XTRACK_GAIN 0.01 +//#define XTRACK_ENTRY_ANGLE 30 +// + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// DEBUGGING +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// Subsystem test and debug. +// +// DEBUG_SUBSYSTEM DEBUG +// +// Selects a subsystem debug mode. Default is 0. +// +// 0 = no debug +// 1 = Debug the Radio input +// 2 = Radio Setup / Servo output +// 4 = Debug the GPS input +// 5 = Debug the GPS input - RAW OUTPUT +// 6 = Debug the IMU +// 7 = Debug the Control Switch +// 8 = Debug the Servo DIP switches +// 9 = Debug the Relay out +// 10 = Debug the Magnetometer +// 11 = Debug the ABS pressure sensor +// 12 = Debug the stored waypoints +// 13 = Debug the Throttle +// 14 = Debug the Radio Min Max +// 15 = Debug the EEPROM - Hex Dump +// 16 = XBee X-CTU Range and RSSI Test +// 17 = Debug IMU - raw gyro and accel outputs +// 20 = Debug Analog Sensors +// +// +//#define DEBUG_SUBSYSTEM 0 +// + +////////////////////////////////////////////////////////////////////////////// +// DEBUG_LEVEL DEBUG +// +// Selects the lowest level of debug messages passed to the telemetry system. +// Default is SEVERITY_LOW. May be one of: +// +// SEVERITY_LOW +// SEVERITY_MEDIUM +// SEVERITY_HIGH +// SEVERITY_CRITICAL +// +//#define DEBUG_LEVEL SEVERITY_LOW +// + +////////////////////////////////////////////////////////////////////////////// +// Dataflash logging control +// +// Each of these logging options may be set to ENABLED to enable, or DISABLED +// to disable the logging of the respective data. +// +// LOG_ATTITUDE_FAST DEBUG +// +// Logs basic attitude info to the dataflash at 50Hz (uses more space). +// Defaults to DISABLED. +// +// LOG_ATTITUDE_MED OPTIONAL +// +// Logs basic attitude info to the dataflash at 10Hz (uses less space than +// LOG_ATTITUDE_FAST). Defaults to ENABLED. +// +// LOG_GPS OPTIONAL +// +// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. +// +// LOG_PM OPTIONAL +// +// Logs IMU performance monitoring info every 20 seconds. +// Defaults to DISABLED. +// +// LOG_CTUN OPTIONAL +// +// Logs control loop tuning info at 10 Hz. This information is useful for tuning +// servo control loop gain values. Defaults to DISABLED. +// +// LOG_NTUN OPTIONAL +// +// Logs navigation tuning info at 10 Hz. This information is useful for tuning +// navigation control loop gain values. Defaults to DISABLED. +// +// LOG_ MODE OPTIONAL +// +// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. +// +// LOG_RAW DEBUG +// +// Logs raw accelerometer and gyro data at 50 Hz (uses more space). +// Defaults to DISABLED. +// +// LOG_CMD OPTIONAL +// +// Logs new commands when they process. +// Defaults to ENABLED. +// +//#define LOG_ATTITUDE_FAST DISABLED +//#define LOG_ATTITUDE_MED ENABLED +//#define LOG_GPS ENABLED +//#define LOG_PM ENABLED +//#define LOG_CTUN DISABLED +//#define LOG_NTUN DISABLED +//#define LOG_MODE ENABLED +//#define LOG_RAW DISABLED +//#define LOG_CMD ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// Navigation defaults +// +// WP_RADIUS_DEFAULT OPTIONAL +// +// When the user performs a factory reset on the APM, set the waypoint radius +// (the radius from a target waypoint within which the APM will consider +// itself to have arrived at the waypoint) to this value in meters. This is +// mainly intended to allow users to start using the APM without running the +// WaypointWriter first. +// +// LOITER_RADIUS_DEFAULT OPTIONAL +// +// When the user performs a factory reset on the APM, set the loiter radius +// (the distance the APM will attempt to maintain from a waypoint while +// loitering) to this value in meters. This is mainly intended to allow +// users to start using the APM without running the WaypointWriter first. +// +//#define WP_RADIUS_DEFAULT 20 +//#define LOITER_RADIUS_DEFAULT 30 +// + +////////////////////////////////////////////////////////////////////////////// +// Debugging interface +// +// DEBUG_PORT OPTIONAL +// +// The APM will periodically send messages reporting what it is doing; this +// variable determines to which serial port they will be sent. Port 0 is the +// USB serial port on the shield, port 3 is the telemetry port. +// +//#define DEBUG_PORT 0 +// + +// +// Do not remove - this is to discourage users from copying this file +// and using it as-is rather than editing their own. +// + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference new file mode 100644 index 0000000000..b05a3c2af5 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference @@ -0,0 +1,914 @@ +// +// Example and reference ArduPilot Mega configuration file +// ======================================================= +// +// This file contains documentation and examples for configuration options +// supported by the ArduPilot Mega software. +// +// Most of these options are just that - optional. You should create +// the APM_Config.h file and use this file as a reference for options +// that you want to change. Don't copy this file directly; the options +// described here and their default values may change over time. +// +// Each item is marked with a keyword describing when you should set it: +// +// REQUIRED +// You must configure this in your APM_Config.h file. The +// software will not compile if the option is not set. +// +// OPTIONAL +// The option has a sensible default (which will be described +// here), but you may wish to override it. +// +// EXPERIMENTAL +// You should configure this option unless you are prepared +// to deal with potential problems. It may relate to a feature +// still in development, or which is not yet adequately tested. +// +// DEBUG +// The option should only be set if you are debugging the +// software, or if you are gathering information for a bug +// report. +// +// NOTE: +// Many of these settings are considered 'factory defaults', and the +// live value is stored and managed in the ArduPilot Mega EEPROM. +// Use the setup 'factoryreset' command after changing options in +// your APM_Config.h file. +// +// Units +// ----- +// +// Unless indicated otherwise, numeric quantities use the following units: +// +// Measurement | Unit +// ------------+------------------------------------- +// angle | degrees +// distance | metres +// speed | metres per second +// servo angle | microseconds +// voltage | volts +// times | seconds +// throttle | percent +// + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// HARDWARE CONFIGURATION AND CONNECTIONS +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// GPS_PROTOCOL REQUIRED +// +// GPS configuration, must be one of: +// +// GPS_PROTOCOL_NONE No GPS attached +// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. +// GPS_PROTOCOL_MTK MediaTek-based GPS. +// GPS_PROTOCOL_UBLOX UBLOX GPS +// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED +// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?) +// +//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX +// + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_SENSOR OPTIONAL +// AIRSPEED_RATIO OPTIONAL +// +// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. +// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed +// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s +// +// The default assumes that an airspeed sensor is connected. +// +//#define AIRSPEED_SENSOR ENABLED +//#define AIRSPEED_RATIO 1.9936 +// + +////////////////////////////////////////////////////////////////////////////// +// GCS_PROTOCOL OPTIONAL +// GCS_PORT OPTIONAL +// +// The GCS_PROTOCOL option determines which (if any) ground control station +// protocol will be used. Must be one of: +// +// GCS_PROTOCOL_NONE No GCS output +// GCS_PROTOCOL_STANDARD standard APM protocol +// GCS_PROTOCOL_SPECIAL special test protocol (?) +// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol +// GCS_PROTOCOL_XPLANE HIL simulation ground station +// GCS_PROTOCOL_IMU ArdiPilot IMU output +// GCS_PROTOCOL_JASON Jason's special secret GCS protocol +// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental) +// +// The GCS_PORT option determines which serial port will be used by the +// GCS protocol. The usual values are 0 for the console/USB port, +// or 3 for the telemetry port on the oilpan. Note that some protocols +// will ignore this value and always use the console port. +// +// The default GCS protocol is the standard ArduPilot Mega protocol. +// +// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD +// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial +// port is the FTDI/console port. GCS_PORT normally should not be set +// in your configuration. +// +//#define GCS_PROTOCOL GCS_PROTOCOL_STANDARD +//#define GCS_PORT 3 +// + +////////////////////////////////////////////////////////////////////////////// +// Serial port speeds. +// +// SERIAL0_BAUD OPTIONAL +// +// Baudrate for the console port. Default is 38400bps. +// +// SERIAL3_BAUD OPTIONAL +// +// Baudrate for the telemetry port. Default is 115200bps. +// +//#define SERIAL0_BAUD 38400 +//#define SERIAL3_BAUD 115200 +// + +////////////////////////////////////////////////////////////////////////////// +// Battery monitoring OPTIONAL +// +// See the manual for details on selecting divider resistors for battery +// monitoring via the oilpan. +// +// BATTERY_EVENT OPTIONAL +// +// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is +// DISABLED. +// +// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set +// +// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s +// battery. +// +// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. +// +// Normally derived from BATTERY_TYPE, the automatic value can be overridden +// here. Value in volts at which ArduPilot Mega should consider the +// battery to be "low". +// +// VOLT_DIV_RATIO OPTIONAL +// +// See the manual for details. The default value corresponds to the resistors +// recommended by the manual. +// +//#define BATTERY_EVENT DISABLED +//#define BATTERY_TYPE 0 +//#define LOW_VOLTAGE 11.4 +//#define VOLT_DIV_RATIO 3.0 +// + +////////////////////////////////////////////////////////////////////////////// +// INPUT_VOLTAGE OPTIONAL +// +// In order to have accurate pressure and battery voltage readings, this +// value should be set to the voltage measured on the 5V rail on the oilpan. +// +// See the manual for more details. The default value should be close. +// +//#define INPUT_VOLTAGE 5.0 +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// RADIO CONFIGURATION +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// FLIGHT_MODE OPTIONAL +// FLIGHT_MODE_CHANNEL OPTIONAL +// +// Flight modes assigned to the control channel, and the input channel that +// is read for the control mode. +// +// Use a servo tester, or the ArduPilotMega_demo test program to check your +// switch settings. +// +// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and +// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option +// uses channel numbers 1-8 (and defaults to 8). +// +// If you only have a three-position switch or just want three modes, set your +// switch to produce 1165, 1425, and 1815 microseconds and configure +// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. +// +// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control +// channel connected to input channel 8, the hardware failsafe mode will +// activate for any control input over 1750ms. +// +// For more modes (up to six), set your switch(es) to produce any of 1165, +// 1295, 1425, 1555, 1685, and 1815 microseconds. +// +// Flight mode | Switch Setting (ms) +// ------------+--------------------- +// 1 | 1165 +// 2 | 1295 +// 3 | 1425 +// 4 | 1555 +// 5 | 1685 +// 6 | 1815 (FAILSAFE if using channel 8) +// +// The following standard flight modes are available: +// +// Name | Description +// -----------------+-------------------------------------------- +// | +// MANUAL | Full manual control via the hardware multiplexer. +// | +// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. +// | +// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. +// | +// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. +// | +// RTL | Returns to the Home location and then LOITERs at a safe altitude. +// | +// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter +// | application or your Ground Control System to edit and upload +// | waypoints and other commands. +// | +// +// +// The following non-standard modes are EXPERIMENTAL: +// +// Name | Description +// -----------------+-------------------------------------------- +// | +// LOITER | Flies in a circle around the current location. +// | +// CIRCLE | Flies in a stabilized 'dumb' circle. +// | +// +// +// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and +// FLIGHT_MODE_6 should be MANUAL. +// +// +//#define FLIGHT_MODE_CHANNEL 8 +// +//#define FLIGHT_MODE_1 RTL +//#define FLIGHT_MODE_2 RTL +//#define FLIGHT_MODE_3 FLY_BY_WIRE_A +//#define FLIGHT_MODE_4 FLY_BY_WIRE_A +//#define FLIGHT_MODE_5 MANUAL +//#define FLIGHT_MODE_6 MANUAL +// + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_FAILSAFE OPTIONAL +// THROTTLE_FS_VALUE OPTIONAL +// THROTTLE_FAILSAFE_ACTION OPTIONAL +// +// The throttle failsafe allows you to configure a software failsafe activated +// by a setting on the throttle input channel (channel 3). +// +// This can be used to achieve a failsafe override on loss of radio control +// without having to sacrifice one of your FLIGHT_MODE settings, as the +// throttle failsafe overrides the switch-selected mode. +// +// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default +// is for it to be disabled. +// +// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value +// below which the failsafe engages. The default is 975ms, which is a very low +// throttle setting. Most transmitters will let you trim the manual throttle +// position up so that you cannot engage the failsafe with a regular stick movement. +// +// Configure your receiver's failsafe setting for the throttle channel to the +// absolute minimum, and use the ArduPilotMega_demo program to check that +// you cannot reach that value with the throttle control. Leave a margin of +// at least 50 microseconds between the lowest throttle setting and +// THROTTLE_FS_VALUE. +// +// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe +// mode is entered while flying in AUTO mode. This is important in order to avoid +// accidental failsafe behaviour when flying waypoints that take the aircraft +// temporarily out of radio range. +// +// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, +// the aircraft will head for home in RTL mode. If the throttle channel moves +// back up, it will return to AUTO or LOITER mode. +// +// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes. +// +//#define THROTTLE_FAILSAFE DISABLED +//#define THROTTLE_FS_VALUE 975 +//#define THROTTLE_FAILSAFE_ACTION 2 +// + +////////////////////////////////////////////////////////////////////////////// +// AUTO_TRIM OPTIONAL +// +// ArduPilot Mega can update its trim settings by looking at the +// radio inputs when switching out of MANUAL mode. This allows you to +// manually trim your aircraft before switching to an assisted mode, but it +// also means that you should avoid switching out of MANUAL while you have +// any control stick deflection. +// +// The default is to enable AUTO_TRIM. +// +//#define AUTO_TRIM ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_REVERSE OPTIONAL +// +// A few speed controls require the throttle servo signal be reversed. Setting +// this to ENABLED will reverse the throttle output signal. Ensure that your +// throttle needs to be reversed by using the hardware failsafe and the +// ArduPilotMega_demo program before setting this option. +// +// The default is to not reverse the signal. +// +//#define THROTTLE_REVERSE DISABLED +// + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_STICK_MIXING OPTIONAL +// +// If this option is set to ENABLED, manual control inputs are are respected +// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) +// +// The default is to enable stick mixing, allowing the pilot to take +// emergency action without switching modes. +// +//#define ENABLE_STICK_MIXING ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_OUT DEBUG +// +// When debugging, it can be useful to disable the throttle output. Set +// this option to DISABLED to disable throttle output signals. +// +// The default is to not disable throttle output. +// +//#define THROTTLE_OUT ENABLED +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// STARTUP BEHAVIOUR +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// GROUND_START_DELAY OPTIONAL +// +// If configured, inserts a delay between power-up and the beginning of IMU +// calibration during a ground start. +// +// Use this setting to give you time to position the aircraft horizontally +// for the IMU calibration. +// +// The default is to begin IMU calibration immediately at startup. +// +//#define GROUND_START_DELAY 0 +// + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_AIR_START OPTIONAL +// +// If air start is disabled then you will get a ground start (including IMU +// calibration) every time the AP is powered up. This means that if you get +// a power glitch or reboot for some reason in the air, you will probably +// crash, but it prevents a lot of problems on the ground like unintentional +// motor start-ups, etc. +// +// If air start is enabled then you will get an air start at power up and a +// ground start will be performed if the speed is near zero when we get gps +// lock. +// +// The default is to disable air start. +// +//#define ENABLE_AIR_START 0 +// + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// FLIGHT AND NAVIGATION CONTROL +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// Altitude measurement and control. +// +// AOA OPTIONAL +// +// The angle in 100ths of a degree that the nose of the aircraft should be +// raised from horizontal in level flight. The default is 1 degree. +// +//#define AOA 100 // note, 100ths of a degree +// +// ALT_EST_GAIN OPTIONAL +// +// The gain of the altitude estimation function; a lower number results +// in slower error correction and smoother output. The default is a +// reasonable starting point. +// +//#define ALT_EST_GAIN 0.01 +// +// ALTITUDE_MIX OPTIONAL +// +// Configures the blend between GPS and pressure altitude. +// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. +// +// The default is to use only GPS altitude. +// +//#define ALTITUDE_MIX 0 +// + + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_HIL OPTIONAL +// +// This will output a binary control string to for use in HIL sims +// such as Xplane 9 or FlightGear. +// +//#define ENABLE_HIL ENABLED +// + + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_CRUISE OPTIONAL +// +// The speed in metres per second to maintain during cruise. The default +// is 10m/s, which is a conservative value suitable for relatively small, +// light aircraft. +// +//#define AIRSPEED_CRUISE 10 +// + +////////////////////////////////////////////////////////////////////////////// +// FLY_BY_WIRE_B airspeed control +// +// AIRSPEED_FBW_MIN OPTIONAL +// AIRSPEED_FBW_MAX OPTIONAL +// +// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode. +// The defaults are 6 and 30 metres per second. +// +// THROTTLE_ALT_P OPTIONAL +// THROTTLE_ALT_I OPTIONAL +// THROTTLE_ALT_D OPTIONAL +// +// P, I and D terms for the throttle control loop. Defaults are 0.5, 0, 0. +// +// THROTTLE_ALT_INT_MAX OPTIONAL +// +// Maximum throttle input due to the integral. Limits the throttle input +// due to persistent inability to maintain the commanded speed. Helps +// prevent the throttle from staying wide open when the control is reduced +// after a period at maxium speed. +// Default is 20 (20%). +// +//#define AIRSPEED_FBW_MIN 6 +//#define AIRSPEED_FBW_MAX 30 +//#define THROTTLE_ALT_P 0.32 +//#define THROTTLE_ALT_I 0.04 +//#define THROTTLE_ALT_D 0.0 +//#define THROTTLE_ALT_INT_MAX 20 +// + +////////////////////////////////////////////////////////////////////////////// +// Throttle control +// +// THROTTLE_MIN OPTIONAL +// +// The minimum throttle setting to which the autopilot will reduce the +// throttle while descending. The default is zero, which is +// suitable for aircraft with a steady power-off glide. Increase this +// value if your aircraft needs throttle to maintain a stable descent in +// level flight. +// +// THROTTLE_CRUISE OPTIONAL +// +// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. +// The default is 45%, which is reasonable for a modestly powered aircraft. +// +// THROTTLE_MAX OPTIONAL +// +// The maximum throttle setting the autopilot will apply. The default is 75%. +// Reduce this value if your aicraft is overpowered, or has complex flight +// characteristics at high throttle settings. +// +//#define THROTTLE_MIN 0 +//#define THROTTLE_CRUISE 45 +//#define THROTTLE_MAX 75 +// + +////////////////////////////////////////////////////////////////////////////// +// Autopilot control limits +// +// HEAD_MAX OPTIONAL +// +// The maximum commanded bank angle in either direction. +// The default is 45 degrees. Decrease this value if your aircraft is not +// stable or has difficulty maintaining altitude in a steep bank. +// +// PITCH_MAX OPTIONAL +// +// The maximum commanded pitch up angle. +// The default is 15 degrees. Care should be taken not to set this value too +// large, as the aircraft may stall. +// +// PITCH_MIN +// +// The maximum commanded pitch down angle. Note that this value must be +// negative. The default is -25 degrees. Care should be taken not to set +// this value too large as it may result in overspeeding the aircraft. +// +//#define HEAD_MAX 45 +//#define PITCH_MAX 15 +//#define PITCH_MIN -25 + +////////////////////////////////////////////////////////////////////////////// +// Attitude control gains +// +// Tuning values for the attitude control PID loops. +// +// The P term is the primary tuning value. This determines how the control +// deflection varies in proportion to the required correction. +// +// The I term is used to help control surfaces settle. This value should +// normally be kept low. +// +// The D term is used to control overshoot. Avoid using or adjusting this +// term if you are not familiar with tuning PID loops. It should normally +// be zero for most aircraft. +// +// Note: When tuning these values, start with changes of no more than 25% at +// a time. +// +// SERVO_ROLL_P OPTIONAL +// SERVO_ROLL_I OPTIONAL +// SERVO_ROLL_D OPTIONAL +// +// P, I and D terms for roll control. Defaults are 0.4, 0, 0. +// +// SERVO_ROLL_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// ROLL_SLEW_LIMIT EXPERIMENTAL +// +// Limits the slew rate of the roll control in degrees per second. If zero, +// slew rate is not limited. Default is to not limit the roll control slew rate. +// (This feature is currently not implemented.) +// +// SERVO_PITCH_P OPTIONAL +// SERVO_PITCH_I OPTIONAL +// SERVO_PITCH_D OPTIONAL +// +// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. +// +// SERVO_PITCH_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. native flight +// AoA). If you find this value is insufficient, consider adjusting the AOA +// parameter. +// Default is 5 degrees. +// +// PITCH_COMP OPTIONAL +// +// Adds pitch input to compensate for the loss of lift due to roll control. +// Default is 0.20 (20% of roll control also applied to pitch control). +// +// SERVO_YAW_P OPTIONAL +// SERVO_YAW_I OPTIONAL +// SERVO_YAW_D OPTIONAL +// +// P, I and D terms for the YAW control. Defaults are 0.5, 0, 0. +// +// SERVO_YAW_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// RUDDER_MIX OPTIONAL +// +// Roll to yaw mixing. This allows for co-ordinated turns. +// Default is 0.50 (50% of roll control also applied to yaw control.) +// +//#define SERVO_ROLL_P 0.4 +//#define SERVO_ROLL_I 0.0 +//#define SERVO_ROLL_D 0.0 +//#define SERVO_ROLL_INT_MAX 5 +//#define ROLL_SLEW_LIMIT 0 +//#define SERVO_PITCH_P 0.6 +//#define SERVO_PITCH_I 0.0 +//#define SERVO_PITCH_D 0.0 +//#define SERVO_PITCH_INT_MAX 5 +//#define PITCH_COMP 0.2 +//#define SERVO_YAW_P 0.5 +//#define SERVO_YAW_I 0.0 +//#define SERVO_YAW_D 0.0 +//#define SERVO_YAW_INT_MAX 5 +//#define RUDDER_MIX 0.5 +// + +////////////////////////////////////////////////////////////////////////////// +// Navigation control gains +// +// Tuning values for the navigation control PID loops. +// +// The P term is the primary tuning value. This determines how the control +// deflection varies in proportion to the required correction. +// +// The I term is used to control drift. +// +// The D term is used to control overshoot. Avoid adjusting this term if +// you are not familiar with tuning PID loops. +// +// Note: When tuning these values, start with changes of no more than 25% at +// a time. +// +// NAV_ROLL_P OPTIONAL +// NAV_ROLL_I OPTIONAL +// NAV_ROLL_D OPTIONAL +// +// P, I and D terms for navigation control over roll, normally used for +// controlling the aircraft's course. The P term controls how aggressively +// the aircraft will bank to change or hold course. +// Defaults are 0.7, 0.01, 0.02. +// +// NAV_ROLL_INT_MAX OPTIONAL +// +// Maximum control offset due to the integral. This prevents the control +// output from being overdriven due to a persistent offset (e.g. crosstracking). +// Default is 5 degrees. +// +// NAV_PITCH_ASP_P OPTIONAL +// NAV_PITCH_ASP_I OPTIONAL +// NAV_PITCH_ASP_D OPTIONAL +// +// P, I and D terms for pitch adjustments made to maintain airspeed. +// Defaults are 0.65, 0, 0. +// +// NAV_PITCH_ASP_INT_MAX OPTIONAL +// +// Maximum pitch offset due to the integral. This limits the control +// output from being overdriven due to a persistent offset (eg. inability +// to maintain the programmed airspeed). +// Default is 5 degrees. +// +// NAV_PITCH_ALT_P OPTIONAL +// NAV_PITCH_ALT_I OPTIONAL +// NAV_PITCH_ALT_D OPTIONAL +// +// P, I and D terms for pitch adjustments made to maintain altitude. +// Defaults are 0.65, 0, 0. +// +// NAV_PITCH_ALT_INT_MAX OPTIONAL +// +// Maximum pitch offset due to the integral. This limits the control +// output from being overdriven due to a persistent offset (eg. inability +// to maintain the programmed altitude). +// Default is 5 degrees. +// +//#define NAV_ROLL_P 0.7 +//#define NAV_ROLL_I 0.01 +//#define NAV_ROLL_D 0.02 +//#define NAV_ROLL_INT_MAX 5 +//#define NAV_PITCH_ASP_P 0.65 +//#define NAV_PITCH_ASP_I 0.0 +//#define NAV_PITCH_ASP_D 0.0 +//#define NAV_PITCH_ASP_INT_MAX 5 +//#define NAV_PITCH_ALT_P 0.65 +//#define NAV_PITCH_ALT_I 0.0 +//#define NAV_PITCH_ALT_D 0.0 +//#define NAV_PITCH_ALT_INT_MAX 5 +// + +////////////////////////////////////////////////////////////////////////////// +// Energy/Altitude control gains +// +// The Energy/altitude control system uses throttle input to control aircraft +// altitude. +// +// The P term is the primary tuning value. This determines how the throttle +// setting varies in proportion to the required correction. +// +// The I term is used to compensate for small offsets. +// +// The D term is used to control overshoot. Avoid adjusting this term if +// you are not familiar with tuning PID loops. +// +// THROTTLE_TE_P OPTIONAL +// THROTTLE_TE_I OPTIONAL +// THROTTLE_TE_D OPTIONAL +// +// P, I and D terms for throttle adjustments made to control altitude. +// Defaults are 0.5, 0, 0. +// +// THROTTLE_TE_INT_MAX OPTIONAL +// +// Maximum throttle input due to the integral term. This limits the +// throttle from being overdriven due to a persistent offset (e.g. +// inability to maintain the programmed altitude). +// Default is 20%. +// +// THROTTLE_SLEW_LIMIT OPTIONAL +// +// Limits the slew rate of the throttle, in percent per second. Helps +// avoid sudden throttle changes, which can destabilise the aircraft. +// A setting of zero disables the feature. +// Default is zero (disabled). +// +// P_TO_T OPTIONAL +// +// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR +// is DISABLED. Default is 2.5. +// +//#define THROTTLE_TE_P 0.50 +//#define THROTTLE_TE_I 0.0 +//#define THROTTLE_TE_D 0.0 +//#define THROTTLE_TE_INT_MAX 20 +//#define THROTTLE_SLEW_LIMIT 0 +//#define P_TO_T 2.5 +// + +////////////////////////////////////////////////////////////////////////////// +// Crosstrack compensation +// +// XTRACK_GAIN OPTIONAL +// +// Crosstrack compensation in degrees per metre off track. +// Default value is 0.01 degrees per metre. Values lower than 0.001 will +// disable crosstrack compensation. +// +// XTRACK_ENTRY_ANGLE OPTIONAL +// +// Maximum angle used to correct for track following. +// Default value is 30 degrees. +// +//#define XTRACK_GAIN 0.01 +//#define XTRACK_ENTRY_ANGLE 30 +// + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// DEBUGGING +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// Subsystem test and debug. +// +// DEBUG_SUBSYSTEM DEBUG +// +// Selects a subsystem debug mode. Default is 0. +// +// 0 = no debug +// 1 = Debug the Radio input +// 2 = Radio Setup / Servo output +// 4 = Debug the GPS input +// 5 = Debug the GPS input - RAW OUTPUT +// 6 = Debug the IMU +// 7 = Debug the Control Switch +// 8 = Debug the Servo DIP switches +// 9 = Debug the Relay out +// 10 = Debug the Magnetometer +// 11 = Debug the ABS pressure sensor +// 12 = Debug the stored waypoints +// 13 = Debug the Throttle +// 14 = Debug the Radio Min Max +// 15 = Debug the EEPROM - Hex Dump +// 16 = XBee X-CTU Range and RSSI Test +// 17 = Debug IMU - raw gyro and accel outputs +// 20 = Debug Analog Sensors +// +// +//#define DEBUG_SUBSYSTEM 0 +// + +////////////////////////////////////////////////////////////////////////////// +// DEBUG_LEVEL DEBUG +// +// Selects the lowest level of debug messages passed to the telemetry system. +// Default is SEVERITY_LOW. May be one of: +// +// SEVERITY_LOW +// SEVERITY_MEDIUM +// SEVERITY_HIGH +// SEVERITY_CRITICAL +// +//#define DEBUG_LEVEL SEVERITY_LOW +// + +////////////////////////////////////////////////////////////////////////////// +// Dataflash logging control +// +// Each of these logging options may be set to ENABLED to enable, or DISABLED +// to disable the logging of the respective data. +// +// LOG_ATTITUDE_FAST DEBUG +// +// Logs basic attitude info to the dataflash at 50Hz (uses more space). +// Defaults to DISABLED. +// +// LOG_ATTITUDE_MED OPTIONAL +// +// Logs basic attitude info to the dataflash at 10Hz (uses less space than +// LOG_ATTITUDE_FAST). Defaults to ENABLED. +// +// LOG_GPS OPTIONAL +// +// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. +// +// LOG_PM OPTIONAL +// +// Logs IMU performance monitoring info every 20 seconds. +// Defaults to DISABLED. +// +// LOG_CTUN OPTIONAL +// +// Logs control loop tuning info at 10 Hz. This information is useful for tuning +// servo control loop gain values. Defaults to DISABLED. +// +// LOG_NTUN OPTIONAL +// +// Logs navigation tuning info at 10 Hz. This information is useful for tuning +// navigation control loop gain values. Defaults to DISABLED. +// +// LOG_ MODE OPTIONAL +// +// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. +// +// LOG_RAW DEBUG +// +// Logs raw accelerometer and gyro data at 50 Hz (uses more space). +// Defaults to DISABLED. +// +// LOG_CMD OPTIONAL +// +// Logs new commands when they process. +// Defaults to ENABLED. +// +//#define LOG_ATTITUDE_FAST DISABLED +//#define LOG_ATTITUDE_MED ENABLED +//#define LOG_GPS ENABLED +//#define LOG_PM ENABLED +//#define LOG_CTUN DISABLED +//#define LOG_NTUN DISABLED +//#define LOG_MODE ENABLED +//#define LOG_RAW DISABLED +//#define LOG_CMD ENABLED +// + +////////////////////////////////////////////////////////////////////////////// +// Navigation defaults +// +// WP_RADIUS_DEFAULT OPTIONAL +// +// When the user performs a factory reset on the APM, set the waypoint radius +// (the radius from a target waypoint within which the APM will consider +// itself to have arrived at the waypoint) to this value in meters. This is +// mainly intended to allow users to start using the APM without running the +// WaypointWriter first. +// +// LOITER_RADIUS_DEFAULT OPTIONAL +// +// When the user performs a factory reset on the APM, set the loiter radius +// (the distance the APM will attempt to maintain from a waypoint while +// loitering) to this value in meters. This is mainly intended to allow +// users to start using the APM without running the WaypointWriter first. +// +//#define WP_RADIUS_DEFAULT 20 +//#define LOITER_RADIUS_DEFAULT 30 +// + +////////////////////////////////////////////////////////////////////////////// +// Debugging interface +// +// DEBUG_PORT OPTIONAL +// +// The APM will periodically send messages reporting what it is doing; this +// variable determines to which serial port they will be sent. Port 0 is the +// USB serial port on the shield, port 3 is the telemetry port. +// +//#define DEBUG_PORT 0 +// + +// +// Do not remove - this is to discourage users from copying this file +// and using it as-is rather than editing their own. +// +#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need. diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h new file mode 100644 index 0000000000..06a3078582 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h @@ -0,0 +1,19 @@ +#define DEBUG_SUBSYSTEM 0 + +#define FLIGHT_MODE_CHANNEL 8 +#define FLIGHT_MODE_1 AUTO +#define FLIGHT_MODE_2 RTL +#define FLIGHT_MODE_3 FLY_BY_WIRE_A +#define FLIGHT_MODE_4 FLY_BY_WIRE_B +#define FLIGHT_MODE_5 STABILIZE +#define FLIGHT_MODE_6 MANUAL + +#define GCS_PROTOCOL GCS_PROTOCOL_DEBUGTERMINAL +#define ENABLE_HIL ENABLED +#define GCS_PORT 3 +#define GPS_PROTOCOL GPS_PROTOCOL_IMU +#define AIRSPEED_CRUISE 25 + +#define THROTTLE_FAILSAFE ENABLED +#define AIRSPEED_SENSOR ENABLED + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde new file mode 100644 index 0000000000..f5dfaef424 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde @@ -0,0 +1,845 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/* +ArduPilotMega Version 1.0.3 Public Alpha +Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short +Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi +Please contribute your ideas! + + +This firmware is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. +*/ + +// Libraries +#include +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot GPS library +#include +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Magnetometer Library + +// AVR runtime +#include +#include +#include +#include + +// Configuration +#include "config.h" + +// Local modules +#include "defines.h" + +// Serial ports +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU) +//FastSerialPort3(Serial3); // Telemetry port (optional, Standard and ArduPilot protocols only) + +// GPS selection +#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA +AP_GPS_NMEA GPS(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF +AP_GPS_SIRF GPS(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX +AP_GPS_UBLOX GPS(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_IMU +AP_GPS_IMU GPS(&Serial); // note, console port +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK +AP_GPS_MTK GPS(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE +AP_GPS_NONE GPS(NULL); +#else +# error Must define GPS_PROTOCOL in your configuration file. +#endif + +// The X-Plane GCS requires the IMU GPS configuration +#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU) +# error Must select GPS_PROTOCOL_IMU when configuring for X-Plane +#endif + +// GENERAL VARIABLE DECLARATIONS +// -------------------------------------------- +byte control_mode = MANUAL; +boolean failsafe = false; // did our throttle dip below the failsafe value? +boolean ch3_failsafe = false; +byte crash_timer; +byte oldSwitchPosition; // for remembering the control mode switch +boolean reverse_switch = 1; // do we read the reversing switches after startup? + +byte ground_start_count = 6; // have we achieved first lock and set Home? +int ground_start_avg; // 5 samples to avg speed for ground start +boolean ground_start = false; // have we started on the ground? +char *comma = ","; + +char* flight_mode_strings[] = { + "Manual", + "Circle", + "Stabilize", + "", + "", + "FBW_A", + "FBW_B", + "", + "", + "", + "Auto", + "RTL", + "Loiter", + "Takeoff", + "Land"}; + + +/* Radio values + Channel assignments + 1 Ailerons (rudder if no ailerons) + 2 Elevator + 3 Throttle + 4 Rudder (if we have ailerons) + 5 Mode + 6 TBD + 7 TBD + 8 TBD +*/ + +int radio_min[8]; // may be reset by init sequence +int radio_trim[8]; // may be reset by init sequence +int radio_max[8]; // may be reset by init sequence +int radio_in[8]; // current values from the transmitter - microseconds +int radio_out[8]; // Send to the PWM library +int servo_out[8]; // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 + +int elevon1_trim = 1500; +int elevon2_trim = 1500; +int ch1_temp = 1500; // Used for elevon mixing +int ch2_temp = 1500; + +int reverse_roll = 1; +int reverse_pitch = 1; +int reverse_rudder = 1; +byte mix_mode = 0; // 0 = normal , 1 = elevons +int reverse_elevons = 1; +int reverse_ch1_elevon = 1; +int reverse_ch2_elevon = 1; +byte yaw_mode; + +byte flight_mode_channel; +byte flight_modes[6]; +byte auto_trim; + +// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon + + +/* PID Control variables + Cases + 1 Aileron servo control (rudder if no ailerons) + 2 Elevator servo control + 3 Rudder servo control (if we have ailerons) + 4 Roll set-point control + 5 Pitch set-point based on airspeed error control + 6 Pitch set-point based on altitude error control (if we do not have airspeed sensor) + 7 Throttle based on Energy Height (Total Energy) error control + 8 Throttle based on altitude error control +*/ + +float kp[8]; +float ki[8]; +float kd[8]; +uint16_t integrator_max[8]; // PID Integrator Max deg * 100 +float integrator[8]; // PID Integrators deg * 100 +long last_error[8]; // PID last error for derivative +float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind + + +/* Feed Forward gains + Cases + 1 Bank angle to desired pitch (Pitch Comp) + 2 Aileron Servo to Rudder Servo + 3 Pitch to Throttle +*/ + +float kff[3]; + + +// GPS variables +// ------------- +const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +float scaleLongUp; // used to reverse longtitude scaling +float scaleLongDown; // used to reverse longtitude scaling +boolean GPS_light = false; // status of the GPS light + +// Location & Navigation +// --------------------- +byte wp_radius = 20; // meters +long hold_course = -1; // deg * 100 dir of plane +long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate +byte loiter_radius; // meters +float x_track_gain; +int x_track_angle; + +long head_max; +long pitch_max; +long pitch_min; +float altitude_mix; + +byte command_must_index; // current command memory location +byte command_may_index; // current command memory location +byte command_must_ID; // current command ID +byte command_may_ID; // current command ID +//byte EEPROM_command // 1 = from the list, 0 = generated + + +// Airspeed +// -------- +int airspeed; // m/s * 100 +int airspeed_cruise; // m/s * 100 : target airspeed sensor value +float airspeed_ratio; // used to scale airspeed +byte airspeed_fbw_min; // m/s +byte airspeed_fbw_max; // m/s + +// Throttle Failsafe +// ------------------ +byte throttle_failsafe_enabled; +int throttle_failsafe_value; +byte throttle_failsafe_action; +uint16_t log_bitmask; + +// Location Errors +// --------------- +long bearing_error; // deg * 100 : 0 to 36000 +long altitude_error; // meters * 100 we are off in altitude +float airspeed_error; // m/s * 100 +float crosstrack_error; // meters we are off trackline +long energy_error; // energy state error (kinetic + potential) for altitude hold +long airspeed_energy_error; // kinetic portion of energy error + +// Sensors +// -------- +float airpressure_raw; // Airspeed Sensor - is a float to better handle filtering +int airpressure_offset; // analog air pressure sensor while still +int airpressure; // airspeed as a pressure value +float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2, initialized above threshold for filter +float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3, initialized above threshold for filter +float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3+4, initialized above threshold for filter + +// From IMU +long roll_sensor; // degrees * 100 +long pitch_sensor; // degrees * 100 +long yaw_sensor; // degrees * 100 + +float roll; // radians +float pitch; // radians +float yaw; // radians + + +// Magnetometer variables +int magnetom_x; // Doug to do +int magnetom_y; +int magnetom_z; +float MAG_Heading; + +// Pressure Sensor variables +unsigned long abs_press; +unsigned long abs_press_filt; +unsigned long abs_press_gnd; +int ground_temperature; +int temp_unfilt; +long ground_alt; // Ground altitude from gps at startup in centimeters +long press_alt; // Pressure altitude + +// flight mode specific +// -------------------- +//boolean takeoff_complete = false; // Flag for using take-off controls +boolean land_complete = false; +int landing_pitch; // pitch for landing set by commands +int takeoff_pitch; +int takeoff_altitude; +int landing_distance; // meters; + +// Loiter management +// ----------------- +long old_target_bearing; // deg * 100 +int loiter_total; // deg : how many times to loiter * 360 +int loiter_delta; // deg : how far we just turned +int loiter_sum; // deg : how far we have turned around a waypoint +long loiter_time; // millis : when we started LOITER mode +int loiter_time_max; // millis : how long to stay in LOITER mode + +// these are the values for navigation control functions +// ---------------------------------------------------- +long nav_roll; // deg * 100 : target roll angle +long nav_pitch; // deg * 100 : target pitch angle +long altitude_estimate; // for smoothing GPS output + +int throttle_min; // 0-100 : target throttle output for average speed +int throttle_cruise; // 0-100 : target throttle output for average speed +int throttle_max; // 0-100 : target throttle output for average speed + +// Waypoints +// --------- +long wp_distance; // meters - distance between plane and next waypoint +long wp_totalDistance; // meters - distance between old and next waypoint +byte wp_total; // # of Commands total including way +byte wp_index; // Current active command index +byte next_wp_index; // Current active command index + +// repeating event control +// ----------------------- +byte event_id; // what to do - see defines +long event_timer; // when the event was asked for in ms +int event_delay; // how long to delay the next firing of event in millis +int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice +int event_value; // per command value, such as PWM for servos +int event_undo_value; // the value used to undo commands +byte repeat_forever; +byte undo_event; // counter for timing the undo + +// delay command +// -------------- +int delay_timeout = 0; // used to delay commands +long delay_start = 0; // used to delay commands + +// 3D Location vectors +// ------------------- +struct Location home; // home location +struct Location prev_WP; // last waypoint +struct Location current_loc; // current location +struct Location next_WP; // next waypoint +struct Location tell_command; // command for telemetry +struct Location next_command; // command preloaded +long target_altitude; // used for +long offset_altitude; // used for +int hold_alt_above_home; +boolean home_is_set = false; // Flag for if we have gps lock and have set the home location + + +// IMU variables +// ------------- + +//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ +uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware +int SENSOR_SIGN[] = { 1, -1, -1, + -1, 1, 1, + -1, -1, -1}; + +// Temp compensation curve constants +// These must be produced by measuring data and curve fitting +// [X / Y/Z gyro][A / B/C or 0 order / 1st order / 2nd order constants] +// values may migrate to a Config file +float GTC[3][3]={{1665, 0, 0}, + {1665, 0, 0}, + {1665, 0, 0}}; + +float AN[6]; // array that store the 6 ADC channels used by IMU +float AN_OFFSET[6]; // Array that store the Offset of the gyros and accelerometers +float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) +float Accel_Vector[3]; // Store the acceleration in a vector +float Accel_Vector_unfiltered[3]; // Store the acceleration in a vector +float Gyro_Vector[3]; // Store the gyros turn rate in a vector +float Omega_Vector[3]; // Corrected Gyro_Vector data +float Omega_P[3]; // Omega Proportional correction +float Omega_I[3]; // Omega Integrator +float Omega[3]; +float errorRollPitch[3]; +float errorYaw[3]; +float errorCourse; +float COGX; // Course overground X axis +float COGY = 1; // Course overground Y axis + +float DCM_Matrix[3][3] = {{1,0,0}, + {0,1,0}, + {0,0,1}}; + +float Update_Matrix[3][3] = {{0,1,2}, + {3,4,5}, + {6,7,8}}; + +float Temporary_Matrix[3][3]; + +// Performance monitoring +// ---------------------- +long perf_mon_timer; +float imu_health; // Metric based on accel gain deweighting +int G_Dt_max; // Max main loop cycle time in milliseconds +byte gyro_sat_count; +byte adc_constraints; +byte renorm_sqrt_count; +byte renorm_blowup_count; +int gps_fix_count; +byte gcs_messages_sent; + + +// GCS +// --- +char GCS_buffer[53]; + +// System Timers +// -------------- +unsigned long fast_loopTimer; // Time in miliseconds of main control loop +unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop +byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops +byte slow_loopCounter = 0; +byte superslow_loopCounter = 0; +unsigned long deltaMiliSeconds; // Delta Time in miliseconds +unsigned long dTnav; // Delta Time in milliseconds for navigation computations +int mainLoop_count; +unsigned long elapsedTime; // for doing custom events +unsigned int GPS_timer; + +//******************************************************************************************************************************** +//******************************************************************************************************************************** +//******************************************************************************************************************************** +//******************************************************************************************************************************** +//******************************************************************************************************************************** + +// Basic Initialization +//--------------------- +void setup() { + jeti_init(); + jeti_status(" ** X-DIY **"); + jeti_update; + init_ardupilot(); +} + +void loop() +{ + // We want this to execute at 50Hz if possible + // ------------------------------------------- + if (millis()-fast_loopTimer > 19) { + deltaMiliSeconds = millis() - fast_loopTimer; + G_Dt = (float)deltaMiliSeconds / 1000.f; + fast_loopTimer = millis(); + mainLoop_count++; + + // Execute the fast loop + // --------------------- + fast_loop(); + + // Execute the medium loop + // ----------------------- + medium_loop(); + + if (millis()- perf_mon_timer > 20000) { + if (mainLoop_count != 0) { + send_message(MSG_PERF_REPORT); + if (log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); + + resetPerfData(); + } + } + } +} + +void fast_loop() +{ + // This is the fast loop - we want it to execute at 50Hz if possible + // ----------------------------------------------------------------- + if (deltaMiliSeconds > G_Dt_max) + G_Dt_max = deltaMiliSeconds; + + // Read radio + // ---------- + read_radio(); + + // check for throttle failsafe condition + // ------------------------------------ +#if THROTTLE_FAILSAFE == 1 + set_failsafe(ch3_failsafe); +#endif + + + // read in the plane's attitude + // ---------------------------- +#if GPS_PROTOCOL == GPS_PROTOCOL_IMU + GPS.update(); + airspeed = (float)GPS.airspeed; //airspeed is * 100 coming in from Xplane for accuracy + calc_airspeed_errors(); + + if(digitalRead(SLIDE_SWITCH_PIN) == 0) { + read_AHRS(); + roll_sensor = -roll_sensor; + pitch_sensor = -pitch_sensor; + //yaw_sensor = -yaw_sensor; + }else{ + roll_sensor = GPS.roll_sensor; + pitch_sensor = GPS.pitch_sensor; + yaw_sensor = GPS.ground_course; + } + +#else + // Read Airspeed + // ------------- + #if AIRSPEED_SENSOR == 1 + read_airspeed(); + #endif + + read_AHRS(); + + if (log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (uint16_t)yaw_sensor); + + if (log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); +#endif + + // altitude smoothing + // ------------------ + if (control_mode != FLY_BY_WIRE_B) + calc_altitude_error(); + + // custom code/exceptions for flight modes + // --------------------------------------- + update_current_flight_mode(); + + // apply desired roll, pitch and yaw to the plane + // ---------------------------------------------- + if (control_mode > MANUAL) + stabilize(); + + // write out the servo PWM values + // ------------------------------ + set_servos_4(); + +#if ENABLE_HIL + output_HIL(); +#endif + +#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL + readgcsinput(); +#endif +} + +void medium_loop() +{ + // This is the start of the medium (10 Hz) loop pieces + // ----------------------------------------- + switch(medium_loopCounter) { + + // This case deals with the GPS + //------------------------------- + case 0: + medium_loopCounter++; + update_GPS(); + + #if MAGNETOMETER == 1 + APM_Compass.Read(); // Read magnetometer + APM_Compass.Calculate(roll,pitch); // Calculate heading + #endif + + break; + + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; + + if(GPS.new_data){ + dTnav = millis() - medium_loopTimer; + medium_loopTimer = millis(); + } + + // calculate the plane's desired bearing + // ------------------------------------- + navigate(); + break; + + // unused? no, jeti gets updated :-) + //------------------------------ + case 2: + medium_loopCounter++; + + // perform next command + // -------------------- + update_commands(); + jeti_update(); + break; + + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; + + if ((log_bitmask & MASK_LOG_ATTITUDE_MED) && !(log_bitmask & MASK_LOG_ATTITUDE_FAST)) + Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (uint16_t)yaw_sensor); + + if (log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + + if (log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); + + if (log_bitmask & MASK_LOG_GPS) + Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats); + + send_message(MSG_ATTITUDE); // Sends attitude data + break; + + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter=0; + slow_loop(); + break; + } +} + +void slow_loop() +{ + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter){ + case 0: + slow_loopCounter++; + superslow_loopCounter++; + if(superslow_loopCounter >=15) { + // keep track of what page is in use in the log + // *** We need to come up with a better scheme to handle this... + eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage()); + superslow_loopCounter = 0; + } + break; + + case 1: + slow_loopCounter++; + + // Read 3-position switch on radio + // ------------------------------- + read_control_switch(); + + // Read Control Surfaces/Mix switches + // ---------------------------------- + if(reverse_switch){ + update_servo_switches(); + } + + // Read main battery voltage if hooked up - does not read the 5v from radio + // ------------------------------------------------------------------------ + #if BATTERY_EVENT == 1 + read_battery(); + #endif + + // Read Baro pressure + // ------------------ + read_airpressure(); + break; + + case 2: + slow_loopCounter = 0; + update_events(); + break; + } +} + + +void update_GPS(void) +{ + GPS.update(); + update_GPS_light(); + + if (GPS.new_data && GPS.fix) { + GPS_timer = millis(); + send_message(MSG_LOCATION); + + // for performance + // --------------- + gps_fix_count++; + + if(ground_start_count > 1){ + ground_start_count--; + ground_start_avg += GPS.ground_speed; + + } else if (ground_start_count == 1) { + // We countdown N number of good GPS fixes + // so that the altitude is more accurate + // ------------------------------------- + if (current_loc.lat == 0) { + SendDebugln("!! bad loc"); + ground_start_count = 5; + + } else { + if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT) { + startup_ground(); + + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + init_home(); + } else if (ENABLE_AIR_START == 0) { + init_home(); + } + + ground_start_count = 0; + } + } + + + current_loc.lng = GPS.longitude; // Lon * 10**7 + current_loc.lat = GPS.latitude; // Lat * 10**7 + +#if GPS_PROTOCOL == GPS_PROTOCOL_IMU + current_loc.alt = GPS.altitude; +#else + current_loc.alt = ((1 - altitude_mix) * GPS.altitude) + (altitude_mix * press_alt); // alt_MSL centimeters (meters * 100) +#endif + + COGX = cos(ToRad(GPS.ground_course/100.0)); + COGY = sin(ToRad(GPS.ground_course/100.0)); + } +} + +void update_current_flight_mode(void) +{ + if(control_mode == AUTO){ + crash_checker(); + + switch(command_must_ID){ + case CMD_TAKEOFF: + #if USE_MAGNETOMETER == ENABLED + calc_nav_roll(); + #else + nav_roll = 0; + #endif + float scaler; + #if AIRSPEED_SENSOR == 1 + scaler = (float)airspeed / (float)airspeed_cruise; + nav_pitch = scaler * takeoff_pitch * 50; + #else + scaler = (float)GPS.ground_speed / (float)airspeed_cruise; + nav_pitch = scaler * takeoff_pitch * 50; + #endif + + nav_pitch = constrain(nav_pitch, 0l, (long)takeoff_pitch); + + servo_out[CH_THROTTLE] = throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle + // ****************************** + + // override pitch_sensor to limit porpoising + // pitch_sensor = constrain(pitch_sensor, -6000, takeoff_pitch * 100); + // throttle = passthrough + break; + + case CMD_LAND: + calc_nav_roll(); + + #if AIRSPEED_SENSOR == 1 + calc_nav_pitch(); + calc_throttle(); + #else + calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle + calc_throttle(); // throttle basedtest landing_pitch; // pitch held constant + #endif + + if (land_complete) { + servo_out[CH_THROTTLE] = 0; + } + break; + + default: + hold_course = -1; + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; + } + }else{ + switch(control_mode){ + case RTL: + case LOITER: + hold_course = -1; + crash_checker(); + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; + + case FLY_BY_WIRE_A: + // fake Navigation output using sticks + nav_roll = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * head_max * reverse_roll) / 350; + nav_pitch = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 3500l * reverse_pitch) / 350; + + nav_roll = constrain(nav_roll, -head_max, head_max); + nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority + break; + + case FLY_BY_WIRE_B: + // fake Navigation output using sticks + // We use pitch_min because its magnitude is normally greater than pitch_max + nav_roll = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * head_max * reverse_roll) / 350; + altitude_error = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * pitch_min * -reverse_pitch) / 350; + nav_roll = constrain(nav_roll, -head_max, head_max); + + #if AIRSPEED_SENSOR == 1 + //airspeed_error = ((AIRSPEED_FBW_MAX - AIRSPEED_FBW_MIN) * servo_out[CH_THROTTLE] / 100) + AIRSPEED_FBW_MIN; + airspeed_error = ((int)(airspeed_fbw_max - airspeed_fbw_min) * servo_out[CH_THROTTLE]) + ((int)airspeed_fbw_min * 100); + // Intermediate calculation - airspeed_error is just desired airspeed at this point + airspeed_energy_error = (long)(((long)airspeed_error * (long)airspeed_error) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation + airspeed_error = (airspeed_error - airspeed); + + #endif + + calc_throttle(); + calc_nav_pitch(); + break; + + case STABILIZE: + nav_roll = 0; + nav_pitch = 0; + // throttle is passthrough + break; + + case CIRCLE: + // we have no GPS installed and have lost radio contact + // or we just want to fly around in a gentle circle w/o GPS + // ---------------------------------------------------- + nav_roll = head_max / 3; + nav_pitch = 0; + + if (failsafe == true){ + servo_out[CH_THROTTLE] = throttle_cruise; + } + break; + + case MANUAL: + // servo_out is for Sim control only + // --------------------------------- + servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; + servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; + servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; + break; + //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 + + } + } +} + +void read_AHRS(void) +{ + // Get gyro and accel data and perform IMU calculations + //----------------------------------------------------- + Read_adc_raw(); // Get current values for IMU sensors + Matrix_update(); // Integrate the DCM matrix + Normalize(); // Normalize the DCM matrix + Drift_correction(); // Perform drift correction + Euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation +} + + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde new file mode 100644 index 0000000000..97850fa696 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde @@ -0,0 +1,343 @@ +//**************************************************************** +// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. +//**************************************************************** + +void stabilize() +{ + float ch1_inf = 1.0; + float ch2_inf = 1.0; + float ch4_inf = 1.0; + + if(crash_timer > 0){ + nav_roll = 0; + } + + // For Testing Only + // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; + // Serial.print(" roll_sensor "); + // Serial.print(roll_sensor,DEC); + + // Calculate dersired servo output for the roll + // --------------------------------------------- + servo_out[CH_ROLL] = PID((nav_roll - roll_sensor), deltaMiliSeconds, CASE_SERVO_ROLL, 1.0); + servo_out[CH_PITCH] = PID((nav_pitch + abs(roll_sensor * kff[CASE_PITCH_COMP]) - pitch_sensor), deltaMiliSeconds, CASE_SERVO_PITCH, 1.0); + //Serial.print(" servo_out[CH_ROLL] "); + //Serial.print(servo_out[CH_ROLL],DEC); + + // Mix Stick input to allow users to override control surfaces + // ----------------------------------------------------------- + if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { + + ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim[CH_ROLL]; + ch1_inf = abs(ch1_inf); + ch1_inf = min(ch1_inf, 400.0); + ch1_inf = ((400.0 - ch1_inf) /400.0); + + ch2_inf = (float)radio_in[CH_PITCH] - radio_trim[CH_PITCH]; + ch2_inf = abs(ch2_inf); + ch2_inf = min(ch2_inf, 400.0); + ch2_inf = ((400.0 - ch2_inf) /400.0); + + // scale the sensor input based on the stick input + // ----------------------------------------------- + servo_out[CH_ROLL] *= ch1_inf; + servo_out[CH_PITCH] *= ch2_inf; + + // Mix in stick inputs + // ------------------- + servo_out[CH_ROLL] += reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; + servo_out[CH_PITCH] += reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; + + //Serial.print(" servo_out[CH_ROLL] "); + //Serial.println(servo_out[CH_ROLL],DEC); + } + + // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes + // important for steering on the ground during landing + // ----------------------------------------------- + if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { + ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim[CH_RUDDER]; + ch4_inf = abs(ch4_inf); + ch4_inf = min(ch4_inf, 400.0); + ch4_inf = ((400.0 - ch4_inf) /400.0); + } + + // Apply output to Rudder + // ---------------------- + calc_nav_yaw(); + servo_out[CH_RUDDER] *= ch4_inf; + servo_out[CH_RUDDER] += reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; + + // Call slew rate limiter if used + // ------------------------------ + //#if(ROLL_SLEW_LIMIT != 0) + // servo_out[CH_ROLL] = roll_slew_limit(servo_out[CH_ROLL]); + //#endif +} + +void crash_checker() +{ + if(pitch_sensor < -4500){ + crash_timer = 255; + } + if(crash_timer > 0) + crash_timer--; +} + + +#if AIRSPEED_SENSOR == 0 +void calc_throttle() +{ + // no airspeed sensor, we use nav pitch to determin the proper throttle output + // AUTO, RTL, etc + // --------------------------------------------------------------------------- + if (nav_pitch >= 0) { + servo_out[CH_THROTTLE] = throttle_cruise + (THROTTLE_MAX - throttle_cruise) * nav_pitch / pitch_max; + } else { + servo_out[CH_THROTTLE] = throttle_cruise - (throttle_cruise - THROTTLE_MIN) * nav_pitch / pitch_min; + } + servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0); + + // are we going too slow? up the throttle to get some more groundspeed + // move into PID loop in the future + // lower the contstant value to limit the effect : 50 = default + int gs_boost = 30 * (1.0 - ((float)GPS.ground_speed / (float)airspeed_cruise)); + gs_boost = max(0, gs_boost); + servo_out[CH_THROTTLE] += gs_boost; + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], THROTTLE_MIN, THROTTLE_MAX); +} +#endif + +#if AIRSPEED_SENSOR == 1 +void calc_throttle() +{ + // throttle control with airspeed compensation + // ------------------------------------------- + energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; + + // positive energy errors make the throttle go higher + servo_out[CH_THROTTLE] = throttle_cruise + PID(energy_error, dTnav, CASE_TE_THROTTLE, 1.0); + servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0); + + // are we going too slow? up the throttle to get some more groundspeed + // move into PID loop in the future + // lower the contstant value to limit the effect : 50 = default + + //JASON - We really should change this to act on airspeed in this case. Let's visit about it.... + /*int gs_boost = 30 * (1.0 - ((float)GPS.ground_speed / (float)airspeed_cruise)); + gs_boost = max(0,gs_boost); + servo_out[CH_THROTTLE] += gs_boost;*/ + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], THROTTLE_MIN, THROTTLE_MAX); +} +#endif + + + +/***************************************** + * Calculate desired roll angle (in medium freq loop) + *****************************************/ +// Yaw is separated into a function for future implementation of heading hold on take-off +// ---------------------------------------------------------------------------------------- +void calc_nav_yaw() +{ + // read_adc(4) is y axis accel; + // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) + servo_out[CH_RUDDER] = kff[CASE_RUDDER_MIX] * servo_out[CH_ROLL] + PID((long)read_adc(4), deltaMiliSeconds, CASE_SERVO_RUDDER, 1.0); + +} + + +#if AIRSPEED_SENSOR == 1 +void calc_nav_pitch() +{ + // Calculate the Pitch of the plane + // -------------------------------- + nav_pitch = -PID(airspeed_error, dTnav, CASE_NAV_PITCH_ASP, 1.0); + nav_pitch = constrain(nav_pitch, pitch_min, pitch_max); +} +#endif + +#if AIRSPEED_SENSOR == 0 +void calc_nav_pitch() +{ + // Calculate the Pitch of the plane + // -------------------------------- + nav_pitch = PID(altitude_error, dTnav, CASE_NAV_PITCH_ALT, 1.0); + nav_pitch = constrain(nav_pitch, pitch_min, pitch_max); +} +#endif + + +void calc_nav_roll() +{ + + // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. + // This does not make provisions for wind speed in excess of airframe speed + nav_gain_scaler = (float)GPS.ground_speed / (float)(AIRSPEED_CRUISE * 100); + nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); + + // Jason -> I will implement the servo gain scaler some time, but it is independant of this scaling. + // Doug to implement the high speed servo gain scale + // use head max to limit turns, make a var + + // negative error = left turn + // positive error = right turn + // Calculate the required roll of the plane + // ---------------------------------------- + nav_roll = PID(bearing_error, dTnav, CASE_NAV_ROLL, nav_gain_scaler); //returns desired bank angle in degrees*100 + nav_roll = constrain(nav_roll,-head_max, head_max); + +} + + +/***************************************** + * Roll servo slew limit + *****************************************/ +/* +float roll_slew_limit(float servo) +{ + static float last; + float temp = constrain(servo, last-ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f); + last = servo; + return temp; +}*/ + +/***************************************** + * Throttle slew limit + *****************************************/ +/*float throttle_slew_limit(float throttle) +{ + static float last; + float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f); + last = throttle; + return temp; +} +*/ + +/***************************************** + * Proportional Integrator Derivative Control + *****************************************/ + +float PID(long PID_error, long dt, int PID_case, float scaler) +{ + // PID_case is used to keep track of which PID controller is being used - e.g. PID_servo_out[CH_ROLL] + float output, derivative; + + derivative = 1000.0f * (float)(PID_error - last_error[PID_case]) / (float)dt; + last_error[PID_case] = PID_error; + output = (kp[PID_case] * scaler * (float)PID_error); // Compute proportional component + //Positive error produces positive output + + integrator[PID_case] += (float)PID_error * ki[PID_case] * scaler * (float)dt / 1000.0f; + integrator[PID_case] = constrain(integrator[PID_case], -1.0*(float)integrator_max[PID_case], (float)integrator_max[PID_case]); + + output += integrator[PID_case]; // Add the integral component + output += kd[PID_case] * scaler * derivative; // Add the derivative component + return output; +} + +// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. +// Keeps outdated data out of our calculations +void reset_I(void) +{ + integrator[CASE_NAV_ROLL] = 0; + integrator[CASE_NAV_PITCH_ASP] = 0; + integrator[CASE_NAV_PITCH_ALT] = 0; + integrator[CASE_TE_THROTTLE] = 0; + integrator[CASE_ALT_THROTTLE] = 0; + + last_error[CASE_NAV_ROLL] = 0; + last_error[CASE_NAV_PITCH_ASP] = 0; + last_error[CASE_NAV_PITCH_ALT] = 0; + last_error[CASE_TE_THROTTLE] = 0; + last_error[CASE_ALT_THROTTLE] = 0; +} + +/***************************************** +* Set the flight control servos based on the current calculated values +*****************************************/ +void set_servos_4(void) +{ + if(control_mode == MANUAL){ + // do a direct pass through of radio values + for(int y=0; y<4; y++) { + radio_out[y] = radio_in[y]; + } + + } else { + if (mix_mode == 0){ + radio_out[CH_ROLL] = radio_trim[CH_ROLL] + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111)); + radio_out[CH_PITCH] = radio_trim[CH_PITCH] + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111)); + radio_out[CH_RUDDER] = radio_trim[CH_RUDDER] + (reverse_rudder * ((float)servo_out[CH_RUDDER] * 0.11111)); + }else{ + /*Elevon mode*/ + float ch1; + float ch2; + ch1 = reverse_elevons * (servo_out[CH_PITCH] - servo_out[CH_ROLL]); + ch2 = servo_out[CH_PITCH] + servo_out[CH_ROLL]; + radio_out[CH_ROLL] = elevon1_trim + (reverse_ch1_elevon * (ch1 * 0.11111)); + radio_out[CH_PITCH] = elevon2_trim + (reverse_ch2_elevon * (ch2 * 0.11111)); + } + + #if THROTTLE_OUT == 0 + radio_out[CH_THROTTLE] = 0; + #endif + + + // convert 0 to 100% into PWM + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); + radio_out[CH_THROTTLE] = servo_out[CH_THROTTLE] * ((radio_max[CH_THROTTLE] - radio_min[CH_THROTTLE]) / 100); + radio_out[CH_THROTTLE] += radio_min[CH_THROTTLE]; + + //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 + + #if THROTTLE_REVERSE == 1 + radio_out[CH_THROTTLE] = radio_max[CH_THROTTLE] + radio_min[CH_THROTTLE] - radio_out[CH_THROTTLE]; + #endif + } + + // send values to the PWM timers for output + // ---------------------------------------- + for(int y=0; y<4; y++) { + //radio_out[y] = constrain(radio_out[y], radio_min[y], radio_max[y]); + APM_RC.OutputCh(y, radio_out[y]); // send to Servos + //Serial.print(radio_out[y],DEC); + //Serial.print(", "); + } + //Serial.println(" "); + } + +void demo_servos(byte i) { + + while(i > 0){ + send_message(SEVERITY_LOW,"Demo Servos!"); + APM_RC.OutputCh(1, 1400); + delay(400); + APM_RC.OutputCh(1, 1600); + delay(200); + APM_RC.OutputCh(1, 1500); + delay(400); + i--; + } +} + +int readOutputCh(unsigned char ch) +{ + int pwm; + switch(ch) + { + case 0: pwm = OCR5B; break; // ch0 + case 1: pwm = OCR5C; break; // ch1 + case 2: pwm = OCR1B; break; // ch2 + case 3: pwm = OCR1C; break; // ch3 + case 4: pwm = OCR4C; break; // ch4 + case 5: pwm = OCR4B; break; // ch5 + case 6: pwm = OCR3C; break; // ch6 + case 7: pwm = OCR3B; break; // ch7 + case 8: pwm = OCR5A; break; // ch8, PL3 + case 9: pwm = OCR1A; break; // ch9, PB5 + case 10: pwm = OCR3A; break; // ch10, PE3 + } + pwm >>= 1; // pwm / 2; + return pwm; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde new file mode 100644 index 0000000000..07e0bb8df9 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde @@ -0,0 +1,387 @@ + +// Read the 6 ADC channels needed for the IMU +// ----------------- +void Read_adc_raw(void) +{ + int tc_temp = APM_ADC.Ch(GYRO_TEMP_CH); + for (int i = 0; i < 6; i++) { + AN[i] = APM_ADC.Ch(sensors[i]); + if (i < 3) { + AN[i] -= gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias + } else { + AN[i] -= 2025; // Subtract typical accel bias + } + } +} + +// Returns the temperature compensated raw gyro value +//--------------------------------------------------- +float gyro_temp_comp(int i, int temp) +{ + // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 + //------------------------------------------------------------------------ + return GTC[i][0] + GTC[i][1] * temp + GTC[i][2] * temp * temp; +} + +// Returns an analog value with the offset removed +// ----------------- +float read_adc(int select) +{ + float temp; + if (SENSOR_SIGN[select] < 0) + temp = (AN_OFFSET[select]-AN[select]); + else + temp = (AN[select]-AN_OFFSET[select]); + + if (abs(temp) > ADC_CONSTRAINT) + adc_constraints++; // We keep track of the number of times we constrain the ADC output for performance reporting + +/* +// For checking the pitch/roll drift correction gain time constants +switch (select) { + case 3: + return 0; + break; + case 4: + return 0; + break; + case 5: + return 400; + break; +} +*/ + + +//End of drift correction gain test code + + return constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values +} + +/**************************************************/ +void Normalize(void) +{ + float error = 0; + float temporary[3][3]; + float renorm = 0; + boolean problem = FALSE; + + error= -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; // eq.19 + + Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); // eq.19 + Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); // eq.19 + + Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19 + Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 + + Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b // eq.20 + + renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]); + if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion + renorm = .5 * (3 - renorm); // eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm = 1. / sqrt(renorm); + renorm_sqrt_count++; + } else { + problem = TRUE; + renorm_blowup_count++; + } + Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); + + renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]); + if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion + renorm = .5 * (3 - renorm); // eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm = 1. / sqrt(renorm); + renorm_sqrt_count++; + } else { + problem = TRUE; + renorm_blowup_count++; + } + Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); + + renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]); + if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion + renorm = .5 * (3 - renorm); // eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm = 1. / sqrt(renorm); + renorm_sqrt_count++; + } else { + problem = TRUE; + renorm_blowup_count++; + } + Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); + + if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! + DCM_Matrix[0][0] = 1.0f; + DCM_Matrix[0][1] = 0.0f; + DCM_Matrix[0][2] = 0.0f; + DCM_Matrix[1][0] = 0.0f; + DCM_Matrix[1][1] = 1.0f; + DCM_Matrix[1][2] = 0.0f; + DCM_Matrix[2][0] = 0.0f; + DCM_Matrix[2][1] = 0.0f; + DCM_Matrix[2][2] = 1.0f; + problem = FALSE; + } +} + +/**************************************************/ +void Drift_correction(void) +{ + //Compensation the Roll, Pitch and Yaw drift. + float mag_heading_x; + float mag_heading_y; + float errorCourse = 0; + static float Scaled_Omega_P[3]; + static float Scaled_Omega_I[3]; + float Accel_magnitude; + float Accel_weight; + float Integrator_magnitude; + + //*****Roll and Pitch*************** + + // Calculate the magnitude of the accelerometer vector + Accel_magnitude = sqrt(Accel_Vector[0] * Accel_Vector[0] + Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]); + Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. + + // Dynamic weighting of accelerometer info (reliability filter) + // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) + Accel_weight = constrain(1 - 2 * abs(1 - Accel_magnitude), 0, 1); // + + // We monitor the amount that the accelerometer based drift correction is deweighted for performanc reporting + imu_health = imu_health + 0.02 * (Accel_weight-.5); + imu_health = constrain(imu_health, 0, 1); + + Vector_Cross_Product(&errorRollPitch[0], &Accel_Vector[0], &DCM_Matrix[2][0]); // adjust the ground of reference + // errorRollPitch are in Accel ADC units + // Limit max errorRollPitch to limit max Omega_P and Omega_I + errorRollPitch[0] = constrain(errorRollPitch[0], -50, 50); + errorRollPitch[1] = constrain(errorRollPitch[1], -50, 50); + errorRollPitch[2] = constrain(errorRollPitch[2], -50, 50); + + Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight); + + Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight); + Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); + + //*****YAW*************** + + #if MAGNETOMETER == ENABLED + // We make the gyro YAW drift correction based on compass magnetic heading + errorCourse= (DCM_Matrix[0][0] * APM_Compass.Heading_Y) - (DCM_Matrix[1][0] * APM_Compass.Heading_X); // Calculating YAW error + #else // Use GPS Ground course to correct yaw gyro drift= + if(GPS.ground_speed >= SPEEDFILT){ + // Optimization: We have precalculated COGX and COGY (Course over Ground X and Y) from GPS info + errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); // Calculating YAW error + } + #endif + + Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); // Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + + Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW); + Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional. + + Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW); + Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I + + + // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros + Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I)); + if (Integrator_magnitude > ToRad(300)) { + Vector_Scale(Omega_I, Omega_I, 0.5f * ToRad(300) / Integrator_magnitude); + } +} + +/**************************************************/ +void Accel_adjust(void) +{ + Accel_Vector[1] += Accel_Scale((GPS.ground_speed / 100) * Omega[2]); // Centrifugal force on Acc_y = GPS_speed * GyroZ + Accel_Vector[2] -= Accel_Scale((GPS.ground_speed / 100) * Omega[1]); // Centrifugal force on Acc_z = GPS_speed * GyroY +} + + +/**************************************************/ +void Matrix_update(void) +{ + Gyro_Vector[0] = Gyro_Scaled_X(read_adc(0)); // gyro x roll + Gyro_Vector[1] = Gyro_Scaled_Y(read_adc(1)); // gyro y pitch + Gyro_Vector[2] = Gyro_Scaled_Z(read_adc(2)); // gyro Z yaw + + //Record when you saturate any of the gyros. + if((abs(Gyro_Vector[0]) >= ToRad(300)) || (abs(Gyro_Vector[1]) >= ToRad(300)) || (abs(Gyro_Vector[2]) >= ToRad(300))) + gyro_sat_count++; + +/* +Serial.print (AN[0]); +Serial.print (" "); +Serial.print (AN_OFFSET[0]); +Serial.print (" "); +Serial.print (Gyro_Vector[0]); +Serial.print (" "); +Serial.print (AN[1]); +Serial.print (" "); +Serial.print (AN_OFFSET[1]); +Serial.print (" "); +Serial.print (Gyro_Vector[1]); +Serial.print (" "); +Serial.print (AN[2]); +Serial.print (" "); +Serial.print (AN_OFFSET[2]); +Serial.print (" "); +Serial.println (Gyro_Vector[2]); +*/ + +// Accel_Vector[0]=read_adc(3); // acc x +// Accel_Vector[1]=read_adc(4); // acc y +// Accel_Vector[2]=read_adc(5); // acc z + // Low pass filter on accelerometer data (to filter vibrations) + Accel_Vector[0] = Accel_Vector[0] * 0.6 + (float)read_adc(3) * 0.4; // acc x + Accel_Vector[1] = Accel_Vector[1] * 0.6 + (float)read_adc(4) * 0.4; // acc y + Accel_Vector[2] = Accel_Vector[2] * 0.6 + (float)read_adc(5) * 0.4; // acc z + + + Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); // adding proportional term + Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); // adding Integrator term + + Accel_adjust(); // Remove centrifugal acceleration. + + #if OUTPUTMODE == 1 + Update_Matrix[0][0] = 0; + Update_Matrix[0][1] = -G_Dt * Omega_Vector[2]; // -z + Update_Matrix[0][2] = G_Dt * Omega_Vector[1]; // y + Update_Matrix[1][0] = G_Dt * Omega_Vector[2]; // z + Update_Matrix[1][1] = 0; + Update_Matrix[1][2] = -G_Dt * Omega_Vector[0]; // -x + Update_Matrix[2][0] = -G_Dt * Omega_Vector[1]; // -y + Update_Matrix[2][1] = G_Dt * Omega_Vector[0]; // x + Update_Matrix[2][2] = 0; + #else // Uncorrected data (no drift correction) + Update_Matrix[0][0] = 0; + Update_Matrix[0][1] = -G_Dt * Gyro_Vector[2]; // -z + Update_Matrix[0][2] = G_Dt * Gyro_Vector[1]; // y + Update_Matrix[1][0] = G_Dt * Gyro_Vector[2]; // z + Update_Matrix[1][1] = 0; + Update_Matrix[1][2] = -G_Dt * Gyro_Vector[0]; + Update_Matrix[2][0] = -G_Dt * Gyro_Vector[1]; + Update_Matrix[2][1] = G_Dt * Gyro_Vector[0]; + Update_Matrix[2][2] = 0; + #endif + + Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); // a * b = c + + for(int x = 0; x < 3; x++){ // Matrix Addition (update) + for(int y = 0; y < 3; y++){ + DCM_Matrix[x][y] += Temporary_Matrix[x][y]; + } + } + +/* +Serial.print (G_Dt * 1000); +Serial.print (" "); +Serial.print (DCM_Matrix[0][0]); +Serial.print (" "); +Serial.print (DCM_Matrix[0][1]); +Serial.print (" "); +Serial.print (DCM_Matrix[0][2]); +Serial.print (" "); +Serial.print (DCM_Matrix[1][0]); +Serial.print (" "); +Serial.print (DCM_Matrix[1][1]); +Serial.print (" "); +Serial.print (DCM_Matrix[1][2]); +Serial.print (" "); +Serial.print (DCM_Matrix[2][0]); +Serial.print (" "); +Serial.print (DCM_Matrix[2][1]); +Serial.print (" "); +Serial.println (DCM_Matrix[2][2]); +*/ +} + +/**************************************************/ +void Euler_angles(void) +{ + #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) + roll = atan2(Accel_Vector[1], Accel_Vector[2]); // atan2(acc_y, acc_z) + pitch = -asin((Accel_Vector[0]) / (double)GRAVITY); // asin(acc_x) + yaw = 0; + roll_sensor = ToDeg(roll) * 100; + pitch_sensor = ToDeg(pitch) * 100; + #else + pitch = -asin(DCM_Matrix[2][0]); + roll = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]); + yaw = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); + pitch_sensor = ToDeg(pitch) * 100; + roll_sensor = ToDeg(roll) * 100; + yaw_sensor = ToDeg(yaw) * 100; + if (yaw_sensor < 0) yaw_sensor += 36000; + #endif + + /* + Serial.print ("Roll "); + Serial.print (roll_sensor / 100); + Serial.print (", Pitch "); + Serial.print (pitch_sensor / 100); + Serial.print (", Yaw "); + Serial.println (yaw_sensor / 100); + */ +} + +/**************************************************/ +//Computes the dot product of two vectors +float Vector_Dot_Product(float vector1[3], float vector2[3]) +{ + float op = 0; + + for(int c = 0; c < 3; c++) + { + op += vector1[c] * vector2[c]; + } + + return op; +} + +/**************************************************/ +//Computes the cross product of two vectors +void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) +{ + vectorOut[0]= (v1[1] * v2[2]) - (v1[2] * v2[1]); + vectorOut[1]= (v1[2] * v2[0]) - (v1[0] * v2[2]); + vectorOut[2]= (v1[0] * v2[1]) - (v1[1] * v2[0]); +} + +/**************************************************/ +//Multiply the vector by a scalar. +void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) +{ + for(int c = 0; c < 3; c++) + { + vectorOut[c] = vectorIn[c] * scale2; + } +} + +/**************************************************/ +void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) +{ + for(int c = 0; c < 3; c++) + { + vectorOut[c] = vectorIn1[c]+vectorIn2[c]; + } +} + +/********* MATRIX FUNCTIONS *****************************************/ +//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). +void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3]) +{ + float op[3]; + for(int x = 0; x < 3; x++){ + for(int y = 0; y < 3; y++){ + for(int w = 0; w < 3; w++){ + op[w] = a[x][w] * b[w][y]; + } + mat[x][y] = 0; + mat[x][y] = op[0] + op[1] + op[2]; + float test = mat[x][y]; + } + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt new file mode 100644 index 0000000000..b7586d293d --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt @@ -0,0 +1,389 @@ +0 0000 16 int radio_trim 1 +1 0001 .. +2 0002 16 int radio_trim 2 +3 0003 .. +4 0004 16 int radio_trim 3 +5 0005 .. +6 0006 16 int radio_trim 4 +7 0007 .. +8 0008 16 int radio_trim 5 +9 0009 .. +10 000A 16 int radio_trim 6 +11 000B .. +12 000C 16 int radio_trim 7 +13 000D .. +14 000E 16 int radio_trim 8 +15 000F .. +16 0010 16 int radio_min 1 +17 0011 .. +18 0012 16 int radio_min 2 +19 0013 .. +20 0014 16 int radio_min 3 +21 0015 .. +22 0016 16 int radio_min 4 +23 0017 .. +24 0018 16 int radio_min 5 +25 0019 .. +26 001A 16 int radio_min 6 +27 001B .. +28 001C 16 int radio_min 7 +29 001D .. +30 001E 16 int radio_min 8 +31 001F .. +32 0020 16 int radio_max 1 +33 0021 .. +34 0022 16 int radio_max 2 +35 0023 .. +36 0024 16 int radio_max 3 +37 0025 .. +38 0026 16 int radio_max 4 +39 0027 .. +40 0028 16 int radio_max 5 +41 0029 .. +42 002A 16 int radio_max 6 +43 002B .. +44 002C 16 int radio_max 7 +45 002D .. +46 002E 16 int radio_max 8 +47 002F .. +48 0030 16 int elevon_trim 1 +49 0031 .. +50 0032 16 int elevon_trim 2 +51 0033 .. +52 0034 16 int XTRACK_GAIN +53 0035 .. +54 0036 16 int XTRACK_ENTRY_ANGLE +55 0037 .. +56 0038 8 int HEAD_MAX +57 0039 8 int PITCH_MAX +58 003A 8 int Pitch_min +59 003B 32 float EE_ALT_MIX +60 003C .. +61 003D .. +62 003E .. +63 003F +64 0040 32 float Kp 0 +65 0041 .. +66 0042 .. +67 0043 .. +68 0044 32 float Kp 1 +69 0045 .. +70 0046 .. +71 0047 .. +72 0048 32 float Kp 2 +73 0049 .. +74 004A .. +75 004B .. +76 004C 32 float Kp 3 +77 004D .. +78 004E .. +79 004F .. +80 0050 32 float Kp 4 +81 0051 .. +82 0052 .. +83 0053 .. +84 0054 32 float Kp 5 +85 0055 .. +86 0056 .. +87 0057 .. +88 0058 32 float Kp 6 +89 0059 .. +90 005A .. +91 005B .. +92 005C 32 float Kp 7 +93 005D .. +94 005E .. +95 005F .. +96 0060 32 float Ki 0 +97 0061 .. +98 0062 .. +99 0063 .. +100 0064 32 float Ki 1 +101 0065 .. +102 0066 .. +103 0067 .. +104 0068 32 float Ki 2 +105 0069 .. +106 006A .. +107 006B .. +108 006C 32 float Ki 3 +109 006D .. +110 006E .. +111 006F .. +112 0070 32 float Ki 4 +113 0071 .. +114 0072 .. +115 0073 .. +116 0074 32 float Ki 5 +117 0075 .. +118 0076 .. +119 0077 .. +120 0078 32 float Ki 6 +121 0079 .. +122 007A .. +123 007B .. +124 007C 32 float Ki 7 +125 007D .. +126 007E .. +127 007F .. +128 0080 32 float kd 0 +129 0081 .. +130 0082 .. +131 0083 .. +132 0084 32 float kd 1 +133 0085 .. +134 0086 .. +135 0087 .. +136 0088 32 float kd 2 +137 0089 .. +138 008A .. +139 008B .. +140 008C 32 float kd 3 +141 008D .. +142 008E .. +143 008F .. +144 0090 32 float kd 4 +145 0091 .. +146 0092 .. +147 0093 .. +148 0094 32 float kd 5 +149 0095 .. +150 0096 .. +151 0097 .. +152 0098 32 float kd 6 +153 0099 .. +154 009A .. +155 009B .. +156 009C 32 float kd 7 +157 009D .. +158 009E .. +159 009F .. +160 00A0 32 float integrator_max 0 +161 00A1 .. +162 00A2 .. +163 00A3 .. +164 00A4 32 float integrator_max 1 +165 00A5 .. +166 00A6 .. +167 00A7 .. +168 00A8 32 float integrator_max 2 +169 00A9 .. +170 00AA .. +171 00AB .. +172 00AC 32 float integrator_max 3 +173 00AD .. +174 00AE .. +175 00AF .. +176 00B0 32 float integrator_max 4 +177 00B1 .. +178 00B2 .. +179 00B3 .. +180 00B4 32 float integrator_max 5 +181 00B5 .. +182 00B6 .. +183 00B7 .. +184 00B8 32 float integrator_max 6 +185 00B9 .. +186 00BA .. +187 00BB .. +188 00BC 32 float integrator_max 7 +189 00BD .. +190 00BE .. +191 00BF .. +192 00C0 32 float Kff 0 +193 00C1 .. +194 00C2 .. +195 00C3 .. +196 00C4 32 float Kff 1 +197 00C5 .. +198 00C6 .. +199 00C7 .. +200 00C8 32 float Kff 2 +201 00C9 .. +202 00CA .. +203 00CB .. +204 00CC 32 float Kff 3 +205 00CD .. +206 00CE .. +207 00CF .. +208 00D0 32 float Kff 4 +209 00D1 .. +210 00D2 .. +211 00D3 .. +212 00D4 32 float Kff 5 +213 00D5 .. +214 00D6 .. +215 00D7 .. +216 00D8 32 float Kff 6 +217 00D9 .. +218 00DA .. +219 00DB .. +220 00DC 32 float Kff 7 +221 00DD .. +222 00DE .. +223 00DF .. +224 00E0 32 int EE_ACCEL_OFFSET 0 +225 00E1 .. +226 00E2 .. +227 00E3 .. +228 00E4 32 int EE_ACCEL_OFFSET 1 +229 00E5 .. +230 00E6 .. +231 00E7 .. +232 00E8 32 int EE_ACCEL_OFFSET 2 +233 00E9 .. +234 00EA .. +235 00EB .. +236 00EC 32 int EE_ACCEL_OFFSET 3 +237 00ED .. +238 00EE .. +239 00EF .. +240 00F0 32 int EE_ACCEL_OFFSET 4 +241 00F1 .. +242 00F2 .. +243 00F3 .. +244 00F4 32 int EE_ACCEL_OFFSET 5 +245 00F5 .. +246 00F6 .. +247 00F7 .. +248 00F8 8 EE_CONFIG +249 00F9 8 EE_WP_MODE +250 00FA 8 EE_YAW_MODE +251 00FB 8 EE_WP_TOTAL 0x106 +252 00FC 8 EE_WP_INDEX 0x107 +253 00FD 8 EE_WP_RADIUS 0x108 +254 00FE 8 EE_LOITER_RADIUS 0x109 +255 00FF 32 EE_ALT_HOLD_HOME 0x10A +256 0100 .. +257 0101 .. +258 0102 .. +259 0103 8 AIRSPEED_CRUISE +260 0104 32 AIRSPEED_RATIO +261 0105 .. +262 0106 .. +263 0107 .. +264 0108 AIRSPEED_FBW_MIN +265 0109 AIRSPEED_FBW_MAX +266 010A 8 THROTTLE_MIN +267 010B 8 THROTTLE_CRUISE +268 010C 8 THROTTLE_MAX +269 010D 8 THROTTLE_FAILSAFE +270 010E 16 THROTTLE_FS_VALUE +271 010F .. +272 0110 8 THROTTLE_FAILSAFE_ACTION +273 0111 +274 0112 8 FLIGHT_MODE_CHANNEL +275 0113 8 AUTO_TRIM +276 0114 uint16 LOGging Bitmask +277 0115 .. +278 0116 uint32 EE_ABS_PRESS_GND +279 0117 .. +280 0118 .. +281 0119 .. +282 011A 16 int EE_GND_TEMP +283 011B .. +284 011C 16 int EE_GND_ALT +285 011D .. +286 011E 16 int EE_AP_OFFSET +287 011F .. +288 0120 8 byte EE_SWITCH_ENABLE +289 0121 8 ARRAY FLIGHT_MODES_1 +290 0122 8 ARRAY FLIGHT_MODES_2 +291 0123 8 ARRAY FLIGHT_MODES_3 +292 0124 8 ARRAY FLIGHT_MODES_4 +293 0125 8 ARRAY FLIGHT_MODES_5 +294 0126 8 ARRAY FLIGHT_MODES_6 +295 0127 +296 0128 +297 0129 +298 012A +299 012B +300 012C +301 012D +302 012E +303 012F (0x130 is the Start Byte for commands) +304 0130 byte Command id (Home) 0 +305 0131 byte Parameter 1 +306 0132 long Altitude, Parameter 2 +307 0133 .. +308 0134 .. +309 0135 .. +310 0136 long Latitude, Parameter 3 +311 0137 .. +312 0138 .. +313 0139 .. +314 013A long Longitude, Parameter 3 +315 013B .. +316 013C .. +317 013D byte Command id (WP 1) 1 +318 013E byte Parameter 1 +319 013F long Altitude, Parameter 2 +320 0140 .. +321 0141 .. +322 0142 .. +323 0143 long Latitude, Parameter 3 +324 0144 .. +325 0145 .. +326 0146 .. +327 0147 long Longitude, Parameter 3 +328 0148 .. +329 0149 .. +330 014A .. +331 014B +332 014C +333 014D +334 014E +335 014F + + + + + +********************************************************************************************** +SECTION 6 - LOGS + +3584 0xE00 int Last log page used +3585 0xE01 .. +3587 0xE02 byte last log number +3588 0xE03 unused +3589 0xE04 int Log 1 start page +3590 0xE05 .. +3591 0xE06 int Log 2 start page +3592 0xE07 .. +3593 0xE08 int Log 3 start page +3594 0xE09 .. +3595 0xE0A int Log 4 start page +3596 0xE0B .. +3597 0xE0C int Log 5 start page +3598 0xE0D .. +3599 0xE0E int Log 6 start page +3600 0xE0F .. +3601 0xE10 int Log 7 start page +3602 0xE11 .. +3603 0xE12 int Log 8 start page +3604 0xE13 .. +3605 0xE14 int Log 9 start page +3606 0xE15 .. +3607 0xE16 int Log 10 start page +3608 0xE17 .. +3609 0xE18 int Log 11 start page +3610 0xE19 .. +3611 0xE1A int Log 12 start page +3612 0xE1B .. +3613 0xE1C int Log 13 start page +3614 0xE1D .. +3615 0xE1E int Log 14 start page +3616 0xE1F .. +3617 0xE20 int Log 15 start page +3618 0xE21 .. +3619 0xE22 int Log 16 start page +3620 0xE23 .. +3621 0xE24 int Log 17 start page +3622 0xE25 .. +3623 0xE26 int Log 18 start page +3624 0xE27 .. +3625 0xE28 int Log 19 start page +3626 0xE29 .. + + + \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde new file mode 100644 index 0000000000..9cf861f3bc --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde @@ -0,0 +1,286 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// ************************************************************************************ +// This function gets critical data from EEPROM to get us underway if restarting in air +// ************************************************************************************ +void read_EEPROM_startup(void) +{ + read_EEPROM_gains(); + read_EEPROM_radio_minmax(); // read Radio limits + read_EEPROM_trims(); // read Radio trims + read_user_configs(); + read_EEPROM_waypoint_info(); +} + +void read_EEPROM_airstart_critical(void) +{ + int16_t temp = 0; + read_EEPROM_IMU_offsets(); + + // For debugging only + /* + Serial.print ("Offsets "); Serial.print (AN_OFFSET[0]); + Serial.print (" "); Serial.print (AN_OFFSET[1]); + Serial.print (" "); Serial.print (AN_OFFSET[2]); + Serial.print (" "); Serial.print (AN_OFFSET[3]); + Serial.print (" "); Serial.print (AN_OFFSET[4]); + Serial.print (" "); Serial.println (AN_OFFSET[5]); + */ + + // Read the home location + //----------------------- + home = get_wp_with_index(0); + + // Read pressure sensor values + //---------------------------- + read_pressure_data(); +} + +void save_EEPROM_groundstart(void) +{ + save_EEPROM_trims(); + save_EEPROM_IMU_offsets(); + // pressure sensor data saved by init_home +} + + +/********************************************************************************/ +long read_alt_to_hold() +{ + byte options = eeprom_read_byte((byte *) EE_CONFIG); + + // save the alitude above home option + if(options & HOLD_ALT_ABOVE_HOME){ + int32_t temp = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME); + return temp + home.alt; + }else{ + return current_loc.alt; + } +} + +long save_alt_to_hold(int32_t alt_to_hold) +{ + byte options = eeprom_read_byte((byte *) EE_CONFIG); + + // save the alitude above home option + if(options & HOLD_ALT_ABOVE_HOME) + eeprom_write_block((const void *)&alt_to_hold, (void *)EE_ALT_HOLD_HOME, sizeof (alt_to_hold)); +} + + +/********************************************************************************/ +void read_EEPROM_waypoint_info(void) +{ + wp_total = eeprom_read_byte((uint8_t *) EE_WP_TOTAL); + wp_radius = eeprom_read_byte((uint8_t *) EE_WP_RADIUS); + loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS); +} + +void save_EEPROM_waypoint_info(void) +{ + eeprom_write_byte((uint8_t *) EE_WP_TOTAL, wp_total); + eeprom_write_byte((uint8_t *) EE_WP_RADIUS, wp_radius); + eeprom_write_byte((uint8_t *) EE_LOITER_RADIUS, loiter_radius); +} + +/********************************************************************************/ +void read_EEPROM_gains(void) +{ + eeprom_read_block((void*)&kp, (const void*)EE_KP, sizeof(kp)); + eeprom_read_block((void*)&ki, (const void*)EE_KI, sizeof(ki)); + eeprom_read_block((void*)&kd, (const void*)EE_KD, sizeof(kd)); + eeprom_read_block((void*)&integrator_max, (const void*)EE_IMAX, sizeof(integrator_max)); + eeprom_read_block((void*)&kff, (const void*)EE_KFF, sizeof(kff)); + + // stored as degree * 100 + x_track_gain = eeprom_read_word((uint16_t *) EE_XTRACK_GAIN); + + // stored as degrees + x_track_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100; + + // stored as degrees + head_max = eeprom_read_byte((uint8_t *) EE_HEAD_MAX) * 100; // scale to degress * 100 + pitch_max = eeprom_read_byte((uint8_t *) EE_PITCH_MAX) * 100; // scale to degress * 100 + pitch_min = -eeprom_read_byte((uint8_t *) EE_PITCH_MIN) * 100; // scale to degress * 100 + + // stored as a float + eeprom_read_block((void*)&altitude_mix, (const void*)EE_ALT_MIX, sizeof(altitude_mix)); +} + +void save_EEPROM_gains(void) +{ + eeprom_write_block((const void *)&kp, (void *)EE_KP, sizeof (kp)); + eeprom_write_block((const void *)&ki, (void *)EE_KI, sizeof (ki)); + eeprom_write_block((const void *)&kd, (void *)EE_KD, sizeof (kd)); + eeprom_write_block((const void *)&integrator_max, (void *)EE_IMAX, sizeof (integrator_max)); + eeprom_write_block((const void *)&kff, (void *)EE_KFF, sizeof (kff)); + + // stored as degree * 100 + eeprom_write_word((uint16_t *) EE_XTRACK_GAIN, x_track_gain); + + // stored as degrees + eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, x_track_angle / 100); + + // stored as degrees + eeprom_write_byte((uint8_t *) EE_HEAD_MAX, head_max / 100); + eeprom_write_byte((uint8_t *) EE_PITCH_MAX, pitch_max / 100); + eeprom_write_byte((uint8_t *) EE_PITCH_MIN, -pitch_min / 100); + + // stored as a float + eeprom_write_block((const void*)&altitude_mix, (void*)EE_ALT_MIX, sizeof(altitude_mix)); +} + + +/********************************************************************************/ + +void read_EEPROM_trims(void) +{ + // Read Radio trim settings + eeprom_read_block((void*)&radio_trim, (const void*)EE_TRIM, sizeof(radio_trim)); + elevon1_trim = eeprom_read_word((uint16_t *) EE_ELEVON1_TRIM); + elevon2_trim = eeprom_read_word((uint16_t *) EE_ELEVON2_TRIM); +} + +void save_EEPROM_trims(void) +{ + // Save Radio trim settings + eeprom_write_block((const void *)&radio_trim, (void *)EE_TRIM, sizeof(radio_trim)); + eeprom_write_word((uint16_t *) EE_ELEVON1_TRIM, elevon1_trim); + eeprom_write_word((uint16_t *) EE_ELEVON2_TRIM, elevon2_trim); +} + +/********************************************************************************/ + +void save_EEPROM_IMU_offsets(void) +{ + eeprom_write_block((const void *)&AN_OFFSET, (void *)EE_AN_OFFSET, sizeof (AN_OFFSET)); +} + +void read_EEPROM_IMU_offsets(void) +{ + eeprom_read_block((void*)&AN_OFFSET, (const void*)EE_AN_OFFSET, sizeof(AN_OFFSET)); +} + +/********************************************************************************/ + +void save_command_index(void) +{ + eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index); +} + +void read_command_index(void) +{ + wp_index = command_must_index = eeprom_read_byte((uint8_t *) EE_WP_INDEX); +} + +/********************************************************************************/ + +void save_pressure_data(void) +{ + eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_press_gnd); + eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature); + eeprom_write_word((uint16_t *)EE_GND_ALT, (ground_alt / 100)); + + #if AIRSPEED_SENSOR == 1 + eeprom_write_word((uint16_t *)EE_AP_OFFSET, airpressure_offset); + #endif +} + +void read_pressure_data(void) +{ + abs_press_gnd = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND); + abs_press_filt = abs_press_gnd; // Better than zero for an air start value + ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP); + ground_alt = eeprom_read_word((uint16_t *) EE_GND_ALT) * 100; + + #if AIRSPEED_SENSOR == 1 + airpressure_offset = eeprom_read_word((uint16_t *) EE_AP_OFFSET); + #endif +} + + + +/********************************************************************************/ + +void read_EEPROM_radio_minmax(void) +{ + // Read Radio min/max settings + eeprom_read_block((void*)&radio_max, (const void*)EE_MAX, sizeof(radio_max)); + eeprom_read_block((void*)&radio_min, (const void*)EE_MIN, sizeof(radio_min)); +} + + +void save_EEPROM_radio_minmax(void) +{ + // Save Radio min/max settings + eeprom_write_block((const void *)&radio_max, (void *)EE_MAX, sizeof(radio_max)); + eeprom_write_block((const void *)&radio_min, (void *)EE_MIN, sizeof(radio_min)); +} + + + +/********************************************************************************/ + +void read_user_configs(void) +{ + // Read Radio min/max settings + airspeed_cruise = eeprom_read_byte((byte *) EE_AIRSPEED_CRUISE)*100; + eeprom_read_block((void*)&airspeed_ratio, (const void*)EE_AIRSPEED_RATIO, sizeof(airspeed_ratio)); + + throttle_min = eeprom_read_byte((byte *) EE_THROTTLE_MIN); + throttle_max = eeprom_read_byte((byte *) EE_THROTTLE_MAX); + throttle_cruise = eeprom_read_byte((byte *) EE_THROTTLE_CRUISE); + + airspeed_fbw_min = eeprom_read_byte((byte *) EE_AIRSPEED_FBW_MIN); + airspeed_fbw_max = eeprom_read_byte((byte *) EE_AIRSPEED_FBW_MAX); + + throttle_failsafe_enabled = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE); + throttle_failsafe_action = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION); + throttle_failsafe_value = eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE); + + //flight_mode_channel = eeprom_read_byte((byte *) EE_FLIGHT_MODE_CHANNEL); + auto_trim = eeprom_read_byte((byte *) EE_AUTO_TRIM); + log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK); + + read_EEPROM_flight_modes(); + reverse_switch = eeprom_read_byte((byte *) EE_REVERSE_SWITCH); +} + + +void save_user_configs(void) +{ + eeprom_write_byte((byte *) EE_AIRSPEED_CRUISE, airspeed_cruise/100); + eeprom_write_block((const void *)&airspeed_ratio, (void *)EE_AIRSPEED_RATIO, sizeof(airspeed_ratio)); + + eeprom_write_byte((byte *) EE_THROTTLE_MIN, throttle_min); + eeprom_write_byte((byte *) EE_THROTTLE_MAX, throttle_max); + eeprom_write_byte((byte *) EE_THROTTLE_CRUISE, throttle_cruise); + + eeprom_write_byte((byte *) EE_AIRSPEED_FBW_MIN, airspeed_fbw_min); + eeprom_write_byte((byte *) EE_AIRSPEED_FBW_MAX, airspeed_fbw_max); + + eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE, throttle_failsafe_enabled); + eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action); + eeprom_write_word((uint16_t *) EE_THROTTLE_FS_VALUE, throttle_failsafe_value); + + //eeprom_write_byte((byte *) EE_FLIGHT_MODE_CHANNEL, flight_mode_channel); + eeprom_write_byte((byte *) EE_AUTO_TRIM, auto_trim); + eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask); + + //save_EEPROM_flight_modes(); + eeprom_write_byte((byte *) EE_REVERSE_SWITCH, reverse_switch); +} + +/********************************************************************************/ +void read_EEPROM_flight_modes(void) +{ + // Read Radio min/max settings + eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes)); +} + + +void save_EEPROM_flight_modes(void) +{ + // Save Radio min/max settings + eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes)); +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde new file mode 100644 index 0000000000..4cc892f32c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde @@ -0,0 +1,176 @@ + +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY + +#if GCS_PORT == 3 +# define SendSer Serial3.print +# define SendSerln Serial3.println +#else +# define SendSer Serial.print +# define SendSerln Serial.println +#endif + +// This is the standard GCS output file for ArduPilot + +/* +Message Prefixes +!!! Position Low rate telemetry ++++ Attitude High rate telemetry +### Mode Change in control mode +%%% Waypoint Current and previous waypoints +XXX Alert Text alert - NOTE: Alert message generation is not localized to a function +PPP IMU Performance Sent every 20 seconds for performance monitoring + +Message Suffix +*** All messages use this suffix +*/ + +/* +void acknowledge(byte id, byte check1, byte check2) {} +void send_message(byte id) {} +void send_message(byte id, long param) {} +void send_message(byte severity, const char *str) {} +*/ + +void acknowledge(byte id, byte check1, byte check2) +{ +} + +void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 +{ + if(severity >= DEBUG_LEVEL){ + SendSer("MSG: "); + SendSerln(str); + } +} + +void send_message(byte id) +{ + send_message(id,0l); +} + +void send_message(byte id, long param) +{ + switch(id) { + case MSG_ATTITUDE: // ** Attitude message + print_attitude(); + break; + case MSG_LOCATION: // ** Location/GPS message + print_position(); + break; + case MSG_HEARTBEAT: // ** Location/GPS message + print_control_mode(); + break; + //case MSG_PERF_REPORT: + // printPerfData(); + } +} + +void print_current_waypoints() +{ +} + +void print_attitude(void) +{ + SendSer("+++"); + SendSer("ASP:"); + SendSer((int)airspeed / 100, DEC); + SendSer(",THH:"); + SendSer(servo_out[CH_THROTTLE], DEC); + SendSer (",RLL:"); + SendSer(roll_sensor / 100, DEC); + SendSer (",PCH:"); + SendSer(pitch_sensor / 100, DEC); + SendSerln(",***"); +} + +void print_control_mode(void) +{ + switch (control_mode){ + case MANUAL: + SendSerln("###MANUAL***"); + break; + case STABILIZE: + SendSerln("###STABILIZE***"); + break; + case CIRCLE: + SendSerln("###CIRCLE***"); + break; + case FLY_BY_WIRE_A: + SendSerln("###FBW A***"); + break; + case FLY_BY_WIRE_B: + SendSerln("###FBW B***"); + break; + case AUTO: + SendSerln("###AUTO***"); + break; + case RTL: + SendSerln("###RTL***"); + break; + case LOITER: + SendSerln("###LOITER***"); + break; + case TAKEOFF: + SendSerln("##TAKEOFF***"); + break; + case LAND: + SendSerln("##LAND***"); + break; + } +} + +void print_position(void) +{ + SendSer("!!!"); + SendSer("LAT:"); + SendSer(current_loc.lat/10,DEC); + SendSer(",LON:"); + SendSer(current_loc.lng/10,DEC); //wp_current_lat + SendSer(",SPD:"); + SendSer(GPS.ground_speed/100,DEC); + SendSer(",CRT:"); + SendSer(climb_rate,DEC); + SendSer(",ALT:"); + SendSer(current_loc.alt/100,DEC); + SendSer(",ALH:"); + SendSer(next_WP.alt/100,DEC); + SendSer(",CRS:"); + SendSer(yaw_sensor/100,DEC); + SendSer(",BER:"); + SendSer(target_bearing/100,DEC); + SendSer(",WPN:"); + SendSer(wp_index,DEC);//Actually is the waypoint. + SendSer(",DST:"); + SendSer(wp_distance,DEC); + SendSer(",BTV:"); + SendSer(battery_voltage,DEC); + SendSer(",RSP:"); + SendSer(servo_out[CH_ROLL]/100,DEC); + SendSer(",TOW:"); + SendSer(GPS.time); + SendSerln(",***"); +} + +void print_waypoint(struct Location *cmd,byte index) +{ + SendSer("command #: "); + SendSer(index, DEC); + SendSer(" id: "); + SendSer(cmd->id,DEC); + SendSer(" p1: "); + SendSer(cmd->p1,DEC); + SendSer(" p2: "); + SendSer(cmd->alt,DEC); + SendSer(" p3: "); + SendSer(cmd->lat,DEC); + SendSer(" p4: "); + SendSerln(cmd->lng,DEC); +} + +void print_waypoints() +{ +} + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde new file mode 100644 index 0000000000..8bd08bc1b2 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde @@ -0,0 +1,1258 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL + +/* This GCS protocol sends text-base information over the telemetry port +*/ + +#include //Allow us to store string constants (there are a lot!) in program memory to avoid using up valuable RAM + +#define ERR(a) ((DEBUGTERMINAL_VERBOSE)>0?(PSTR(a)):(0)) + +#if GCS_PORT == 3 +# define DTSTREAM Serial3 +#else +# define DTSTREAM Serial +#endif + +//Read buffer +#define DEBUGTERM_BUFFERSIZE (120) +char gcsinputbuffer[DEBUGTERM_BUFFERSIZE]; +byte bufferidx; + +//Reporting flags +byte report_heartbeat = 1; +byte report_attitude = 0; +byte report_location = 0; +byte report_command = 1; +byte report_severity = 0; +byte first_location = 0; + +void readgcsinput() { + byte numc, i, c; + + numc = DTSTREAM.available(); + for (i=0;i 0) bufferidx--; + } else if (c==10) { + //Received a linefeed; do nothing + } else if (c==13) { + //Received a carriage return; process command + gcsinputbuffer[bufferidx] = 0; + run_debugt_command(gcsinputbuffer); + bufferidx = 0; + } else { + gcsinputbuffer[bufferidx++] = c; + if (bufferidx >= DEBUGTERM_BUFFERSIZE) bufferidx = 0; + } +} + +void run_debugt_command(char *buf) { + //*********** Ignore comments *********** + if (strmatch(buf, PSTR("//"))) { + + //*********** Process 'show' commands *********** + } else if (strmatch(buf, PSTR("show "))) { + if (strmatch(buf+5, PSTR("heartbeat"))) + report_heartbeat = 1; + else if (strmatch(buf+5, PSTR("attitude"))) + report_attitude = 1; + else if (strmatch(buf+5, PSTR("location"))) + report_location = 1; + else if (strmatch(buf+5, PSTR("command"))) + report_command = 1; + else if (strmatch(buf+5, PSTR("severity"))) + report_severity = atoi(buf+14); + else + print_error(ERR("USAGE: show {heartbeat|attitude|location|command|severity }")); + + //*********** Process 'hide' commands *********** + } else if (strmatch(buf, PSTR("hide "))) { + if (strmatch(buf+5, PSTR("heartbeat"))) + report_heartbeat = 0; + else if (strmatch(buf+5, PSTR("attitude"))) + report_attitude = 0; + else if (strmatch(buf+5, PSTR("location"))) + report_location = 0; + else if (strmatch(buf+5, PSTR("command"))) + report_command = 0; + else if (strmatch(buf+5, PSTR("all"))) { + report_heartbeat = 0; + report_attitude = 0; + report_location = 0; + report_command = 0; + } else + print_error(ERR("USAGE: hide {heartbeat|attitude|location|command|all}")); + + //*********** Process 'echo' command *********** + } else if (strmatch(buf, PSTR("echo "))) { + DTSTREAM.println(buf+5); + + //*********** Process 'groundstart' command *********** + } else if (strmatch(buf, PSTR("groundstart"))) { + startup_ground(); + + //*********** Process 'inithome' command *********** + } else if (strmatch(buf, PSTR("inithome"))) { + init_home(); + DTSTREAM.println_P("Home set."); + + //*********** Process 'print' commands *********** + } else if (strmatch(buf, PSTR("print "))) { + //------- print altitude ------- + if (strmatch(buf+6, PSTR("altitude"))) { + DTSTREAM.println_P("Altitude:"); + DTSTREAM.print_P(" Pressure: "); DTSTREAM.print(((float)press_alt)/100,2); DTSTREAM.println_P(" m"); + DTSTREAM.print_P(" GPS: "); DTSTREAM.print(((float)GPS.altitude)/100,2); DTSTREAM.println_P(" m"); + DTSTREAM.print_P(" Mix ratio: "); DTSTREAM.println(altitude_mix,3); + DTSTREAM.print_P(" Mix: "); DTSTREAM.print((float)(((1 - altitude_mix) * GPS.altitude) + (altitude_mix * press_alt))/100,2); DTSTREAM.println_P(" m"); + + //------- print attitude ------- + } else if (strmatch(buf+6, PSTR("attitude"))) { + print_attitude(); + + //------- print commands ------- + } else if (strmatch(buf+6, PSTR("commands"))) { + print_waypoints(); + + //------- print ctrlmode ------- + } else if (strmatch(buf+6, PSTR("ctrlmode"))) { + print_control_mode(); + + //------- print curwaypts ------- + } else if (strmatch(buf+6, PSTR("curwaypts"))) { + print_current_waypoints(); + + //------- print flightmodes ------- + } else if (strmatch(buf+6, PSTR("flightmodes"))) { + int i; + DTSTREAM.print_P("EEPROM read: "); + for (i=0; i<6; i++) { + DTSTREAM.print_P("Ch "); DTSTREAM.print(i,DEC); DTSTREAM.print_P(" = "); DTSTREAM.print(flight_modes[i],DEC); DTSTREAM.print_P(", "); + } + DTSTREAM.println(" "); + + //------- print k -? ------- + } else if (strmatch(buf+6, PSTR("k -?"))) { + print_error(ERR("USAGE: print k{p|i|d} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}")); + print_error(ERR("USAGE: print kff {pitchcomp|ruddermix|pitchtothrottle}")); + + //------- print kp ------- + } else if (strmatch(buf+6, PSTR("kp "))) { + int i; + unsigned char j; + if (get_pid_offset_name(buf+9, &i, &j)) { + DTSTREAM.print_P("P gain for "); + DTSTREAM.print(buf+9); + DTSTREAM.print_P(" = "); + DTSTREAM.println(kp[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize P keyword; type print k -? for more information")); + + //------- print ki ------- + } else if (strmatch(buf+6, PSTR("ki "))) { + int i; + unsigned char j; + if (get_pid_offset_name(buf+9, &i, &j)) { + DTSTREAM.print_P("I gain for "); + DTSTREAM.print(buf+9); + DTSTREAM.print_P(" = "); + DTSTREAM.println(ki[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize I keyword; type print k -? for more information")); + + //------- print kd ------- + } else if (strmatch(buf+6, PSTR("kd "))) { + int i; + unsigned char j; + if (get_pid_offset_name(buf+9, &i, &j)) { + DTSTREAM.print_P("D gain for "); + DTSTREAM.print(buf+9); + DTSTREAM.print_P(" = "); + DTSTREAM.println(kd[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize D keyword; type print k -? for more information")); + + //------- print kff ------- + } else if (strmatch(buf+6, PSTR("kff "))) { + if (strmatch(buf+10, PSTR("pitchcomp"))) { + DTSTREAM.print_P("FF gain for pitchcomp = "); + DTSTREAM.println(kff[CASE_PITCH_COMP],DEC); + } else if (strmatch(buf+10, PSTR("ruddermix"))) { + DTSTREAM.print_P("FF gain for ruddermix = "); + DTSTREAM.println(kff[CASE_RUDDER_MIX],DEC); + } else if (strmatch(buf+10, PSTR("pitchtothrottle"))) { + DTSTREAM.print_P("FF gain for pitchtothrottle = "); + DTSTREAM.println(kff[CASE_P_TO_T],DEC); + } else + print_error(ERR("ERROR: Did not recognize FF keyword; type print k -? for more information")); + + //------- print location ------- + } else if (strmatch(buf+6, PSTR("location"))) { + print_position(); + + //------- print navsettings ------- + } else if (strmatch(buf+6, PSTR("navsettings"))) { + DTSTREAM.println_P("Navigation settings:"); +#if AIRSPEED_SENSOR == 1 + DTSTREAM.print_P(" Cruise airspeed: "); DTSTREAM.println((float)airspeed_cruise/100.0,2); +#else + DTSTREAM.print_P(" Cruise throttle: "); DTSTREAM.println(throttle_cruise,DEC); +#endif + DTSTREAM.print_P(" Hold altitude above home: "); DTSTREAM.println(read_alt_to_hold(),DEC); + DTSTREAM.print_P(" Loiter radius: "); DTSTREAM.println(loiter_radius,DEC); + DTSTREAM.print_P(" Waypoint radius: "); DTSTREAM.println(wp_radius,DEC); + + //------- print pressure ------- + } else if (strmatch(buf+6, PSTR("pressure"))) { + DTSTREAM.println_P("BMP085 pressure sensor:"); + DTSTREAM.print_P(" Temperature: "); DTSTREAM.println(APM_BMP085.Temp,DEC); + DTSTREAM.print_P(" Pressure: "); DTSTREAM.println(APM_BMP085.Press,DEC); + + //------- print rlocation home ------- + } else if (strmatch(buf+6, PSTR("rlocation home"))) { + print_rlocation(&home); + + //------- print rlocation ------- + //(implication is "relative to next waypoint") + } else if (strmatch(buf+6, PSTR("rlocation"))) { + print_rlocation(&next_WP); + + //------- print speed ------- + } else if (strmatch(buf+6, PSTR("speed"))) { + DTSTREAM.println_P("Speed:"); + DTSTREAM.print_P(" Ground: "); DTSTREAM.println((float)GPS.ground_speed/100.0,2); +#if AIRSPEED_SENSOR == 1 + DTSTREAM.print_P(" Air: "); DTSTREAM.println((float)airspeed/100.0,2); + DTSTREAM.print_P(" Cruise: "); DTSTREAM.println((float)airspeed_cruise/100.0,2); +#endif + + //------- print tuning ------- + } else if (strmatch(buf+6, PSTR("tuning"))) { + print_tuning(); + + } else + print_error(ERR("USAGE: print {altitude|attitude|commands|ctrlmode|curwaypts|flightmodes|k -?|kp|ki|kd|kff|location|navsettings|pressure|rlocation [home]|speed|tuning|}")); + + //*********** Process 'reset' commands *********** + } else if (strmatch(buf, PSTR("reset "))) { + //------- reset commands ------- + if (strmatch(buf+6, PSTR("commands"))) { + reload_commands(); + DTSTREAM.println_P("Commands reloaded."); + } else + print_error(ERR("USAGE: reset commands")); + + //*********** Process 'rtl' command *********** + } else if (strmatch(buf, PSTR("rtl"))) { + return_to_launch(); + DTSTREAM.println_P("Returning to launch..."); + + //*********** Process 'set' commands *********** + } else if (strmatch(buf, PSTR("set "))) { + //------- set cmd ------- + if (strmatch(buf+4, PSTR("cmd "))) { + process_set_cmd(buf+8, bufferidx-8); + + //------- set cmdcount ------- + } else if (strmatch(buf+4, PSTR("cmdcount "))) { + wp_total = atoi(buf+13); + save_EEPROM_waypoint_info(); + DTSTREAM.print_P("wp_total = "); DTSTREAM.println(wp_total,DEC); + + //------- set cmdindex ------- + } else if (strmatch(buf+4, PSTR("cmdindex "))) { + wp_index = atoi(buf+13); + decrement_WP_index(); + next_command = get_wp_with_index(wp_index+1); + DTSTREAM.print_P("Command set to index "); DTSTREAM.print(wp_index,DEC); + if (next_command.id >= 0x10 && next_command.id <= 0x1F) { //TODO: create a function the defines what type of command each command ID is + command_must_index = 0; + DTSTREAM.println_P(" (must)"); + } else if (next_command.id >= 0x20 && next_command.id <= 0x2F) { + command_may_index = 0; + DTSTREAM.println_P(" (may)"); + } else + DTSTREAM.println_P(" (now)"); + + next_command.id = CMD_BLANK; + if (wp_index > wp_total) { + wp_total = wp_index; + DTSTREAM.print_P(" The total number of commands is now "); + DTSTREAM.println(wp_total,DEC); + } + update_commands(); + + //------- set cruise ------- + } else if (strmatch(buf+4, PSTR("cruise "))) { + unsigned char j = 4+7; +#if AIRSPEED_SENSOR == 1 + DTSTREAM.print_P("airspeed_cruise changed from "); + DTSTREAM.print((float)airspeed_cruise/100,2); + DTSTREAM.print_P(" to "); + airspeed_cruise = (int)(readfloat(buf, &j)/100000); + airspeed_cruise = constrain(airspeed_cruise,0,9000); //TODO: constrain minimum as stall speed, maximum as Vne + DTSTREAM.println(((float)airspeed_cruise)/100,2); +#else + DTSTREAM.print_P("throttle_cruise changed from "); + DTSTREAM.print(throttle_cruise,DEC); + DTSTREAM.print_P(" to "); + throttle_cruise = constrain((int)(readfloat(buf, &j)/10000000),0,200); + DTSTREAM.println(throttle_cruise,DEC); +#endif + + //------- set holdalt ------- + } else if (strmatch(buf+4, PSTR("holdalt "))) { + int tempalt = atoi(buf+12)*100; + save_alt_to_hold((int32_t)tempalt); + DTSTREAM.println_P("Hold altitude above home set."); + + //------- set k -? ------- + } else if (strmatch(buf+4, PSTR("k -?"))) { + print_error(ERR("USAGE: set k{p|i|d} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ")); + print_error(ERR("USAGE: set kff {pitchcomp|ruddermix|pitchtothrottle} ")); + + //------- set kp ------- + } else if (strmatch(buf+4, PSTR("kp "))) { + int i; + unsigned char j = 0; + if (get_pid_offset_name(buf+7, &i, &j)) { + buf[7+j] = 0; + DTSTREAM.print_P("P gain for "); + DTSTREAM.print(buf+7); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(kp[i],DEC); + DTSTREAM.print_P(" to "); + j += 7+1; + kp[i] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(kp[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize P keyword; type set k -? for more information")); + + //------- set ki ------- + } else if (strmatch(buf+4, PSTR("ki "))) { + int i; + unsigned char j = 0; + if (get_pid_offset_name(buf+7, &i, &j)) { + buf[7+j] = 0; + DTSTREAM.print_P("I gain for "); + DTSTREAM.print(buf+7); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(ki[i],DEC); + DTSTREAM.print_P(" to "); + j += 7+1; + ki[i] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(ki[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize I keyword; type set k -? for more information")); + + //------- set kd ------- + } else if (strmatch(buf+4, PSTR("kd "))) { + int i; + unsigned char j = 0; + if (get_pid_offset_name(buf+7, &i, &j)) { + buf[7+j] = 0; + DTSTREAM.print_P("D gain for "); + DTSTREAM.print(buf+7); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(kd[i],DEC); + DTSTREAM.print_P(" to "); + j += 7+1; + kd[i] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(kd[i],DEC); + } else + print_error(ERR("ERROR: Did not recognize D keyword; type set k -? for more information")); + + //------- set kff ------- + } else if (strmatch(buf+4, PSTR("kff "))) { + unsigned char j = 0; + if (strmatch(buf+8, PSTR("pitchcomp"))) { + buf[8+9] = 0; + DTSTREAM.print_P("FF gain for "); + DTSTREAM.print(buf+8); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(kff[CASE_PITCH_COMP],DEC); + DTSTREAM.print_P(" to "); + j = 8+9+1; + kff[CASE_PITCH_COMP] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(kff[CASE_PITCH_COMP],DEC); + } else if (strmatch(buf+8, PSTR("ruddermix"))) { + buf[8+9] = 0; + DTSTREAM.print_P("FF gain for "); + DTSTREAM.print(buf+8); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(kff[CASE_RUDDER_MIX],DEC); + DTSTREAM.print_P(" to "); + j = 8+9+1; + kff[CASE_RUDDER_MIX] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(kff[CASE_RUDDER_MIX],DEC); + } else if (strmatch(buf+8, PSTR("pitchtothrottle"))) { + buf[8+15] = 0; + DTSTREAM.print_P("FF gain for "); + DTSTREAM.print(buf+8); + DTSTREAM.print_P(" changed from "); + DTSTREAM.print(kff[CASE_P_TO_T],DEC); + DTSTREAM.print_P(" to "); + j = 8+15+1; + kff[CASE_P_TO_T] = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.println(kff[CASE_P_TO_T],DEC); + } else + print_error(ERR("ERROR: Did not recognize FF keyword; type print k -? for more information")); + + //------- set loiterradius ------- + } else if (strmatch(buf+4, PSTR("loiterradius "))) { + loiter_radius = atoi(buf+17); + save_EEPROM_waypoint_info(); + DTSTREAM.print_P("Set loiter radius to "); DTSTREAM.print(loiter_radius,DEC); DTSTREAM.println_P(" meters"); + + //------- set rcin ------- + } else if (strmatch(buf+4, PSTR("rcin"))) { + unsigned char index = buf[8]-'1'; + if (index < 8) { + radio_in[index] = atoi(buf+9); + } else + print_error(ERR("USAGE: set rcin ")); + + //------- set rcout ------- + } else if (strmatch(buf+4, PSTR("rcout"))) { + unsigned char index = buf[9]-'1'; + if (index < 8) { + radio_out[index] = atoi(buf+10); + APM_RC.OutputCh(index, radio_out[index]); + } else + print_error(ERR("USAGE: set rcout ")); + + //------- set wpradius ------- + } else if (strmatch(buf+4, PSTR("wpradius "))) { + wp_radius = atoi(buf+13); + save_EEPROM_waypoint_info(); + DTSTREAM.print_P("Set waypoint radius to "); DTSTREAM.print(wp_radius,DEC); DTSTREAM.println_P(" meters"); + + //------- set xtrackentryangle ------- + } else if (strmatch(buf+4, PSTR("xtrackentryangle "))) { + unsigned char j = 21; + x_track_angle = readfloat(buf, &j)/100000; + DTSTREAM.print_P("Crosstrack entry angle set to "); DTSTREAM.println((float)x_track_angle/100,2); + + //------- set xtrackgain ------- + } else if (strmatch(buf+4, PSTR("xtrackgain "))) { + unsigned char j = 15; + x_track_gain = ((float)readfloat(buf, &j))/10000000; + DTSTREAM.print_P("Crosstrack gain set to "); DTSTREAM.println(x_track_gain,2); + + } else + print_error(ERR("USAGE: set {cmd|cmdcount|cmdindex|cruise|holdalt|kp|ki|kd|kff|loiterradius|rcin|rcout|wpradius}")); + + //*********** Process 'status' commands *********** + } else if (strmatch(buf, PSTR("status "))) { + //------- status control ------- + if (strmatch(buf+7, PSTR("control"))) { + DTSTREAM.println_P("Control status:"); + DTSTREAM.print_P(" Roll: nav="); DTSTREAM.print(nav_roll/100.0,2); DTSTREAM.print_P(", servo_out="); DTSTREAM.println((float)servo_out[CH_ROLL]/100.0,2); + DTSTREAM.print_P(" Pitch: nav="); DTSTREAM.print(nav_pitch/100.0,2); DTSTREAM.print_P(", servo_out="); DTSTREAM.println((float)servo_out[CH_PITCH]/100.0,2); + DTSTREAM.print_P(" Throttle: nav="); DTSTREAM.print(throttle_cruise,DEC); DTSTREAM.print_P(", servo_out="); DTSTREAM.println(servo_out[CH_THROTTLE],DEC); + + //------- status gps ------- + } else if (strmatch(buf+7, PSTR("gps"))) { + DTSTREAM.println_P("GPS status:"); + DTSTREAM.print_P(" Fix: "); + if (GPS.fix == 0) + DTSTREAM.print_P("INVALID ("); + else + DTSTREAM.print_P("Valid ("); + DTSTREAM.print(GPS.fix,DEC); + DTSTREAM.println_P(")"); + DTSTREAM.print_P(" Satellites: "); DTSTREAM.println(GPS.num_sats,DEC); + DTSTREAM.print_P(" Fix count: "); DTSTREAM.println(gps_fix_count,DEC); + + //------- status landing ------- + } else if (strmatch(buf+7, PSTR("landing"))) { + DTSTREAM.println_P("Landing status:"); + DTSTREAM.print_P(" land_complete = "); DTSTREAM.println(land_complete,DEC); + DTSTREAM.print_P(" landing_pitch = "); DTSTREAM.println(landing_pitch,DEC); + DTSTREAM.print_P(" nav_pitch = "); DTSTREAM.println(nav_pitch,DEC); + DTSTREAM.print_P(" airspeed_cruise = "); DTSTREAM.println(airspeed_cruise,DEC); + DTSTREAM.print_P(" throttle_cruise = "); DTSTREAM.println(throttle_cruise,DEC); + DTSTREAM.print_P(" hold_course = "); DTSTREAM.println(hold_course,DEC); + DTSTREAM.print_P(" nav_bearing = "); DTSTREAM.println(nav_bearing,DEC); + DTSTREAM.print_P(" bearing_error = "); DTSTREAM.println(bearing_error,DEC); + + //------- status loiter ------- + } else if (strmatch(buf+7, PSTR("loiter"))) { + DTSTREAM.println_P("Loiter status:"); + DTSTREAM.print_P(" Loiter radius: "); DTSTREAM.println(loiter_radius,DEC); + DTSTREAM.print_P(" Progress: "); DTSTREAM.print(loiter_sum,DEC); DTSTREAM.print_P("°/"); DTSTREAM.print(loiter_total,DEC); DTSTREAM.println_P("°"); + DTSTREAM.print_P(" Time: "); DTSTREAM.print(loiter_time,DEC); DTSTREAM.print_P("ms/"); DTSTREAM.print(loiter_time_max,DEC); DTSTREAM.println_P("ms"); + + //------- status navigation ------- + } else if (strmatch(buf+7, PSTR("navigation"))) { + DTSTREAM.println_P("Navigation status:"); + DTSTREAM.print_P(" From <"); DTSTREAM.print((float)prev_WP.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)prev_WP.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)prev_WP.alt/100.0,1); DTSTREAM.print_P(">: "); print_rlocation(&prev_WP); + DTSTREAM.print_P(" At <"); DTSTREAM.print((float)current_loc.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)current_loc.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)current_loc.alt/100.0,1); DTSTREAM.println_P(">"); + DTSTREAM.print_P(" To <"); DTSTREAM.print((float)next_WP.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)next_WP.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)next_WP.alt/100.0,1); DTSTREAM.print_P(">: "); print_rlocation(&next_WP); + DTSTREAM.print_P(" Distance: "); DTSTREAM.print(100.0*(float)(wp_totalDistance-wp_distance)/(float)wp_totalDistance,1); DTSTREAM.print_P("% "); DTSTREAM.print(wp_totalDistance-wp_distance,DEC); DTSTREAM.print_P("m / "); DTSTREAM.print(wp_totalDistance,DEC); DTSTREAM.print_P("m; "); DTSTREAM.print(altitude_error/100.0,2); DTSTREAM.println_P("m vertically"); + DTSTREAM.print_P(" Nav bearing: "); DTSTREAM.print(nav_bearing/100.0,2); DTSTREAM.print_P("; error = "); DTSTREAM.println(bearing_error/100.0,2); + DTSTREAM.print_P(" Ground course: "); DTSTREAM.print(GPS.ground_course/100.0,1); DTSTREAM.print_P(" (current), "); DTSTREAM.print(target_bearing/100.0,1); DTSTREAM.println_P(" (target)"); + if (hold_course >= 0) { + DTSTREAM.print_P(" HOLD COURSE: "); DTSTREAM.println(hold_course/100.0,2); + } + + /*DTSTREAM.print_P(" Crosstrack bearing: "); DTSTREAM.println(crosstrack_bearing/100.0,DEC); + DTSTREAM.print_P(" Crosstrack error: "); DTSTREAM.println(crosstrack_error/100.0,DEC); + DTSTREAM.print_P(" Climb rate: "); DTSTREAM.println(climb_rate/100.0,DEC);*/ + + + //------- status navpitch ------- + } else if (strmatch(buf+7, PSTR("navpitch"))) { +#if AIRSPEED_SENSOR == 1 + DTSTREAM.print_P("nav_pitch = PID[airspeed_error ("); DTSTREAM.print(airspeed_error/100.0,2); DTSTREAM.print_P(") = airspeed_cruise ("); DTSTREAM.print((float)airspeed_cruise/100.0,2); DTSTREAM.print_P(") - airspeed ("); DTSTREAM.print((float)airspeed/100.0,2); DTSTREAM.println_P(")]"); +#else + DTSTREAM.print_P("nav_pitch = PID[altitude_error ("); DTSTREAM.print(altitude_error,DEC); DTSTREAM.print_P(") = target_altitude ("); DTSTREAM.print(target_altitude,DEC); DTSTREAM.print_P(") - current_loc.alt ("); DTSTREAM.print(current_loc.alt,DEC); DTSTREAM.println_P(")]"); +#endif + DTSTREAM.print_P("nav_pitch ("); DTSTREAM.print((float)nav_pitch/100.0,2); DTSTREAM.print_P(") - pitch_sensor ("); DTSTREAM.print((float)pitch_sensor/100.0,2); DTSTREAM.print_P(") + pitch_comp ("); DTSTREAM.print(abs(roll_sensor*kff[CASE_PITCH_COMP])/100.0,2); DTSTREAM.print_P(") = "); DTSTREAM.println((float)(nav_pitch-pitch_sensor+abs(roll_sensor*kff[CASE_PITCH_COMP]))/100.0,2); + DTSTREAM.print_P("servo_out[CH_PITCH] ("); DTSTREAM.print((float)servo_out[CH_PITCH]/100.0,2); DTSTREAM.println_P(") = PID[nav_pitch + pitch_comp - pitch_sensor]"); + + //------- status navroll ------- + } else if (strmatch(buf+7, PSTR("navroll"))) { + print_rlocation(&next_WP); + DTSTREAM.print_P("bearing_error ("); DTSTREAM.print(bearing_error,DEC); DTSTREAM.print_P(") = nav_bearing ("); DTSTREAM.print(nav_bearing,DEC); DTSTREAM.print_P(") - yaw_sensor ("); DTSTREAM.print(yaw_sensor,DEC); DTSTREAM.println_P(")"); + DTSTREAM.print_P("nav_roll ("); DTSTREAM.print(nav_roll,DEC); DTSTREAM.print_P(") - roll_sensor ("); DTSTREAM.print(roll_sensor,DEC); DTSTREAM.print_P(") = "); DTSTREAM.print(nav_roll-roll_sensor,DEC); DTSTREAM.println_P(")"); + DTSTREAM.print_P("servo_out[CH_ROLL] = "); DTSTREAM.println(servo_out[CH_ROLL],DEC); + + //------- status rcinputch ------- + } else if (strmatch(buf+7, PSTR("rcinputch"))) { + unsigned char i; + DTSTREAM.println_P("RC hardware input:"); + for (i=0; i<8; i++) { + DTSTREAM.print_P(" Ch"); + DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P(": "); + DTSTREAM.println(APM_RC.InputCh(i)); + } + + //------- status rcin ------- + } else if (strmatch(buf+7, PSTR("rcin"))) { + unsigned char i; + DTSTREAM.println_P("RC software input:"); + for (i=0; i<8; i++) { + DTSTREAM.print_P(" Ch"); + DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P(": "); + DTSTREAM.println(radio_in[i]); + } + + //------- status rcout ------- + } else if (strmatch(buf+7, PSTR("rcout"))) { + unsigned char i; + DTSTREAM.println_P("RC software output:"); + for (i=0; i<8; i++) { + DTSTREAM.print_P(" Ch"); + DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P(": "); + DTSTREAM.println(radio_out[i]); + } + + //------- status rcpwm ------- + } else if (strmatch(buf+7, PSTR("rcpwm"))) { + unsigned char i; + DTSTREAM.println_P("RC hardware output:"); + for (i=0; i<8; i++) { + DTSTREAM.print_P(" Ch"); + DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P(": "); + DTSTREAM.println(readOutputCh(i)); + } + + //------- status rctrim ------- + } else if (strmatch(buf+7, PSTR("rctrim"))) { + unsigned char i; + DTSTREAM.println_P("RC trim:"); + for (i=0; i<8; i++) { + DTSTREAM.print_P(" Ch"); + DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P(": "); + DTSTREAM.println(radio_trim[i]); + } + DTSTREAM.print_P(" elevon1_trim = "); DTSTREAM.println(elevon1_trim,DEC); + DTSTREAM.print_P(" elevon2_trim = "); DTSTREAM.println(elevon2_trim,DEC); + + //------- status rc ------- + } else if (strmatch(buf+7, PSTR("rc"))) { + unsigned char i; + DTSTREAM.println_P("RC:\tCh\tHWin\tSWtrim\tSWin\tServo\tSWout\tHWout\t"); + for (i=0; i<8; i++) { + DTSTREAM.print_P("\t"); DTSTREAM.print(i+1,DEC); + DTSTREAM.print_P("\t"); DTSTREAM.print(APM_RC.InputCh(i),DEC); + DTSTREAM.print_P("\t"); DTSTREAM.print(radio_trim[i],DEC); + DTSTREAM.print_P("\t"); DTSTREAM.print(radio_in[i],DEC); + DTSTREAM.print_P("\t"); DTSTREAM.print(((float)servo_out[i])/100,2); + DTSTREAM.print_P("\t"); DTSTREAM.print(radio_out[i],DEC); + DTSTREAM.print_P("\t"); DTSTREAM.println(readOutputCh(i),DEC); + } + + //------- status system ------- + } else if (strmatch(buf+7, PSTR("system"))) { + DTSTREAM.println_P("System status:"); + DTSTREAM.print_P(" Ground start: "); if (ground_start) DTSTREAM.print_P("YES ("); else DTSTREAM.print_P("NO ("); + DTSTREAM.print(ground_start,DEC); DTSTREAM.println_P(")"); + DTSTREAM.print_P(" Home: "); if (home_is_set) DTSTREAM.print_P(" SET("); else DTSTREAM.print_P("NOT SET ("); + DTSTREAM.print(home_is_set,DEC); DTSTREAM.println_P(")"); + + //------- status takeoff ------- + } else if (strmatch(buf+7, PSTR("takeoff"))) { + DTSTREAM.println_P("Takeoff status:"); + DTSTREAM.print_P(" takeoff_pitch = "); DTSTREAM.println((float)takeoff_pitch/100.0,2); + DTSTREAM.print_P(" scaler = "); DTSTREAM.println((float)airspeed/(float)airspeed_cruise,3); + DTSTREAM.print_P(" nav_pitch = "); DTSTREAM.println((float)nav_pitch/100.0,2); + DTSTREAM.print_P(" throttle = "); DTSTREAM.println(servo_out[CH_THROTTLE],DEC); + DTSTREAM.print_P(" hold_course = "); DTSTREAM.println((float)hold_course/100.0,2); + DTSTREAM.print_P(" nav_bearing = "); DTSTREAM.println(nav_bearing,DEC); + DTSTREAM.print_P(" bearing_error = "); DTSTREAM.println(bearing_error,DEC); + DTSTREAM.print_P(" current_loc.alt = "); DTSTREAM.println(current_loc.alt,DEC); + DTSTREAM.print_P(" home.alt + takeoff_altitude = "); DTSTREAM.println(home.alt + takeoff_altitude,DEC); + + //nav_pitch = constrain(nav_pitch, 0, takeoff_pitch * 100); + //calc_nav_pitch(); + nav_pitch = constrain(nav_pitch, 0, takeoff_pitch * 100); + + //------- status telemetry ------- + } else if (strmatch(buf+7, PSTR("telemetry"))) { + DTSTREAM.println_P("Telemetry status:"); + if (report_heartbeat) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("heartbeat"); + if (report_location) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("location"); + if (report_attitude) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("attitude"); + if (report_command) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("command"); + DTSTREAM.print_P(" Severity report level "); DTSTREAM.println(report_severity,DEC); + + //------- status throttle ------- + } else if (strmatch(buf+7, PSTR("throttle"))) { +#if AIRSPEED_SENSOR == 1 + DTSTREAM.print_P("airspeed_energy_error ("); DTSTREAM.print(airspeed_energy_error,DEC); DTSTREAM.print_P(") = 0.5 * (airspeed_cruise ("); DTSTREAM.print((float)airspeed_cruise/100.0,2); DTSTREAM.print_P(")^2 - airspeed ("); DTSTREAM.print((float)airspeed/100.0,2); DTSTREAM.println_P(")^2)"); + DTSTREAM.print_P("energy_error ("); DTSTREAM.print(energy_error,DEC); DTSTREAM.print_P(") = airspeed_energy_error ("); DTSTREAM.print(airspeed_energy_error,DEC); DTSTREAM.print_P(") + altitude_error*0.098 ("); DTSTREAM.print((long)((float)altitude_error*0.098),DEC); DTSTREAM.println_P(")"); + DTSTREAM.print_P("servo_out[CH_THROTTLE] ("); DTSTREAM.print(servo_out[CH_THROTTLE],DEC); DTSTREAM.println_P(") = PID[energy_error]"); +#else + DTSTREAM.print_P("altitude_error ("); DTSTREAM.print(altitude_error,DEC); DTSTREAM.print_P(") = target_altitude ("); DTSTREAM.print(target_altitude,DEC); DTSTREAM.print_P(") - current_loc.alt ("); DTSTREAM.print(current_loc.alt,DEC); DTSTREAM.println_P(")"); + DTSTREAM.print_P("servo_out[CH_THROTTLE] ("); DTSTREAM.print(servo_out[CH_THROTTLE],DEC); DTSTREAM.println_P(") = PID[altitude_error]"); +#endif + + //------- status waypoints ------- + } else if (strmatch(buf+7, PSTR("waypoints"))) { + DTSTREAM.println_P("Waypoints status:"); + DTSTREAM.print_P(" Distance: "); DTSTREAM.print(wp_distance,DEC); DTSTREAM.print_P("/"); DTSTREAM.println(wp_totalDistance,DEC); + DTSTREAM.print_P(" Index: "); DTSTREAM.print(wp_index,DEC); DTSTREAM.print_P("/"); DTSTREAM.println(wp_total,DEC); + DTSTREAM.print_P(" Next: "); DTSTREAM.println(next_wp_index,DEC); + + } else if (strmatch(buf+7, PSTR("james"))) { + DTSTREAM.println_P("James is a monkey"); + } else { + print_error(ERR("USAGE: status {control|gps|landing|loiter|mixing|navigation|navpitch|navroll|rc|rcinputch|rcin|rcout|rcpwm|rctrim|system|takeoff|telemetry|throttle|waypoints}")); + } + } else { + print_error(ERR("USAGE: {echo |groundstart|inithome|show|hide|print|status|set|reset|rtl}")); + print_error(ERR("Type -? for specific usage guidance")); + } +} + +/* strmatch compares two strings and returns 1 if they match and 0 if they don't + s2 must be stored in program memory (via PSTR) rather than RAM (like standard strings) + s1 must be at least as long as s2 for a valid match + if s1 is longer than s2, then only the beginning of s1 (the length of s2) must match s2 for a valid match */ +int strmatch(char* s1, const char* s2) { + int i = 0; + char c1 = s1[0], c2 = pgm_read_byte(s2); + + while (c1 != 0 && c2 != 0) { + if (c1 < c2) + return 0; + if (c1 > c2) + return 0; + i++; + c1 = s1[i]; + c2 = pgm_read_byte(s2+i); + } + + if (c2==0) + return 1; + else + return 0; +} + +/* readfloat parses a string written as a float starting at the offset in *i and updates *i as it parses + numbers without a decimal place are read as integers + numbers with a decimial place are multiplied by 10,000,000 and decimals beyond 7 places are discarded + parsing is terminated with either a space or a null, other characters are ignored */ +long readfloat(char* s, unsigned char* i) { + long result = 0, multiplier = 0; + char c, neg = 0; + + for (;;(*i)++) { + c = s[*i]; + if (c == ' ') + break; + else if (c == 0) + break; + else if (c == '-') + neg = 1-neg; + else if (c == '.') { + result *= 10000000; + multiplier = 1000000; + } else if (c >= '0' && c <= '9') { + if (multiplier == 0) + result = (result*10) + c-'0'; + else { + result += (c-'0')*multiplier; + multiplier /= 10; + } + } + } + + if (multiplier == 0) + result *= 10000000; + + if (neg) + return -result; + else + return result; +} + +/* process_set_cmd processing the parameters of a 'set cmd' command and takes the appropriate action + *buffer is the buffer containing the parameters of the command; it should start after the space after 'set cmd' + bufferlen is the length of the buffer; the routine will stop looking for parameters after the offset index reaches this value +*/ +#define SETPARAM_NONE (0) +#define SETPARAM_ID (1) +#define SETPARAM_P1 (2) +#define SETPARAM_LAT (3) +#define SETPARAM_LNG (4) +#define SETPARAM_ALT (5) +#define SETPARAM_P2 (6) +#define SETPARAM_P3 (7) +#define SETPARAM_P4 (8) + +void process_set_cmd(char *buffer, int bufferlen) { + unsigned char i, j, err=1, setparam=SETPARAM_NONE; + unsigned char cmdindex=0, p1=0, cmdid; + long lat, lng, alt; + Location temp; + + //Parse the command index + for (i=0; i= '0' && buffer[i] <= '9') + cmdindex = (cmdindex*10) + buffer[i]-'0'; + else + break; + + if (buffer[i] == ' ') { + //Find the end of the command-type string + i++; + for (j=i; j SETPARAM_NONE) { + //Process new parameter value + i = j+1; + lat = readfloat(buffer, &i); + temp = get_wp_with_index(cmdindex); + if (setparam == SETPARAM_ID) + temp.id = lat/10000000; + else if (setparam == SETPARAM_P1) + temp.p1 = lat/10000000; + else if (setparam == SETPARAM_ALT) + temp.alt = lat/100000; + else if (setparam == SETPARAM_LAT) + temp.lat = lat; + else if (setparam == SETPARAM_LNG) + temp.lng = lat; + else if (setparam == SETPARAM_P2) + temp.alt = lat/10000000; + else if (setparam == SETPARAM_P3) + temp.lat = lat/10000000; + else if (setparam == SETPARAM_P4) + temp.lng = lat/10000000; + cmdid = temp.id; + p1 = temp.p1; + lat = temp.lat; + lng = temp.lng; + alt = temp.alt; + err = 0; + } else { + //Process param 1 + for (i=j+1; i= '0' && buffer[i] <= '9') + p1 = (p1*10) + buffer[i]-'0'; + else + break; + } + + if (buffer[i] == ' ') { + //Process altitude + i++; + if (strmatch(buffer+i, PSTR("here"))) { + lat = GPS.latitude; + lng = GPS.longitude; + alt = get_altitude_above_home(); //GPS.altitude; + err = 0; + } else { + alt = readfloat(buffer, &i)/100000; + + if (buffer[i] == ' ') { + //Process latitude + i++; + lat = readfloat(buffer, &i); + if (strmatch(buffer+i, PSTR("here"))) { + lat = GPS.latitude; + lng = GPS.longitude; + err = 0; + } else { + if (buffer[i] == ' ') { + //Process longitude + i++; + lng = readfloat(buffer, &i); + err = 0; + } else + print_error(ERR("Error processing set cmd: Could not find longitude parameter")); + } + } else + print_error(ERR("Error processing set cmd: Could not find latitude parameter")); + } + } else + print_error(ERR("Error processing set cmd: Could not find altitude parameter")); + } + } else + print_error(ERR("Error processing set cmd: Could not find command type")); + } else + print_error(ERR("Error processing set cmd: Could not find command index")); + + if (err == 0) { + temp.id = cmdid; + if (cmdid == CMD_LAND_OPTIONS) { + temp.p1 = p1; + temp.alt = alt; + temp.lat = lat / 10000000; + temp.lng = lng / 10000000; + } else { + temp.p1 = p1; + temp.lat = lat; + temp.lng = lng; + temp.alt = alt; + } + if (cmdindex >= wp_total) { + wp_total = cmdindex+1; + save_EEPROM_waypoint_info(); + } + set_wp_with_index(temp, cmdindex); + + DTSTREAM.print_P("Set command "); + DTSTREAM.print((int)cmdindex); + DTSTREAM.print_P(" to "); + DTSTREAM.print((int)temp.id); + DTSTREAM.print_P(" with p1="); + DTSTREAM.print((int)temp.p1); + DTSTREAM.print_P(", lat="); + DTSTREAM.print(temp.lat); + DTSTREAM.print_P(", lng="); + DTSTREAM.print(temp.lng); + DTSTREAM.print_P(", alt="); + DTSTREAM.println(temp.alt); + } +} + +/* get_pid_offset_name matches a string expressed in *buffer with a pid keyword and returns the k-array + gain offset in *offset, and the length of that string expression in *length. If a good keyword + match is found, 1 is returned; 0 otherwise +*/ +int get_pid_offset_name(char *buffer, int *offset, unsigned char *length) { + if (strmatch(buffer, PSTR("servoroll"))) { + *length += 9; *offset = CASE_SERVO_ROLL; + } else if (strmatch(buffer, PSTR("servopitch"))) { + *length += 10; *offset = CASE_SERVO_PITCH; + } else if (strmatch(buffer, PSTR("servorudder"))) { + *length += 11; *offset = CASE_SERVO_RUDDER; + } else if (strmatch(buffer, PSTR("navroll"))) { + *length += 7; *offset = CASE_NAV_ROLL; + } else if (strmatch(buffer, PSTR("navpitchasp"))) { + *length += 11; *offset = CASE_NAV_PITCH_ASP; + } else if (strmatch(buffer, PSTR("navpitchalt"))) { + *length += 11; *offset = CASE_NAV_PITCH_ALT; + } else if (strmatch(buffer, PSTR("throttlete"))) { + *length += 10; *offset = CASE_TE_THROTTLE; + } else if (strmatch(buffer, PSTR("throttlealt"))) { + *length += 11; *offset = CASE_ALT_THROTTLE; + } else { + return 0; + } + + return 1; +} + +/* print_rlocation prints the relative location of the specified waypoint from the plane in easy-to-read cartesian format +*/ +void print_rlocation(Location *wp) { + float x, y; + y = (float)(wp->lat - current_loc.lat) * 0.0111194927; + x = (float)(wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; + DTSTREAM.print_P("dP = <"); + DTSTREAM.print(abs((int)y),DEC); + if (y >= 0) DTSTREAM.print_P("N, "); else DTSTREAM.print_P("S, "); + DTSTREAM.print(abs((int)x),DEC); + if (x >= 0) DTSTREAM.print_P("E, "); else DTSTREAM.print_P("W, "); + DTSTREAM.print((float)(wp->alt - current_loc.alt)/100,1); + DTSTREAM.println_P("> meters"); +} + +/* print_error prints an error message if the user sends an invalid command +*/ +void print_error(const char *msg) { + if (msg == 0) + DTSTREAM.println_P(PSTR("ERROR: Invalid command")); + else + DTSTREAM.println_P(msg); +} + +void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 +{ + if(severity >= DEBUG_LEVEL){ + DTSTREAM.print_P("MSG: "); + DTSTREAM.println(str); + } +} + +void send_message(byte id) { + send_message(id,(long)0); +} + +void send_message(byte id, long param) { + + switch(id) { + case MSG_HEARTBEAT: + if (report_heartbeat) + print_control_mode(); + break; + + case MSG_ATTITUDE: + if (report_attitude) + print_attitude(); + break; + + case MSG_LOCATION: + if (report_location) + print_position(); + if (first_location == 0) { + send_message(0,"First location received"); + first_location = 1; + } + break; + + case MSG_COMMAND: + struct Location cmd = get_wp_with_index(param); + print_waypoint(&cmd, param); + break; + + } +} + +void pipe() +{ + DTSTREAM.print_P("|"); +} + +void print_current_waypoints() +{ + DTSTREAM.println_P("Current waypoints:"); + DTSTREAM.print_P(" Prev:"); + DTSTREAM.print_P("\t"); + DTSTREAM.print(prev_WP.lat,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.print(prev_WP.lng,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.println(prev_WP.alt,DEC); + + DTSTREAM.print_P(" Cur: "); + DTSTREAM.print_P("\t"); + DTSTREAM.print(current_loc.lat,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.print(current_loc.lng,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.println(current_loc.alt,DEC); + + DTSTREAM.print_P(" Next:"); + DTSTREAM.print(wp_index,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.print(next_WP.lat,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.print(next_WP.lng,DEC); + DTSTREAM.print_P(",\t"); + DTSTREAM.println(next_WP.alt,DEC); +} + +void print_position(void) +{ + DTSTREAM.print_P("Pos: "); + DTSTREAM.print(current_loc.lat,DEC); // 0 + DTSTREAM.print_P(", "); //° + DTSTREAM.print(current_loc.lng,DEC); // 1 + DTSTREAM.print_P(", "); + DTSTREAM.print(current_loc.alt,DEC); // 2 + DTSTREAM.print_P("cm, "); + DTSTREAM.print(GPS.ground_speed,DEC); // 3 + DTSTREAM.print_P("cm/s GS, "); + DTSTREAM.print(airspeed,DEC); // 4 + DTSTREAM.print_P("cm/s AS, "); + DTSTREAM.print(get_altitude_above_home(),DEC); // 5 + DTSTREAM.print_P("cm above home, "); + DTSTREAM.print(climb_rate,DEC); // 6 + DTSTREAM.print_P("? climb, "); + DTSTREAM.print(wp_distance,DEC); // 7 + DTSTREAM.print_P("m from wp, "); + DTSTREAM.print(throttle_cruise,DEC); // 8 + DTSTREAM.print_P("% throttle, "); + DTSTREAM.print(altitude_error,DEC); // 9 + DTSTREAM.println_P("alt err"); +} + +void print_attitude(void) +{ + DTSTREAM.print_P("Att: "); + DTSTREAM.print(radio_in[CH_ROLL],DEC); // 0 + DTSTREAM.print_P(" roll in, "); + DTSTREAM.print(radio_in[CH_PITCH],DEC); // 1 + DTSTREAM.print_P(" pitch in, "); + DTSTREAM.print(radio_in[CH_THROTTLE],DEC); // 2 + DTSTREAM.print_P(" throttle in, "); + DTSTREAM.print(roll_sensor,DEC); // 3 + DTSTREAM.print_P(" roll sensor, "); + DTSTREAM.print(pitch_sensor,DEC); // 4 + DTSTREAM.print_P(" pitch sensor, "); + DTSTREAM.print(GPS.ground_course,DEC); // 6 + DTSTREAM.print_P(" ground course, "); + DTSTREAM.print(target_bearing,DEC); // 7 + DTSTREAM.print_P(" target bearing, "); + DTSTREAM.print(nav_roll,DEC); // 8 + DTSTREAM.print_P(" nav roll, "); + DTSTREAM.print(loiter_sum,DEC); // 8 + DTSTREAM.print_P(" loiter sum, "); + DTSTREAM.print(servo_out[CH_ROLL],DEC); + DTSTREAM.print_P(" roll servo_out, "); + DTSTREAM.print(servo_out[CH_PITCH],DEC); + DTSTREAM.println_P(" pitch servo_out"); +} + +// required by Groundstation to plot lateral tracking course +void print_new_wp_info() +{ + DTSTREAM.print_P("New waypt ("); + DTSTREAM.print(wp_index,DEC); //0 + DTSTREAM.print_P("): "); + DTSTREAM.print(prev_WP.lat,DEC); //1 + DTSTREAM.print_P(", "); //° + DTSTREAM.print(prev_WP.lng,DEC); //2 + DTSTREAM.print_P(", "); + DTSTREAM.print(prev_WP.alt,DEC); //3 + DTSTREAM.print_P("m -> "); + DTSTREAM.print(next_WP.lat,DEC); //4 + DTSTREAM.print_P("°, "); + DTSTREAM.print(next_WP.lng,DEC); //5 + DTSTREAM.print_P("°, "); + DTSTREAM.print(next_WP.alt,DEC); //6 + DTSTREAM.print_P("m; "); + DTSTREAM.print(wp_totalDistance,DEC); //7 + DTSTREAM.print_P("m dist, "); + DTSTREAM.print(radio_trim[CH_ROLL],DEC); //8 + DTSTREAM.print_P(" roll trim, "); + DTSTREAM.print(radio_trim[CH_PITCH],DEC); //9 + DTSTREAM.println_P(" pitch trim"); +} + +void print_control_mode(void) +{ + switch (control_mode){ + case MANUAL: + DTSTREAM.println_P("##MANUAL"); + break; + case STABILIZE: + DTSTREAM.println_P("##STABILIZE"); + break; + case FLY_BY_WIRE_A: + DTSTREAM.println_P("##FBW A"); + break; + case FLY_BY_WIRE_B: + DTSTREAM.println_P("##FBW B"); + break; + case AUTO: + DTSTREAM.println_P("##AUTO"); + break; + case RTL: + DTSTREAM.println_P("##RTL"); + break; + case LOITER: + DTSTREAM.println_P("##LOITER"); + break; + case 98: + DTSTREAM.println_P("##Air Start Complete"); + break; + case 99: + DTSTREAM.println_P("##Ground Start Complete"); + break; + } +} + +void print_tuning(void) { + DTSTREAM.print_P("TUN:"); + DTSTREAM.print(servo_out[CH_ROLL]); + DTSTREAM.print_P(", "); + DTSTREAM.print(nav_roll/100,DEC); + DTSTREAM.print_P(", "); + DTSTREAM.print(roll_sensor/100,DEC); + DTSTREAM.print_P(", "); + DTSTREAM.print(servo_out[CH_PITCH]); + DTSTREAM.print_P(", "); + DTSTREAM.print(nav_pitch/100,DEC); + DTSTREAM.print_P(", "); + DTSTREAM.println(pitch_sensor/100,DEC); +} + +void printPerfData(void) +{ +} + + +void print_waypoint(struct Location *cmd,byte index) +{ + //TODO: Why does this stop printing in the middle?? --BJP + DTSTREAM.print_P(" command #: "); + DTSTREAM.print(index, DEC); + DTSTREAM.print_P(" id: "); + switch (cmd->id) { + // Command IDs - Must + case CMD_BLANK: DTSTREAM.print_P("CMD_BLANK"); break; + case CMD_WAYPOINT: DTSTREAM.print_P("CMD_WAYPOINT"); break; + case CMD_LOITER: DTSTREAM.print_P("CMD_LOITER"); break; + case CMD_LOITER_N_TURNS: DTSTREAM.print_P("CMD_LOITER_N_TURNS"); break; + case CMD_LOITER_TIME: DTSTREAM.print_P("CMD_LOITER_TIME"); break; + case CMD_RTL: DTSTREAM.print_P("CMD_RTL"); break; + case CMD_LAND: DTSTREAM.print_P("CMD_LAND"); break; + case CMD_TAKEOFF: DTSTREAM.print_P("CMD_TAKEOFF"); break; + + // Command IDs - May + case CMD_DELAY: DTSTREAM.print_P("CMD_DELAY"); break; + case CMD_CLIMB: DTSTREAM.print_P("CMD_CLIMB"); break; + case CMD_LAND_OPTIONS: DTSTREAM.print_P("CMD_LAND_OPTIONS"); break; + + // Command IDs - Now + case CMD_RESET_INDEX: DTSTREAM.print_P("CMD_RESET_INDEX"); break; + case CMD_GOTO_INDEX: DTSTREAM.print_P("CMD_GOTO_INDEX"); break; + case CMD_GETVAR_INDEX: DTSTREAM.print_P("CMD_GETVAR_INDEX"); break; + case CMD_SENDVAR_INDEX: DTSTREAM.print_P("CMD_SENDVAR_INDEX"); break; + case CMD_TELEMETRY: DTSTREAM.print_P("CMD_TELEMETRY"); break; + + case CMD_THROTTLE_CRUISE: DTSTREAM.print_P("CMD_THROTTLE_CRUISE"); break; + case CMD_AIRSPEED_CRUISE: DTSTREAM.print_P("CMD_AIRSPEED_CRUISE"); break; + case CMD_RESET_HOME: DTSTREAM.print_P("CMD_RESET_HOME"); break; + + case CMD_KP_GAIN: DTSTREAM.print_P("CMD_KP_GAIN"); break; + case CMD_KI_GAIN: DTSTREAM.print_P("CMD_KI_GAIN"); break; + case CMD_KD_GAIN: DTSTREAM.print_P("CMD_KD_GAIN"); break; + case CMD_KI_MAX: DTSTREAM.print_P("CMD_KI_MAX"); break; + case CMD_KFF_GAIN: DTSTREAM.print_P("CMD_KFF_GAIN"); break; + + case CMD_RADIO_TRIM: DTSTREAM.print_P("CMD_RADIO_TRIM"); break; + case CMD_RADIO_MAX: DTSTREAM.print_P("CMD_RADIO_MAX"); break; + case CMD_RADIO_MIN: DTSTREAM.print_P("CMD_RADIO_MIN"); break; + case CMD_ELEVON_TRIM: DTSTREAM.print_P("CMD_ELEVON_TRIM"); break; + + case CMD_INDEX: DTSTREAM.print_P("CMD_INDEX"); break; + case CMD_REPEAT: DTSTREAM.print_P("CMD_REPEAT"); break; + case CMD_RELAY: DTSTREAM.print_P("CMD_RELAY"); break; + case CMD_SERVO: DTSTREAM.print_P("CMD_SERVO"); break; + + default: DTSTREAM.print(cmd->id,DEC); + } + DTSTREAM.print_P(" p1: "); + DTSTREAM.print(cmd->p1,DEC); + DTSTREAM.print_P(" p2/alt: "); + DTSTREAM.print(cmd->alt,DEC); + DTSTREAM.print_P(" p3/lat: "); + DTSTREAM.print(cmd->lat,DEC); + DTSTREAM.print_P(" p4/lng: "); + DTSTREAM.println(cmd->lng,DEC); +} + +void print_waypoints() +{ + DTSTREAM.println_P("Commands in memory:"); + DTSTREAM.print_P(" "); + DTSTREAM.print(wp_total, DEC); + DTSTREAM.println_P(" commands total"); + // create a location struct to hold the temp Waypoints for printing + //Location tmp; + DTSTREAM.println_P(" Home: "); + struct Location cmd = get_wp_with_index(0); + print_waypoint(&cmd, 0); + DTSTREAM.println_P(" Commands: "); + + for (int i=1; i= DEBUG_LEVEL){ + Serial.print("MSG: "); + Serial.println(str); + } +} + +void print_current_waypoints() +{ +} + +void print_control_mode(void) +{ +} + +void print_attitude(void) +{ + //Serial.print("!!!VER:"); + //Serial.print(SOFTWARE_VER); //output the software version + //Serial.print(","); + +// Analogs + Serial.print("AN0:"); + Serial.print(read_adc(0)); //Reversing the sign. + Serial.print(",AN1:"); + Serial.print(read_adc(1)); + Serial.print(",AN2:"); + Serial.print(read_adc(2)); + Serial.print(",AN3:"); + Serial.print(read_adc(3)); + Serial.print (",AN4:"); + Serial.print(read_adc(4)); + Serial.print (",AN5:"); + Serial.print(read_adc(5)); + Serial.print (","); + +// DCM + Serial.print ("EX0:"); + Serial.print(convert_to_dec(DCM_Matrix[0][0])); + Serial.print (",EX1:"); + Serial.print(convert_to_dec(DCM_Matrix[0][1])); + Serial.print (",EX2:"); + Serial.print(convert_to_dec(DCM_Matrix[0][2])); + Serial.print (",EX3:"); + Serial.print(convert_to_dec(DCM_Matrix[1][0])); + Serial.print (",EX4:"); + Serial.print(convert_to_dec(DCM_Matrix[1][1])); + Serial.print (",EX5:"); + Serial.print(convert_to_dec(DCM_Matrix[1][2])); + Serial.print (",EX6:"); + Serial.print(convert_to_dec(DCM_Matrix[2][0])); + Serial.print (",EX7:"); + Serial.print(convert_to_dec(DCM_Matrix[2][1])); + Serial.print (",EX8:"); + Serial.print(convert_to_dec(DCM_Matrix[2][2])); + Serial.print (","); + +// Euler + Serial.print("RLL:"); + Serial.print(ToDeg(roll)); + Serial.print(",PCH:"); + Serial.print(ToDeg(pitch)); + Serial.print(",YAW:"); + Serial.print(ToDeg(yaw)); + Serial.print(",IMUH:"); + Serial.print(((int)imu_health>>8)&0xff); + Serial.print (","); + + + /* + #if MAGNETOMETER == ENABLED + Serial.print("MGX:"); + Serial.print(magnetom_x); + Serial.print (",MGY:"); + Serial.print(magnetom_y); + Serial.print (",MGZ:"); + Serial.print(magnetom_z); + Serial.print (",MGH:"); + Serial.print(MAG_Heading); + Serial.print (","); + #endif + */ + + //Serial.print("Temp:"); + //Serial.print(temp_unfilt/20.0); // Convert into degrees C + //alti(); + //Serial.print(",Pressure: "); + //Serial.print(press); + //Serial.print(",Alt: "); + //Serial.print(press_alt/1000); // Original floating point full solution in meters + //Serial.print (","); + Serial.println("***"); +} + +void print_location(void) +{ + Serial.print("LAT:"); + Serial.print(current_loc.lat); + Serial.print(",LON:"); + Serial.print(current_loc.lng); + Serial.print(",ALT:"); + Serial.print(current_loc.alt/100); // meters + Serial.print(",COG:"); + Serial.print(GPS.ground_course); + Serial.print(",SOG:"); + Serial.print(GPS.ground_speed); + Serial.print(",FIX:"); + Serial.print((int)GPS.fix); + Serial.print(",SAT:"); + Serial.print((int)GPS.num_sats); + Serial.print (","); + Serial.print("TOW:"); + Serial.print(GPS.time); + Serial.println("***"); +} + +void print_waypoints() +{ +} + +void print_waypoint(struct Location *cmd,byte index) +{ + Serial.print("command #: "); + Serial.print(index, DEC); + Serial.print(" id: "); + Serial.print(cmd->id,DEC); + Serial.print(" p1: "); + Serial.print(cmd->p1,DEC); + Serial.print(" p2: "); + Serial.print(cmd->alt,DEC); + Serial.print(" p3: "); + Serial.print(cmd->lat,DEC); + Serial.print(" p4: "); + Serial.println(cmd->lng,DEC); + +} + +long convert_to_dec(float x) +{ + return x*10000000; +} + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde new file mode 100644 index 0000000000..37c9de372c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde @@ -0,0 +1,238 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + + +#if GCS_PROTOCOL == GCS_PROTOCOL_JASON + +// this is my personal GCS - Jason + + +void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 +{ + if(severity >= DEBUG_LEVEL){ + Serial.print("MSG: "); + Serial.println(str); + } +} + +void send_message(byte id) +{ + send_message(id,(long)0); +} + +void send_message(byte id, long param) +{ + + switch(id) { + case MSG_HEARTBEAT: + print_control_mode(); + break; + + case MSG_ATTITUDE: + print_attitude(); + break; + + case MSG_LOCATION: + print_position(); + break; + + case MSG_COMMAND: + struct Location cmd = get_wp_with_index(param); + print_waypoint(&cmd, param); + break; + + } +} + +void pipe() +{ + Serial.print("|"); +} + +void print_current_waypoints() +{ + Serial.print("MSG: "); + + Serial.print("CUR:"); + Serial.print("\t"); + Serial.print(current_loc.lat,DEC); + Serial.print(",\t"); + Serial.print(current_loc.lng,DEC); + Serial.print(",\t"); + Serial.println(current_loc.alt,DEC); + + Serial.print("NWP:"); + Serial.print(wp_index,DEC); + Serial.print(",\t"); + Serial.print(next_WP.lat,DEC); + Serial.print(",\t"); + Serial.print(next_WP.lng,DEC); + Serial.print(",\t"); + Serial.println(next_WP.alt,DEC); +} + +void print_position(void) +{ + Serial.print("!!"); + Serial.print(current_loc.lat,DEC); // 0 + pipe(); + Serial.print(current_loc.lng,DEC); // 1 + pipe(); + Serial.print(current_loc.alt,DEC); // 2 + pipe(); + Serial.print(GPS.ground_speed,DEC); // 3 + pipe(); + Serial.print(airspeed,DEC); // 4 + pipe(); + Serial.print(get_altitude_above_home(),DEC); // 5 + pipe(); + Serial.print(climb_rate,DEC); // 6 + pipe(); + Serial.print(wp_distance,DEC); // 7 + pipe(); + Serial.print(throttle_cruise,DEC); // 8 + pipe(); + Serial.println(altitude_error,DEC); // 9 + +} + +void print_attitude(void) +{ + //return; + + Serial.print("++"); + Serial.print(radio_in[0],DEC); // 0 + pipe(); + Serial.print(radio_in[1],DEC); // 1 + pipe(); + Serial.print(radio_in[CH_THROTTLE],DEC); // 2 + pipe(); + Serial.print(roll_sensor,DEC); // 3 + pipe(); + Serial.print(pitch_sensor,DEC); // 4 + pipe(); + Serial.print("0"); // 5 ir_max - removed, no longer applicable + pipe(); + Serial.print(yaw_sensor,DEC); // 6 + pipe(); + Serial.print(target_bearing,DEC); // 7 + pipe(); + Serial.print(nav_roll,DEC); // 8 + pipe(); + Serial.println(loiter_sum,DEC); // 8 + +} + +// required by Groundstation to plot lateral tracking course +void print_new_wp_info() +{ + Serial.print("??"); + Serial.print(wp_index,DEC); //0 + pipe(); + Serial.print(prev_WP.lat,DEC); //1 + pipe(); + Serial.print(prev_WP.lng,DEC); //2 + pipe(); + Serial.print(prev_WP.alt,DEC); //3 + pipe(); + Serial.print(next_WP.lat,DEC); //4 + pipe(); + Serial.print(next_WP.lng,DEC); //5 + pipe(); + Serial.print(next_WP.alt,DEC); //6 + pipe(); + Serial.print(wp_totalDistance,DEC); //7 + pipe(); + Serial.print(radio_trim[CH_ROLL],DEC); //8 + pipe(); + Serial.println(radio_trim[CH_PITCH],DEC); //9 +} + +void print_control_mode(void) +{ + switch (control_mode){ + case MANUAL: + Serial.println("##MANUAL"); + break; + case STABILIZE: + Serial.println("##STABILIZE"); + break; + case FLY_BY_WIRE_A: + Serial.println("##FBW A"); + break; + case FLY_BY_WIRE_B: + Serial.println("##FBW B"); + break; + case AUTO: + Serial.println("##AUTO"); + break; + case RTL: + Serial.println("##RTL"); + break; + case LOITER: + Serial.println("##LOITER"); + break; + case 98: + Serial.println("##Air Start Complete"); + break; + case 99: + Serial.println("##Ground Start Complete"); + break; + } +} + +void print_tuning(void) { + Serial.print("TUN:"); + Serial.print(servo_out[CH_ROLL]/100); + Serial.print(", "); + Serial.print(nav_roll/100,DEC); + Serial.print(", "); + Serial.print(roll_sensor/100,DEC); + Serial.print(", "); + Serial.print(servo_out[CH_PITCH]/100); + Serial.print(", "); + Serial.print(nav_pitch/100,DEC); + Serial.print(", "); + Serial.println(pitch_sensor/100,DEC); +} + +void printPerfData(void) +{ +} + + +void print_waypoint(struct Location *cmd,byte index) +{ + Serial.print("command #: "); + Serial.print(index, DEC); + Serial.print(" id: "); + Serial.print(cmd->id,DEC); + Serial.print(" p1: "); + Serial.print(cmd->p1,DEC); + Serial.print(" p2: "); + Serial.print(cmd->alt,DEC); + Serial.print(" p3: "); + Serial.print(cmd->lat,DEC); + Serial.print(" p4: "); + Serial.println(cmd->lng,DEC); +} + +void print_waypoints() +{ + Serial.println("Commands in memory"); + Serial.print("commands total: "); + Serial.println(wp_total, DEC); + // create a location struct to hold the temp Waypoints for printing + //Location tmp; + Serial.println("Home: "); + struct Location cmd = get_wp_with_index(0); + print_waypoint(&cmd, 0); + Serial.println("Commands: "); + + for (int i=1; i> 8) & 0xff; + tempint = battery_voltage1 * 100; // Battery voltage ( * 100) + mess_buffer[6] = tempint & 0xff; + mess_buffer[7] = (tempint >> 8) & 0xff; + tempint = command_must_index; // Command Index (waypoint level) + mess_buffer[8] = tempint & 0xff; + mess_buffer[9] = (tempint >> 8) & 0xff; + break; + + case MSG_ATTITUDE: // ** Attitude message + mess_buffer[0] = 0x06; + ck = 6; + tempint = roll_sensor; // Roll (degrees * 100) + mess_buffer[3] = tempint & 0xff; + mess_buffer[4] = (tempint >> 8) & 0xff; + tempint = pitch_sensor; // Pitch (degrees * 100) + mess_buffer[5] = tempint & 0xff; + mess_buffer[6] = (tempint >> 8) & 0xff; + tempint = yaw_sensor; // Yaw (degrees * 100) + mess_buffer[7] = tempint & 0xff; + mess_buffer[8] = (tempint >> 8) & 0xff; + break; + + case MSG_LOCATION: // ** Location / GPS message + mess_buffer[0] = 0x12; + ck = 18; + templong = current_loc.lat; // Latitude *10 * *7 in 4 bytes + mess_buffer[3] = templong & 0xff; + mess_buffer[4] = (templong >> 8) & 0xff; + mess_buffer[5] = (templong >> 16) & 0xff; + mess_buffer[6] = (templong >> 24) & 0xff; + + templong = current_loc.lng; // Longitude *10 * *7 in 4 bytes + mess_buffer[7] = templong & 0xff; + mess_buffer[8] = (templong >> 8) & 0xff; + mess_buffer[9] = (templong >> 16) & 0xff; + mess_buffer[10] = (templong >> 24) & 0xff; + + tempint = GPS.altitude / 100; // Altitude MSL in meters * 10 in 2 bytes + mess_buffer[11] = tempint & 0xff; + mess_buffer[12] = (tempint >> 8) & 0xff; + + tempint = GPS.ground_speed; // Speed in M / S * 100 in 2 bytes + mess_buffer[13] = tempint & 0xff; + mess_buffer[14] = (tempint >> 8) & 0xff; + + tempint = yaw_sensor; // Ground Course in degreees * 100 in 2 bytes + mess_buffer[15] = tempint & 0xff; + mess_buffer[16] = (tempint >> 8) & 0xff; + + templong = GPS.time; // Time of Week (milliseconds) in 4 bytes + mess_buffer[17] = templong & 0xff; + mess_buffer[18] = (templong >> 8) & 0xff; + mess_buffer[19] = (templong >> 16) & 0xff; + mess_buffer[20] = (templong >> 24) & 0xff; + break; + + case MSG_PRESSURE: // ** Pressure message + mess_buffer[0] = 0x04; + ck = 4; + tempint = current_loc.alt / 10; // Altitude MSL in meters * 10 in 2 bytes + mess_buffer[3] = tempint & 0xff; + mess_buffer[4] = (tempint >> 8) & 0xff; + tempint = (int)airspeed / 100; // Airspeed pressure + mess_buffer[5] = tempint & 0xff; + mess_buffer[6] = (tempint >> 8) & 0xff; + break; + +// case 0xMSG_STATUS_TEXT: // ** Status Text message +// mess_buffer[0]=sizeof(status_message[0])+1; +// ck=mess_buffer[0]; +// mess_buffer[2] = param&0xff; +// for (int i=3;i> 8) & 0xff; + mess_buffer[5] = (templong >> 16) & 0xff; + mess_buffer[6] = (templong >> 24) & 0xff; + tempint = mainLoop_count; // Main Loop cycles + mess_buffer[7] = tempint & 0xff; + mess_buffer[8] = (tempint >> 8) & 0xff; + mess_buffer[9] = G_Dt_max & 0xff; + mess_buffer[10] = gyro_sat_count; // Problem counts + mess_buffer[11] = adc_constraints; + mess_buffer[12] = renorm_sqrt_count; + mess_buffer[13] = renorm_blowup_count; + mess_buffer[14] = gps_fix_count; + tempint = (int)(imu_health * 1000); // IMU health metric + mess_buffer[15] = tempint & 0xff; + mess_buffer[16] = (tempint >> 8) & 0xff; + tempint = gcs_messages_sent; // GCS messages sent + mess_buffer[17] = tempint & 0xff; + mess_buffer[18] = (tempint >> 8) & 0xff; + break; + + case MSG_VALUE: // ** Requested Value message + mess_buffer[0] = 0x06; + ck = 6; + templong = param; // Parameter being sent + mess_buffer[3] = templong & 0xff; + mess_buffer[4] = (templong >> 8) & 0xff; + switch(param) { +// case 0x00: templong = roll_mode; break; +// case 0x01: templong = pitch_mode; break; + case 0x02: templong = yaw_mode; break; +// case 0x03: templong = throttle_mode; break; + case 0x04: templong = elevon1_trim; break; + case 0x05: templong = elevon2_trim; break; + case 0x10: templong = integrator[0] * 1000; break; + case 0x11: templong = integrator[1] * 1000; break; + case 0x12: templong = integrator[2] * 1000; break; + case 0x13: templong = integrator[3] * 1000; break; + case 0x14: templong = integrator[4] * 1000; break; + case 0x15: templong = integrator[5] * 1000; break; + case 0x16: templong = integrator[6] * 1000; break; + case 0x17: templong = integrator[7] * 1000; break; + case 0x1a: templong = kff[0]; break; + case 0x1b: templong = kff[1]; break; + case 0x1c: templong = kff[2]; break; + case 0x20: templong = target_bearing; break; + case 0x21: templong = nav_bearing; break; + case 0x22: templong = bearing_error; break; + case 0x23: templong = crosstrack_bearing; break; + case 0x24: templong = crosstrack_error; break; + case 0x25: templong = altitude_error; break; + case 0x26: templong = wp_radius; break; + case 0x27: templong = loiter_radius; break; +// case 0x28: templong = wp_mode; break; +// case 0x29: templong = loop_commands; break; + case 0x2a: templong = nav_gain_scaler; break; + } + mess_buffer[5] = templong & 0xff; + mess_buffer[6] = (templong >> 8) & 0xff; + mess_buffer[7] = (templong >> 16) & 0xff; + mess_buffer[8] = (templong >> 24) & 0xff; + break; + + case MSG_COMMAND: // Command list item message + mess_buffer[0] = 0x10; + ck = 16; + tempint = param; // item number + mess_buffer[3] = tempint & 0xff; + mess_buffer[4] = (tempint >> 8) & 0xff; + tempint = wp_total; // list length (# of commands in mission) + mess_buffer[5] = tempint & 0xff; + mess_buffer[6] = (tempint >> 8) & 0xff; + tell_command = get_wp_with_index((int)param); + mess_buffer[7] = tell_command.id; // command id + mess_buffer[8] = tell_command.p1; // P1 + tempint = tell_command.alt; // P2 + mess_buffer[9] = tempint & 0xff; + mess_buffer[10] = (tempint >> 8) & 0xff; + templong = tell_command.lat; // P3 + mess_buffer[11] = templong & 0xff; + mess_buffer[12] = (templong >> 8) & 0xff; + mess_buffer[13] = (templong >> 16) & 0xff; + mess_buffer[14] = (templong >> 24) & 0xff; + templong = tell_command.lng; // P4 + mess_buffer[15] = templong & 0xff; + mess_buffer[16] = (templong >> 8) & 0xff; + mess_buffer[17] = (templong >> 16) & 0xff; + mess_buffer[18] = (templong >> 24) & 0xff; + break; + + case MSG_TRIMS: // Radio Trims message + mess_buffer[0] = 0x10; + ck = 16; + for(int i = 0; i < 8; i++) { + tempint = radio_trim[i]; // trim values + mess_buffer[3 + 2 * i] = tempint & 0xff; + mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; + } + break; + + case MSG_MINS: // Radio Mins message + mess_buffer[0] = 0x10; + ck = 16; + for(int i = 0; i < 8; i++) { + tempint = radio_min[i]; // min values + mess_buffer[3 + 2 * i] = tempint & 0xff; + mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; + } + break; + + case MSG_MAXS: // Radio Maxs message + mess_buffer[0] = 0x10; + ck = 16; + for(int i = 0; i < 8; i++) { + tempint = radio_max[i]; // max values + mess_buffer[3 + 2 * i] = tempint & 0xff; + mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; + } + break; + + case MSG_PID: // PID Gains message + mess_buffer[0] = 0x0f; + ck = 15; + mess_buffer[3] = param & 0xff; // PID set # + templong = (kp[param] * 1000000); // P gain + mess_buffer[4] = templong & 0xff; + mess_buffer[5] = (templong >> 8) & 0xff; + mess_buffer[6] = (templong >> 16) & 0xff; + mess_buffer[7] = (templong >> 24) & 0xff; + templong = (ki[param] * 1000000); // I gain + mess_buffer[8] = templong & 0xff; + mess_buffer[9] = (templong >> 8) & 0xff; + mess_buffer[10] = (templong >> 16) & 0xff; + mess_buffer[11] = (templong >> 24) & 0xff; + templong = (kd[param] * 1000000); // D gain + mess_buffer[12] = templong & 0xff; + mess_buffer[13] = (templong >> 8) & 0xff; + mess_buffer[14] = (templong >> 16) & 0xff; + mess_buffer[15] = (templong >> 24) & 0xff; + tempint = integrator_max[param]; // Integrator max value + mess_buffer[16] = tempint & 0xff; + mess_buffer[17] = (tempint >> 8) & 0xff; + break; + } + + mess_buffer[1] = id; // Message ID + mess_buffer[2] = 0x01; // Message version + + for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); + + for (int i = 0; i < ck + 3; i++) { + mess_ck_a += mess_buffer[i]; // Calculates checksums + mess_ck_b += mess_ck_a; + } + + SendSer(mess_ck_a); + SendSer(mess_ck_b); + + gcs_messages_sent++; +} + +void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT +{ + if(severity >= DEBUG_LEVEL){ + byte length = strlen(str) + 1; + + byte mess_buffer[54]; + byte mess_ck_a = 0; + byte mess_ck_b = 0; + int ck; + + SendSer("4D"); // This is the message preamble + if(length > 50) length = 50; + mess_buffer[0] = length; + ck = length; + mess_buffer[1] = 0x05; // Message ID + mess_buffer[2] = 0x01; // Message version + + mess_buffer[3] = severity; + + for (int i = 3; i < ck + 2; i++) + mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+ + + for (int i = 0; i < ck + 3; i++) + SendSer(mess_buffer[i]); + + for (int i = 0; i < ck + 3; i++) { + mess_ck_a += mess_buffer[i]; // Calculates checksums + mess_ck_b += mess_ck_a; + } + SendSer(mess_ck_a); + SendSer(mess_ck_b); + } +} + +void print_current_waypoints() +{ +} + +void print_waypoint(struct Location *cmd, byte index) +{ + Serial.print("command #: "); + Serial.print(index, DEC); + Serial.print(" id: "); + Serial.print(cmd->id, DEC); + Serial.print(" p1: "); + Serial.print(cmd->p1, DEC); + Serial.print(" p2: "); + Serial.print(cmd->alt, DEC); + Serial.print(" p3: "); + Serial.print(cmd->lat, DEC); + Serial.print(" p4: "); + Serial.println(cmd->lng, DEC); +} + +void print_waypoints() +{ +} + + +#endif + +#if GCS_PROTOCOL == GCS_PROTOCOL_NONE +void acknowledge(byte id, byte check1, byte check2) {} +void send_message(byte id) {} +void send_message(byte id, long param) {} +void send_message(byte severity, const char *str) {} +void print_current_waypoints(){} +void print_waypoint(struct Location *cmd, byte index){} +void print_waypoints(){} +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde new file mode 100644 index 0000000000..6f99c2efc1 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde @@ -0,0 +1,9 @@ +#if GCS_PROTOCOL == GCS_PROTOCOL_XDIY +void acknowledge(byte id, byte check1, byte check2) {} +void send_message(byte id) {} +void send_message(byte id, long param) {} +void send_message(byte severity, const char *str) {} +void print_current_waypoints(){} +void print_waypoint(struct Location *cmd, byte index){} +void print_waypoints(){} +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde new file mode 100644 index 0000000000..e6f4c2f235 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde @@ -0,0 +1,127 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE + + +void acknowledge(byte id, byte check1, byte check2) +{ +} + +void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 +{ + if(severity >= DEBUG_LEVEL){ + Serial.print("MSG: "); + Serial.println(str); + } +} + +void send_message(byte id) { + send_message(id,0l); +} + +void send_message(byte id, long param) { + switch(id) { + case MSG_HEARTBEAT: + print_control_mode(); + break; + + case MSG_ATTITUDE: + //print_attitude(); + break; + + case MSG_LOCATION: + //print_position(); + break; + + case MSG_COMMAND: + struct Location cmd = get_wp_with_index(param); + print_waypoint(&cmd, param); + break; + + } +} + + +// required by Groundstation to plot lateral tracking course +void print_new_wp_info() +{ +} + +void print_control_mode(void) +{ + Serial.print("MSG: "); + Serial.print(flight_mode_strings[control_mode]); +} + + +void print_current_waypoints() +{ + Serial.print("MSG: "); + Serial.print("CUR:"); + Serial.print("\t"); + Serial.print(current_loc.lat,DEC); + Serial.print(",\t"); + Serial.print(current_loc.lng,DEC); + Serial.print(",\t"); + Serial.println(current_loc.alt,DEC); + + Serial.print("MSG: "); + Serial.print("NWP:"); + Serial.print(wp_index,DEC); + Serial.print(",\t"); + Serial.print(next_WP.lat,DEC); + Serial.print(",\t"); + Serial.print(next_WP.lng,DEC); + Serial.print(",\t"); + Serial.println(next_WP.alt,DEC); +} + +void print_position(void) +{ +} + +void printPerfData(void) +{ +} + +void print_attitude(void) +{ +} +void print_tuning(void) { +} +void print_waypoint(struct Location *cmd,byte index) +{ + Serial.print("MSG: command #: "); + Serial.print(index, DEC); + Serial.print(" id: "); + Serial.print(cmd->id,DEC); + Serial.print(" p1: "); + Serial.print(cmd->p1,DEC); + Serial.print(" p2: "); + Serial.print(cmd->alt,DEC); + Serial.print(" p3: "); + Serial.print(cmd->lat,DEC); + Serial.print(" p4: "); + Serial.println(cmd->lng,DEC); +} + +void print_waypoints() +{ + Serial.println("Commands in memory"); + Serial.print("commands total: "); + Serial.println(wp_total, DEC); + // create a location struct to hold the temp Waypoints for printing + //Location tmp; + Serial.println("Home: "); + struct Location cmd = get_wp_with_index(0); + print_waypoint(&cmd, 0); + Serial.println("Commands: "); + + for (int i=1; i> 8) & 0xff; +} + +void output_byte(byte value) +{ + out_buffer[buf_len++] = value; +} + +void print_buffer() +{ + byte ck_a = 0; + byte ck_b = 0; + for (byte i = 0;i < buf_len; i++){ + Serial.print (out_buffer[i]); + } + Serial.print('\r'); + Serial.print('\n'); + buf_len = 0; +} + + +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde new file mode 100644 index 0000000000..11dfcc9b39 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde @@ -0,0 +1,264 @@ +// menu structure +// +// [status] - [data1] - [data2] - [data3] - [data4] - [dataXY] +// |_________| _______|_________|_________|_________| +// | +// [SETUP1] - [SETUP2] - [SETUPXY] +// | | | +// [para1.1] [PARAM2.1] [PARAMX.Y] +// | | | +// [para1.2] [PARAM2.2] [PARAMX.Y] +// | | | +// [para1.3] [PARAM2.3] [PARAMX.Y] + +// Statusmenu +const uint8_t mnuSTATUS = 0; +const uint8_t mnuGPS1 = 1; +const uint8_t mnuGPS2 = 2; +const uint8_t mnuIMU = 3; +const uint8_t mnuACC = 4; +const uint8_t mnuGYRO = 5; +const uint8_t mnuCOMPASS = 6; +const uint8_t mnuSERVO1 = 7; +const uint8_t mnuSERVO2 = 8; +const uint8_t mnuDATA9 = 9; + +// Setup 1 +const uint8_t mnuSETUP1 = 10; +const uint8_t mnuPARAM11 = 11; +const uint8_t mnuPARAM12 = 12; +const uint8_t mnuPARAM13 = 13; +const uint8_t mnuPARAM14 = 14; + +// Setup 2 +const uint8_t mnuSETUP2 = 20; +const uint8_t mnuPARAM21 = 21; +const uint8_t mnuPARAM22 = 22; +const uint8_t mnuPARAM23 = 23; +const uint8_t mnuPARAM24 = 24; + +// Setup 3 +const uint8_t mnuSETUP3 = 30; +const uint8_t mnuPARAM31 = 31; +const uint8_t mnuPARAM32 = 32; +const uint8_t mnuPARAM33 = 33; +const uint8_t mnuPARAM34 = 34; + +// Setup 4 +const uint8_t mnuSETUP4 = 40; +const uint8_t mnuPARAM41 = 41; +const uint8_t mnuPARAM42 = 42; +const uint8_t mnuPARAM43 = 43; +const uint8_t mnuPARAM44 = 44; + +#define LCDMaxPos 32 + +static uint8_t MnuPtr = 0; +static uint8_t statustxt[LCDMaxPos]; + +void jeti_status(const char *str) +{ + byte i; + byte length = strlen(str) + 1; + if (length > LCDMaxPos) length = LCDMaxPos; + for (i = 0; i fill up with SPACE + } + } +} + +void jeti_menuctrl(uint8_t btns) { + // Algorithm for menubuttons + // if MnuPtr == 0 then Status + // if MnuPtr <10 then Data + // if (MnuPtr mod 10) = 0 then Setupp + // if (MnuPtr mod 10) <> 0 then Parameter + + if (MnuPtr == mnuSTATUS) { // MnuPtr = Status + switch (btns) { + case JB_key_right: + MnuPtr += 1; + break; + + case JB_key_left: + MnuPtr = mnuDATA9; + break; + + case JB_key_down: + MnuPtr = mnuSETUP1; + break; + } + } + else if (MnuPtr < 10) { // MnuPtr = Data + switch (btns) { + case JB_key_right: + if (MnuPtr < mnuDATA9) MnuPtr += 1; else MnuPtr = mnuSTATUS; + break; + + case JB_key_left: + if (MnuPtr > mnuSTATUS) MnuPtr -= 1; else MnuPtr = mnuDATA9; + break; + } + } + else if (MnuPtr % 10 == 0) { // MnuPtr = Setup + switch (btns) { + case JB_key_right: + if (MnuPtr < mnuSETUP4) MnuPtr += 10; else MnuPtr = mnuSETUP1; + break; + + case JB_key_left: + if (MnuPtr > mnuSETUP1) MnuPtr -= 10; else MnuPtr = mnuSETUP4; + break; + + case JB_key_up: + MnuPtr = mnuSTATUS; + break; + + case JB_key_down: + MnuPtr += 1; + break; + } + } + else { // MnuPtr = Parameter + switch (btns) { + case JB_key_down: + if ((MnuPtr % 10) < 4) MnuPtr += 1; + break; + + case JB_key_up: + if ((MnuPtr % 10) > 0) MnuPtr -= 1; + break; + } + } +} + +void jeti_init() { + // Init Jeti Serial Port + JB.begin(); +} + +void jeti_update() { + JB.clear(0); + uint8_t Buttons = JB.readbuttons(); + jeti_menuctrl(Buttons); + switch (MnuPtr) { + case mnuGPS1: + JB.print("GPS> "); + if (GPS.fix == 1) { + JB.print("FIX"); + JB.print(" SAT>"); + JB.print((int)GPS.num_sats); + JB.setcursor(LCDLine2); + JB.print("Alt: "); + JB.print((int)GPS.altitude/100); + } else { + JB.print("no FIX"); + } + break; + + case mnuGPS2: + if (GPS.fix ==1 ) { + JB.print("Lat "); + JB.print((float)GPS.latitude/10000000, 9); + JB.setcursor(LCDLine2); + JB.print("Lon "); + JB.print((float)GPS.longitude/10000000, 9); + } else { + JB.print("no Data avail."); + } + break; + + case mnuIMU: + //read_AHRS(); + JB.print("IMU> R:"); + JB.print(roll_sensor/100,DEC); + JB.print(" P:"); + JB.print(pitch_sensor/100,DEC); + JB.setcursor(LCDLine2); + JB.print("Y:"); + JB.print(yaw_sensor/100,DEC); + break; + + case mnuACC: + for (int i = 0; i < 6; i++) { + AN[i] = APM_ADC.Ch(sensors[i]); + } + JB.print("ACC> X:"); + JB.print((int)AN[3]); + JB.setcursor(LCDLine2); + JB.print(" Y:"); + JB.print((int)AN[4]); + JB.print(" Z:"); + JB.print((int)AN[5]); + break; + + case mnuGYRO: + for (int i = 0; i < 6; i++) { + AN[i] = APM_ADC.Ch(sensors[i]); + } + JB.print("GYRO> Y:"); + JB.print((int)AN[0]); + JB.setcursor(LCDLine2); + JB.print(" R:"); + JB.print((int)AN[1]); + JB.print(" P:"); + JB.print((int)AN[2]); + break; + + case mnuCOMPASS: + JB.print("MAGN> Head:"); + JB.print((int)ToDeg(APM_Compass.Heading)); + JB.setcursor(LCDLine2); + JB.println("["); + JB.print((int)APM_Compass.Mag_X); + JB.print(comma); + JB.print((int)APM_Compass.Mag_Y); + JB.print(comma); + JB.print((int)APM_Compass.Mag_Z); + JB.println("]"); + break; + + case mnuSERVO1: + JB.print("#1:"); + JB.print((int)radio_in[CH_1]); + JB.print(" #2:"); + JB.print((int)radio_in[CH_2]); + JB.setcursor(LCDLine2); + JB.print("#3:"); + JB.print((int)radio_in[CH_3]); + JB.print(" #4:"); + JB.print((int)radio_in[CH_4]); + break; + + case mnuSERVO2: + JB.print("#Q:"); + JB.print((int)servo_out[CH_ROLL]/100); + JB.print(" #H:"); + JB.print((int)servo_out[CH_PITCH]/100); + JB.setcursor(LCDLine2); + JB.print("#G:"); + JB.print((int)servo_out[CH_THROTTLE]); + JB.print(" #S:"); + JB.print((int)servo_out[CH_RUDDER]/100); + break; + + case mnuSETUP1: + JB.print("SETUP 1"); + break; + + case mnuSTATUS: + for (byte i = 0; i<32; i++) { + JB.print(statustxt[i]); + } + //JB.print("STATUS*"); + break; + + default: + JB.print("Menu: n/a #"); + JB.print((int) MnuPtr); + break; + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde new file mode 100644 index 0000000000..348b815b83 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde @@ -0,0 +1,596 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Code to Write and Read packets from DataFlash log memory +// Code to interact with the user to dump or erase logs + +#define HEAD_BYTE1 0xA3 // Decimal 163 +#define HEAD_BYTE2 0x95 // Decimal 149 +#define END_BYTE 0xBA // Decimal 186 + + +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); +static int8_t dump_log(uint8_t argc, const Menu::arg *argv); +static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); +static int8_t select_logs(uint8_t argc, const Menu::arg *argv); + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +static int8_t help_log(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("\n" + "Commands:\n" + " dump dump log \n" + " erase erase all logs\n" + " enable |all enable logging or everything\n" + " disable |all disable logging or everything\n" + "\n")); +} + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +const struct Menu::command log_menu_commands[] PROGMEM = { + {"dump", dump_log}, + {"erase", erase_logs}, + {"enable", select_logs}, + {"disable", select_logs}, + {"help", help_log} +}; + +// A Macro to create the Menu +MENU2(log_menu, "Log", log_menu_commands, print_log_menu); + +static bool +print_log_menu(void) +{ + int log_start; + int log_end; + byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); + + Serial.printf_P(PSTR("logs enabled: ")); + if (0 == log_bitmask) { + Serial.printf_P(PSTR("none")); + } else { + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // the bit being set and print the name of the log option to suit. +#define PLOG(_s) if (log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) + PLOG(ATTITUDE_FAST); + PLOG(ATTITUDE_MED); + PLOG(GPS); + PLOG(PM); + PLOG(CTUN); + PLOG(NTUN); + PLOG(MODE); + PLOG(RAW); + PLOG(CMD); +#undef PLOG + } + Serial.println(); + + if (last_log_num == 0) { + Serial.printf_P(PSTR("\nNo logs available for download\n")); + } else { + + Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); + for(int i=1;i last_log_num)) { + Serial.printf_P(PSTR("bad log number\n")); + return(-1); + } + + dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02)); + dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1; + if (dump_log == last_log_num) { + dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); + } + Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), + dump_log, dump_log_start, dump_log_end); + Log_Read(dump_log_start, dump_log_end); + Serial.printf_P(PSTR("Log read complete\n")); +} + +static int8_t +erase_logs(uint8_t argc, const Menu::arg *argv) +{ + for(int i = 10 ; i > 0; i--) { + Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); + delay(1000); + } + Serial.printf_P(PSTR("\nErasing log...\n")); + for(int j = 1; j < 4001; j++) + DataFlash.PageErase(j); + eeprom_write_byte((uint8_t *)EE_LAST_LOG_NUM, 0); + eeprom_write_byte((uint8_t *)EE_LAST_LOG_PAGE, 1); + Serial.printf_P(PSTR("\nLog erased.\n")); +} + +static int8_t +select_logs(uint8_t argc, const Menu::arg *argv) +{ + uint16_t bits; + + if (argc != 2) { + Serial.printf_P(PSTR("missing log type\n")); + return(-1); + } + + bits = 0; + + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // that name as the argument to the command, and set the bit in + // bits accordingly. + // + if (!strcasecmp_P(argv[1].str, PSTR("all"))) { + bits = ~(bits = 0); + } else { +#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s + TARG(ATTITUDE_FAST); + TARG(ATTITUDE_MED); + TARG(GPS); + TARG(PM); + TARG(CTUN); + TARG(NTUN); + TARG(MODE); + TARG(RAW); + TARG(CMD); +#undef TARG + } + + if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { + log_bitmask |= bits; + } else { + log_bitmask &= ~bits; + } + save_user_configs(); // XXX this is a bit heavyweight... + + return(0); +} + +int8_t +process_logs(uint8_t argc, const Menu::arg *argv) +{ + log_menu.run(); +} + +// Write an attitude packet. Total length : 10 bytes +void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_ATTITUDE_MSG); + DataFlash.WriteInt(log_roll); + DataFlash.WriteInt(log_pitch); + DataFlash.WriteInt(log_yaw); + DataFlash.WriteByte(END_BYTE); +} + +// Write a performance monitoring packet. Total length : 19 bytes +void Log_Write_Performance() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_PERFORMANCE_MSG); + DataFlash.WriteLong(millis()- perf_mon_timer); + DataFlash.WriteInt(mainLoop_count); + DataFlash.WriteInt(G_Dt_max); + DataFlash.WriteByte(gyro_sat_count); + DataFlash.WriteByte(adc_constraints); + DataFlash.WriteByte(renorm_sqrt_count); + DataFlash.WriteByte(renorm_blowup_count); + DataFlash.WriteByte(gps_fix_count); + DataFlash.WriteInt((int)(imu_health*1000)); + DataFlash.WriteByte(END_BYTE); +} + +// Write a command processing packet. Total length : 19 bytes +//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) +void Log_Write_Cmd(byte num, struct Location *wp) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CMD_MSG); + DataFlash.WriteByte(num); + DataFlash.WriteByte(wp->id); + DataFlash.WriteByte(wp->p1); + DataFlash.WriteLong(wp->alt); + DataFlash.WriteLong(wp->lat); + DataFlash.WriteLong(wp->lng); + DataFlash.WriteByte(END_BYTE); +} + +void Log_Write_Startup(byte type) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_STARTUP_MSG); + DataFlash.WriteByte(type); + DataFlash.WriteByte(wp_total); + DataFlash.WriteByte(END_BYTE); + + // create a location struct to hold the temp Waypoints for printing + struct Location cmd = get_wp_with_index(0); + Log_Write_Cmd(0, &cmd); + + for (int i=1; i7) logvar = DataFlash.ReadInt(); + else logvar = DataFlash.ReadByte(); + //if(y > 7) logvar = logvar/1000.f; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read a command processing packet +void Log_Read_Cmd() +{ + byte logvarb; + long logvarl; + + Serial.print("CMD:"); + for(int i=1;i<4;i++) { + logvarb = DataFlash.ReadByte(); + Serial.print(logvarb,DEC); + Serial.print(comma); + } + for(int i=1;i<4;i++) { + logvarl = DataFlash.ReadLong(); + Serial.print(logvarl,DEC); + Serial.print(comma); + } + Serial.println(" "); +} + +void Log_Read_Startup() +{ + byte logbyte = DataFlash.ReadByte(); + if (logbyte == TYPE_AIRSTART_MSG) + Serial.print("AIR START - "); + else if (logbyte == TYPE_GROUNDSTART_MSG) + Serial.print("GROUND START - "); + else + Serial.print("UNKNOWN STARTUP TYPE -"); + Serial.print(DataFlash.ReadByte(), DEC); + Serial.println(" commands in memory"); +} + +// Read an attitude packet +void Log_Read_Attitude() +{ + int log_roll; + int log_pitch; + uint16_t log_yaw; + + log_roll = DataFlash.ReadInt(); + log_pitch = DataFlash.ReadInt(); + log_yaw = (uint16_t)DataFlash.ReadInt(); + + Serial.print("ATT:"); + Serial.print(log_roll); + Serial.print(comma); + Serial.print(log_pitch); + Serial.print(comma); + Serial.print(log_yaw); + Serial.println(); +} + +// Read a mode packet +void Log_Read_Mode() +{ + byte mode; + + mode = DataFlash.ReadByte(); + Serial.print("MOD:"); + switch (mode) { + case 0: + Serial.println("Manual"); + break; + case 1: + Serial.println("Stab"); + break; + case 5: + Serial.println("FBW_A"); + break; + case 6: + Serial.println("FBW_B"); + break; + case 10: + Serial.println("AUTO"); + break; + case 11: + Serial.println("RTL"); + break; + case 12: + Serial.println("Loiter"); + break; + case 98: + Serial.println("AS_COM"); + break; + case 99: + Serial.println("GS_COM"); + break; + } +} + +// Read a GPS packet +void Log_Read_GPS() +{ + + Serial.print("GPS:"); + Serial.print(DataFlash.ReadLong()); // Time + Serial.print(comma); + Serial.print((int)DataFlash.ReadByte()); // Fix + Serial.print(comma); + Serial.print((int)DataFlash.ReadByte()); // Num of Sats + Serial.print(comma); + Serial.print((float)DataFlash.ReadLong()/t7, 7); // Lattitude + Serial.print(comma); + Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude + Serial.print(comma); + Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix + Serial.print(comma); + Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude + Serial.print(comma); + Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed + Serial.print(comma); + Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course + +} + +// Read a raw accel/gyro packet +void Log_Read_Raw() +{ + float logvar; + Serial.print("RAW:"); + for (int y=0;y<6;y++) { + logvar = DataFlash.ReadLong()/1000.f; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read the DataFlash log memory : Packet Parser +void Log_Read(int start_page, int end_page) +{ + byte data; + byte log_step=0; + int packet_count=0; + int page = start_page; + + + DataFlash.StartRead(start_page); + while (page < end_page && page != -1) + { + data = DataFlash.ReadByte(); + switch(log_step) //This is a state machine to read the packets + { + case 0: + if(data==HEAD_BYTE1) // Head byte 1 + log_step++; + break; + case 1: + if(data==HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + case 2: + if(data==LOG_ATTITUDE_MSG){ + Log_Read_Attitude(); + log_step++; + + }else if(data==LOG_MODE_MSG){ + Log_Read_Mode(); + log_step++; + + }else if(data==LOG_CONTROL_TUNING_MSG){ + Log_Read_Control_Tuning(); + log_step++; + + }else if(data==LOG_NAV_TUNING_MSG){ + Log_Read_Nav_Tuning(); + log_step++; + + }else if(data==LOG_PERFORMANCE_MSG){ + Log_Read_Performance(); + log_step++; + + }else if(data==LOG_RAW_MSG){ + Log_Read_Raw(); + log_step++; + + }else if(data==LOG_CMD_MSG){ + Log_Read_Cmd(); + log_step++; + + }else if(data==LOG_STARTUP_MSG){ + Log_Read_Startup(); + log_step++; + }else { + if(data==LOG_GPS_MSG){ + Log_Read_GPS(); + log_step++; + } else { + Serial.print("Error Reading Packet: "); + Serial.print(packet_count); + log_step=0; // Restart, we have a problem... + } + } + break; + case 3: + if(data==END_BYTE){ + packet_count++; + }else{ + Serial.print("Error Reading END_BYTE "); + Serial.println(data,DEC); + } + log_step=0; // Restart sequence: new packet... + break; + } + page = DataFlash.GetPage(); + } + Serial.print("Number of packets read: "); + Serial.println(packet_count); +} + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt new file mode 100644 index 0000000000..3e8c4edf45 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt @@ -0,0 +1,75 @@ +ArduPilotMega 1.0.0 Commands + +Command Structure in bytes +0 0x00 byte Command ID +1 0x01 byte Parameter 1 +2 0x02 int Parameter 2 +3 0x03 .. +4 0x04 long Parameter 3 +5 0x05 .. +6 0x06 .. +7 0x07 .. +8 0x08 long Parameter 4 +9 0x09 .. +10 0x0A .. +11 0x0B .. + +0x00 Reserved +.... +0x0F Reserved + +*********************************** +Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude" +*********************************** +Command ID Name Parameter 1 Altitude Latitude Longitude +0x10 CMD_WAYPOINT - altitude lat lon +0x11 CMD_LOITER (indefinitely) - altitude lat lon +0x12 CMD_LOITER_N_TURNS turns altitude lat lon +0x13 CMD_LOITER_TIME time (seconds*10) altitude lat lon +0x14 CMD_RTL - altitude lat lon +0x15 CMD_LAND - altitude lat lon +0x16 CMD_TAKEOFF angle - - - + + +*********************************** +May Commands - these commands are optional to finish +Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 +0x20 CMD_DELAY - - time (milliseconds) - +0x21 CMD_CLIMB rate (cm/sec) alt (finish) - - +0x22 CMD_LAND_OPTIONS distance to WP airspeed m/s, throttle %, pitch deg + + + +*********************************** +Unexecuted commands >= 0x20 are dropped when ready for the next command <= 0x1F so plan/queue commands accordingly! +For example if you had a string of 0x21 commands following a 0x10 command that had not finished when the waypoint was +reached, the unexecuted 0x21 commands would be skipped and the next command <= 0x1F would be loaded +*********************************** +Now Commands - these commands are executed once until no more new now commands are available +0x31 CMD_RESET_INDEX +0x32 CMD_GOTO_INDEX index repeat count +0x33 CMD_GETVAR_INDEX variable ID +0x34 CMD_SENDVAR_INDEX off/on = 0/1 +0x35 CMD_TELEMETRY off/on = 0/1 + +0x40 CMD_THROTTLE_CRUISE speed +0x41 CMD_AIRSPEED_CRUISE speed +0x44 CMD_RESET_HOME altitude lat lon + +0x60 CMD_KP_GAIN array index gain value*100,000 +0x61 CMD_KI_GAIN array index gain value*100,000 +0x62 CMD_KD_GAIN array index gain value*100,000 +0x63 CMD_KI_MAX array index gain value*100,000 +0x64 CMD_KFF_GAIN array index gain value*100,000 + +0x70 CMD_RADIO_TRIM array index value +0x71 CMD_RADIO_MAX array index value +0x72 CMD_RADIO_MIN array index value +0x73 CMD_ELEVON_TRIM array index value (index 0 = elevon 1 trim, 1 = elevon 2 trim) +0x75 CMD_INDEX index + +0x80 CMD_REPEAT type value delay in sec repeat count +0x81 CMD_RELAY (0,1 to change swicth position) +0x82 CMD_SERVO number value + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde new file mode 100644 index 0000000000..0d2a1c4ba2 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde @@ -0,0 +1,228 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +void init_commands() +{ + read_EEPROM_waypoint_info(); + wp_index = 0; + command_must_index = 0; + command_may_index = 0; + next_command.id = CMD_BLANK; +} + +void update_auto() +{ + if (wp_index == wp_total){ + return_to_launch(); + //wp_index = 0; + } +} + +void reload_commands() +{ + init_commands(); + read_command_index(); // Get wp_index = command_must_index from EEPROM + if(wp_index > 0){ + decrement_WP_index; + } +} + +// Getters +// ------- +struct Location get_wp_with_index(int i) +{ + struct Location temp; + long mem; + + + // Find out proper location in memory by using the start_byte position + the index + // -------------------------------------------------------------------------------- + if (i > wp_total) { + temp.id = CMD_BLANK; + }else{ + // read WP position + mem = (WP_START_BYTE) + (i * WP_SIZE); + temp.id = eeprom_read_byte((uint8_t*)mem); + mem++; + temp.p1 = eeprom_read_byte((uint8_t*)mem); + mem++; + temp.alt = (long)eeprom_read_dword((uint32_t*)mem); + mem += 4; + temp.lat = (long)eeprom_read_dword((uint32_t*)mem); + mem += 4; + temp.lng = (long)eeprom_read_dword((uint32_t*)mem); + } + return temp; +} + +// Setters +// ------- +void set_wp_with_index(struct Location temp, int i) +{ + + i = constrain(i,0,wp_total); + uint32_t mem = WP_START_BYTE + (i * WP_SIZE); + + eeprom_write_byte((uint8_t *) mem, temp.id); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.p1); + + mem++; + eeprom_write_dword((uint32_t *) mem, temp.alt); + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lat); + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lng); +} + +void increment_WP_index() +{ + if(wp_index < wp_total){ + wp_index++; + SendDebug("MSG WP index is incremented to "); + SendDebugln(wp_index,DEC); + }else{ + SendDebug("MSG Failed to increment WP index of "); + SendDebugln(wp_index,DEC); + } +} +void decrement_WP_index() +{ + if(wp_index > 0){ + wp_index--; + } +} + +void loiter_at_location() +{ + next_WP = current_loc; +} + +// add a new command at end of command set to RTL. +void return_to_launch(void) +{ + // home is WP 0 + // ------------ + wp_index = 0; + + // Loads WP from Memory + // -------------------- + set_next_WP(&home); + + // Altitude to hold over home + // Set by configuration tool + // ------------------------- + next_WP.alt = read_alt_to_hold(); +} + +struct Location get_LOITER_home_wp() +{ + // read home position + struct Location temp = get_wp_with_index(0); + temp.id = CMD_LOITER; + + temp.alt = read_alt_to_hold(); + return temp; +} + +/* +This function stores waypoint commands +It looks to see what the next command type is and finds the last command. +*/ +void set_next_WP(struct Location *wp) +{ + //send_message(SEVERITY_LOW,"load WP"); + SendDebug("MSG wp_index: "); + SendDebugln(wp_index,DEC); + send_message(MSG_COMMAND, wp_index); + + // copy the current WP into the OldWP slot + // --------------------------------------- + prev_WP = current_loc; + // Load the next_WP slot + // --------------------- + next_WP = *wp; + // offset the altitude relative to home position + // --------------------------------------------- + next_WP.alt += home.alt; + + // used to control FBW and limit the rate of climb + // ----------------------------------------------- + target_altitude = current_loc.alt; + offset_altitude = next_WP.alt - prev_WP.alt; + + // zero out our loiter vals to watch for missed waypoints + loiter_delta = 0; + loiter_sum = 0; + loiter_total = 0; + + float rads = (abs(next_WP.lat)/t7) * 0.0174532925; + //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); + + // this is handy for the groundstation + wp_totalDistance = getDistance(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + + print_current_waypoints(); + + target_bearing = get_bearing(¤t_loc, &next_WP); + old_target_bearing = target_bearing; + // this is used to offset the shrinking longitude as we go towards the poles + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); +} + + +// run this at setup on the ground +// ------------------------------- +void init_home() +{ + SendDebugln("MSG: init home"); + + // Extra read just in case + // ----------------------- + //GPS.Read(); + + // block until we get a good fix + // ----------------------------- + while (!GPS.new_data || !GPS.fix) { + GPS.update(); + } + home.id = CMD_WAYPOINT; + home.lng = GPS.longitude; // Lon * 10**7 + home.lat = GPS.latitude; // Lat * 10**7 + home.alt = GPS.altitude; + home_is_set = TRUE; + + // ground altitude in centimeters for pressure alt calculations + // ------------------------------------------------------------ + ground_alt = GPS.altitude; + press_alt = GPS.altitude; // Set initial value for filter + save_pressure_data(); + + // Save Home to EEPROM + // ------------------- + set_wp_with_index(home, 0); + + // Save prev loc + // ------------- + prev_WP = home; + + // Signal ready to fly + // Make the servos wiggle - 3 times + // ----------------------- + demo_servos(3); + send_message(SEVERITY_LOW,"\n\n Ready to FLY."); + // 12345678901234567890123456789012 + jeti_status(" X-DIY Ready to FLY"); + jeti_update(); +} + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde new file mode 100644 index 0000000000..93e1ae6c53 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde @@ -0,0 +1,480 @@ +// called by 10 Hz loop +// -------------------- +void update_commands(void) +{ + // This function loads commands into three buffers + // when a new command is loaded, it is processed with process_XXX() + // ---------------------------------------------------------------- + if((home_is_set == FALSE) || (control_mode != AUTO)){ + return; // don't do commands + } + + if(control_mode == AUTO){ + load_next_command(); + process_next_command(); + } + + //verify_must(); + //verify_may(); +} + + +void load_next_command() +{ + // fetch next command if it's empty + // -------------------------------- + if(next_command.id == CMD_BLANK){ + next_command = get_wp_with_index(wp_index+1); + if(next_command.id != CMD_BLANK){ + //SendDebug("MSG fetch found new cmd from list at index "); + //SendDebug((wp_index+1),DEC); + //SendDebug(" with cmd id "); + //SendDebugln(next_command.id,DEC); + } + } + + // If the preload failed, return or just Loiter + // generate a dynamic command for RTL + // -------------------------------------------- + if(next_command.id == CMD_BLANK){ + // we are out of commands! + //send_message(SEVERITY_LOW,"out of commands!"); + //SendDebug("MSG out of commands, wp_index: "); + //SendDebugln(wp_index,DEC); + + + switch (control_mode){ + case LAND: + // don't get a new command + break; + + default: + next_command = get_LOITER_home_wp(); + //SendDebug("MSG Preload RTL cmd id: "); + //SendDebugln(next_command.id,DEC); + break; + } + } +} + +void process_next_command() +{ + // these are waypoint/Must commands + // --------------------------------- + if (command_must_index == 0){ // no current command loaded + if (next_command.id >= 0x10 && next_command.id <= 0x1F ){ + increment_WP_index(); + save_command_index(); // to Recover from in air Restart + command_must_index = wp_index; + + //SendDebug("MSG new command_must_id "); + //SendDebug(next_command.id,DEC); + //SendDebug(" index:"); + //SendDebugln(command_must_index,DEC); + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(wp_index, &next_command); + process_must(); + } + } + + // these are May commands + // ---------------------- + if (command_may_index == 0){ + if (next_command.id >= 0x20 && next_command.id <= 0x2F ){ + increment_WP_index();// this command is from the command list in EEPROM + command_may_index = wp_index; + //Serial.print("new command_may_index "); + //Serial.println(command_may_index,DEC); + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(wp_index, &next_command); + process_may(); + } + } + + // these are do it now commands + // --------------------------- + if (next_command.id >= 0x30){ + increment_WP_index();// this command is from the command list in EEPROM + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(wp_index, &next_command); + process_now(); + } + +} + +/* +These functions implement the waypoint commands. +*/ +void process_must() +{ + //SendDebug("process must index: "); + //SendDebugln(command_must_index,DEC); + + send_message(SEVERITY_LOW,"New cmd: "); + send_message(MSG_COMMAND, wp_index); + + // clear May indexes + command_may_index = 0; + command_may_ID = 0; + + command_must_ID = next_command.id; + + // invalidate command so a new one is loaded + // ----------------------------------------- + next_command.id = 0; + + // reset navigation integrators + // ------------------------- + reset_I(); + + // loads the waypoint into Next_WP struct + // -------------------------------------- + set_next_WP(&next_command); + + switch(command_must_ID){ + + case CMD_TAKEOFF: // TAKEOFF! + // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 + takeoff_pitch = next_command.p1 * 100; + takeoff_altitude = next_WP.alt; // next_WP.alt is calculated by the set_next_WP command + next_WP.lat = home.lat + 1000; // so we don't have bad calcs + next_WP.lng = home.lng + 1000; // so we don't have bad calcs + break; + + case CMD_WAYPOINT: // Navigate to Waypoint + break; + + case CMD_LAND: // LAND to Waypoint + break; + + case CMD_LOITER: // Loiter indefinitely + break; + + case CMD_LOITER_N_TURNS: // Loiter N Times + loiter_total = next_command.p1 * 360; + break; + + case CMD_LOITER_TIME: + loiter_time = millis(); + loiter_time_max = next_command.p1 * 10000; // seconds * 10 * 1000 + break; + + case CMD_RTL: + break; + } +} + +void process_may() +{ + //Serial.print("process_may cmd# "); + //Serial.println(wp_index,DEC); + command_may_ID = next_command.id; + + // invalidate command so a new one is loaded + // ----------------------------------------- + next_command.id = 0; + + send_message(SEVERITY_LOW," New may command loaded:"); + send_message(MSG_COMMAND, wp_index); + + // do the command + // -------------- + switch(command_may_ID){ + + case CMD_DELAY: // Navigate to Waypoint + delay_start = millis(); + delay_timeout = next_command.lat; + break; + + case CMD_LAND_OPTIONS: // Land this puppy + //send_message(SEVERITY_LOW,"Landing"); + + // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 + landing_pitch = next_command.lng * 100; + airspeed_cruise = next_command.alt * 100; + throttle_cruise = next_command.lat; + landing_distance = next_command.p1; + //landing_roll = command.lng; + + SendDebug("MSG: throttle_cruise = "); + SendDebugln(throttle_cruise,DEC); + break; + + default: + break; + } +} + +void process_now() +{ + const float t5 = 100000.0; + //Serial.print("process_now cmd# "); + //Serial.println(wp_index,DEC); + + byte id = next_command.id; + + // invalidate command so a new one is loaded + // ----------------------------------------- + next_command.id = 0; + + send_message(SEVERITY_LOW, " New now command loaded: "); + send_message(MSG_COMMAND, wp_index); + + // do the command + // -------------- + switch(id){ + + //case CMD_AP_MODE: + //next_command.p1 = constrain(next_command.p1, MANUAL, LAND); + //set_mode(next_command.p1); + //break; + + case CMD_RESET_INDEX: + init_commands(); + break; + + case CMD_GETVAR_INDEX: + // + break; + + case CMD_SENDVAR_INDEX: + // + break; + + case CMD_TELEMETRY: + // + break; + + case CMD_AIRSPEED_CRUISE: + airspeed_cruise = next_command.p1 * 100; + // todo save to EEPROM + break; + + case CMD_THROTTLE_CRUISE: + throttle_cruise = next_command.p1; + // todo save to EEPROM + break; + + case CMD_RESET_HOME: + init_home(); + break; + + case CMD_KP_GAIN: + kp[next_command.p1] = next_command.alt/t5; + // todo save to EEPROM + break; + case CMD_KI_GAIN: + ki[next_command.p1] = next_command.alt/t5; + // todo save to EEPROM + break; + case CMD_KD_GAIN: + kd[next_command.p1] = next_command.alt/t5; + // todo save to EEPROM + break; + + case CMD_KI_MAX: + integrator_max[next_command.p1] = next_command.alt/t5; + // todo save to EEPROM + break; + + case CMD_KFF_GAIN: + kff[next_command.p1] = next_command.alt/t5; + // todo save to EEPROM + break; + + case CMD_RADIO_TRIM: + radio_trim[next_command.p1] = next_command.alt; + save_EEPROM_trims(); + break; + + case CMD_RADIO_MAX: + radio_max[next_command.p1] = next_command.alt; + save_EEPROM_radio_minmax(); + break; + + case CMD_RADIO_MIN: + radio_min[next_command.p1] = next_command.alt; + save_EEPROM_radio_minmax(); + break; + + case CMD_REPEAT: + new_event(&next_command); + break; + + case CMD_SERVO: + //Serial.print("CMD_SERVO "); + //Serial.print(next_command.p1,DEC); + //Serial.print(" PWM: "); + //Serial.println(next_command.alt,DEC); + APM_RC.OutputCh(next_command.p1, next_command.alt); + break; + + case CMD_INDEX: + command_must_index = 0; + command_may_index = 0; + wp_index = next_command.p1 - 1; + break; + + case CMD_RELAY: + if(next_command.p1 = 0){ + relay_A(); + }else{ + relay_B(); + } + break; + } +} + +// Verify commands +// --------------- +void verify_must() +{ + float power; + + switch(command_must_ID) { + + case CMD_TAKEOFF: // Takeoff! + //Serial.print("verify_must cmd# "); + //Serial.println(command_must_index,DEC); + + if (GPS.ground_speed > 3){ + if(hold_course == -1){ + // save our current course to land + hold_course = yaw_sensor; + } + } + if(hold_course > -1){ + // recalc bearing error with hold_course; + nav_bearing = hold_course; + // recalc bearing error + calc_bearing_error(); + } + if (current_loc.alt > (home.alt + takeoff_altitude)) { + command_must_index = 0; + hold_course = -1; + } + break; + + + + case CMD_LAND: + // we don't verify landing - we never go to a new Must command after Land. + if ( ((wp_distance > 0) && (wp_distance <= (2*GPS.ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300) ) + { + land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude + if(hold_course == -1){ + // save our current course to land + //hold_course = yaw_sensor; + // save the course line of the runway to land + hold_course = crosstrack_bearing; + } + } + if(hold_course > -1){ + // recalc bearing error with hold_course; + nav_bearing = hold_course; + // recalc bearing error + calc_bearing_error(); + } + update_crosstrack(); + + break; + + case CMD_WAYPOINT: // reach a waypoint + hold_course == -1; + update_crosstrack(); + if ((wp_distance > 0) && (wp_distance <= wp_radius)) { + SendDebug("MSG REACHED_WAYPOINT #"); + SendDebugln(command_must_index,DEC); + // clear the command queue; + command_must_index = 0; + } + // add in a more complex case + // Doug to do + if(loiter_sum > 300){ + send_message(SEVERITY_MEDIUM," Missed WP"); + command_must_index = 0; + } + break; + + case CMD_LOITER_N_TURNS: // LOITER N times + if (wp_distance <= loiter_radius){ + nav_bearing -= 9000; + + }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ + power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0, 1); + nav_bearing -= power * 9000; + + }else{ + update_crosstrack(); + } + if(loiter_sum > loiter_total) { + loiter_total = 0; + send_message(SEVERITY_LOW," LOITER orbits complete "); + // clear the command queue; + command_must_index = 0; + } + // recalc bearing error + nav_bearing = wrap_360(nav_bearing); + calc_bearing_error(); + break; + + case CMD_LOITER_TIME: // loiter N milliseconds + + if (wp_distance <= loiter_radius){ + nav_bearing -= 9000; + + }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ + power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0, 1); + nav_bearing -= power * 9000; + + }else{ + update_crosstrack(); + } + + if ((millis() - loiter_time) > loiter_time_max) { + send_message(SEVERITY_LOW," LOITER time complete "); + command_must_index = 0; + } + nav_bearing = wrap_360(nav_bearing); + // recalc bearing error + calc_bearing_error(); + break; + + case CMD_RTL: + if (wp_distance <= 30) { + //Serial.println("REACHED_HOME"); + command_must_index = 0; + } + break; + + case CMD_LOITER: // Just plain LOITER + break; + + default: + send_message(SEVERITY_HIGH," No current Must commands"); + break; + } +} + +void verify_may() +{ + switch(command_may_ID) { + case CMD_DELAY: + if ((millis() - delay_start) > delay_timeout){ + command_may_index = 0; + delay_timeout = 0; + } + + case CMD_LAND_OPTIONS: + if ((wp_distance > 0) && (wp_distance <= landing_distance)) { + //Serial.print("XXX REACHED_WAYPOINT #"); + //Serial.println(command_must_index,DEC); + // clear the command queue; + command_may_index = 0; + } + break; + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h new file mode 100644 index 0000000000..d3e3b0e2a4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h @@ -0,0 +1,569 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// +// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING +// +// DO NOT EDIT this file to adjust your configuration. Create your own +// APM_Config.h and use APM_Config.h.example as a reference. +// +// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING +/// +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// +// Default and automatic configuration details. +// +// Notes for maintainers: +// +// - Try to keep this file organised in the same order as APM_Config.h.example +// + +#include "defines.h" + +/// +/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that +/// change in your local copy of APM_Config.h. +/// +#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT +/// +/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that +/// change in your local copy of APM_Config.h. +/// + +// Just so that it's completely clear... +#define ENABLED 1 +#define DISABLED 0 + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// HARDWARE CONFIGURATION AND CONNECTIONS +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_HIL +// Hardware in the loop output +// +#ifndef ENABLE_HIL +# define ENABLE_HIL DISABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// GPS_PROTOCOL +// +#ifndef GPS_PROTOCOL +# error XXX +# error XXX You must define GPS_PROTOCOL in APM_Config.h +# error XXX +#endif + +// The X-Plane GCS requires the IMU GPS configuration +#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU) +# error Must select GPS_PROTOCOL_IMU when configuring for X-Plane or Flight Gear HIL +#endif + + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_SENSOR +// AIRSPEED_RATIO +// +#ifndef AIRSPEED_SENSOR +# define AIRSPEED_SENSOR DISABLED +#endif +#ifndef AIRSPEED_RATIO +# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution +#endif + + +////////////////////////////////////////////////////////////////////////////// +// GCS_PROTOCOL +// GCS_PORT +// +#ifndef GCS_PROTOCOL +# define GCS_PROTOCOL GCS_PROTOCOL_STANDARD +#endif + +#ifndef GCS_PORT +# if (GCS_PROTOCOL == GCS_PROTOCOL_STANDARD) || (GCS_PROTOCOL == GCS_PROTOCOL_LEGACY) || (GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL) +# define GCS_PORT 3 +# else +# define GCS_PORT 0 +# endif +#endif + +#ifndef DEBUGTERMINAL_VERBOSE +# define DEBUGTERMINAL_VERBOSE 1 //Set this to 1 to get more detail in DEBUGTERMINAL messages, or 0 to save flash space +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Serial port speeds. +// +#ifndef SERIAL0_BAUD +# define SERIAL0_BAUD 38400 +#endif +#ifndef SERIAL3_BAUD +# define SERIAL3_BAUD 115200 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Battery monitoring +// +#ifndef BATTERY_EVENT +# define BATTERY_EVENT DISABLED +#endif +#ifndef BATTERY_TYPE +# define BATTERY_TYPE 0 +#endif +#ifndef LOW_VOLTAGE +# define LOW_VOLTAGE 11.4 +#endif +#ifndef VOLT_DIV_RATIO +# define VOLT_DIV_RATIO 3.0 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// INPUT_VOLTAGE +// +#ifndef INPUT_VOLTAGE +# define INPUT_VOLTAGE 5.0 +#endif + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// RADIO CONFIGURATION +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + + +////////////////////////////////////////////////////////////////////////////// +// Radio channel limits +// +// Note that these are not called out in APM_Config.h.example as there +// is currently no configured behaviour for these channels. +// +#ifndef CH5_MIN +# define CH5_MIN 1000 +#endif +#ifndef CH5_MAX +# define CH5_MAX 2000 +#endif +#ifndef CH6_MIN +# define CH6_MIN 1000 +#endif +#ifndef CH6_MAX +# define CH6_MAX 2000 +#endif +#ifndef CH7_MIN +# define CH7_MIN 1000 +#endif +#ifndef CH7_MAX +# define CH7_MAX 2000 +#endif +#ifndef CH8_MIN +# define CH8_MIN 1000 +#endif +#ifndef CH8_MAX +# define CH8_MAX 2000 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// FLIGHT_MODE +// FLIGHT_MODE_CHANNEL +// +#ifndef FLIGHT_MODE_CHANNEL +# define FLIGHT_MODE_CHANNEL 8 +#endif +#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8) +# error XXX +# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8 +# error XXX +#endif + +#if !defined(FLIGHT_MODE_1) +# define FLIGHT_MODE_1 FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_2) +# define FLIGHT_MODE_2 FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_3) +# define FLIGHT_MODE_3 STABILIZE +#endif +#if !defined(FLIGHT_MODE_4) +# define FLIGHT_MODE_4 STABILIZE +#endif +#if !defined(FLIGHT_MODE_5) +# define FLIGHT_MODE_5 MANUAL +#endif +#if !defined(FLIGHT_MODE_6) +# define FLIGHT_MODE_6 MANUAL +#endif + + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_FAILSAFE +// THROTTLE_FS_VALUE +// THROTTLE_FAILSAFE_ACTION +// +#ifndef THROTTLE_FAILSAFE +# define THROTTLE_FAILSAFE DISABLED +#endif +#ifndef THROTTE_FS_VALUE +# define THROTTLE_FS_VALUE 975 +#endif +#ifndef THROTTLE_FAILSAFE_ACTION +# define THROTTLE_FAILSAFE_ACTION 2 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// AUTO_TRIM +// +#ifndef AUTO_TRIM +# define AUTO_TRIM ENABLED +#endif + + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_REVERSE +// +#ifndef THROTTLE_REVERSE +# define THROTTLE_REVERSE DISABLED +#endif + + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_STICK_MIXING +// +#ifndef ENABLE_STICK_MIXING +# define ENABLE_STICK_MIXING ENABLED +#endif + + +////////////////////////////////////////////////////////////////////////////// +// THROTTLE_OUT +// +#ifndef THROTTE_OUT +# define THROTTLE_OUT ENABLED +#endif + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// STARTUP BEHAVIOUR +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// GROUND_START_DELAY +// +#ifndef GROUND_START_DELAY +# define GROUND_START_DELAY 0 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// ENABLE_AIR_START +// +#ifndef ENABLE_AIR_START +# define ENABLE_AIR_START DISABLED +#endif + +////////////////////////////////////////////////////////////////////////////// +// ENABLE REVERSE_SWITCH +// +#ifndef REVERSE_SWITCH +# define REVERSE_SWITCH ENABLED +#endif + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// FLIGHT AND NAVIGATION CONTROL +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// Altitude measurement and control. +// +#ifndef AOA +# define AOA 100 // XXX still 100ths of a degree +#endif +#ifndef ALT_EST_GAIN +# define ALT_EST_GAIN 0.01 +#endif +#ifndef ALTITUDE_MIX +# define ALTITUDE_MIX 0 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// AIRSPEED_CRUISE +// +#ifndef AIRSPEED_CRUISE +# define AIRSPEED_CRUISE 10 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// FLY_BY_WIRE_B airspeed control +// +#ifndef AIRSPEED_FBW_MIN +# define AIRSPEED_FBW_MIN 6 +#endif +#ifndef AIRSPEED_FBW_MAX +# define AIRSPEED_FBW_MAX 30 +#endif +#ifndef THROTTLE_ALT_P +# define THROTTLE_ALT_P 0.32 +#endif +#ifndef THROTTLE_ALT_I +# define THROTTLE_ALT_I 0.04 +#endif +#ifndef THROTTLE_ALT_D +# define THROTTLE_ALT_D 0.0 +#endif +#ifndef THROTTLE_ALT_INT_MAX +# define THROTTLE_ALT_INT_MAX 20 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Throttle control +// +#ifndef THROTTLE_MIN +# define THROTTLE_MIN 0 +#endif +#ifndef THROTTLE_CRUISE +# define THROTTLE_CRUISE 45 +#endif +#ifndef THROTTLE_MAX +# define THROTTLE_MAX 75 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Autopilot control limits +// +#ifndef HEAD_MAX +# define HEAD_MAX 45 +#endif +#ifndef PITCH_MAX +# define PITCH_MAX 15 +#endif +#ifndef PITCH_MIN +# define PITCH_MIN -25 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Attitude control gains +// +#ifndef SERVO_ROLL_P +# define SERVO_ROLL_P 0.4 +#endif +#ifndef SERVO_ROLL_I +# define SERVO_ROLL_I 0.0 +#endif +#ifndef SERVO_ROLL_D +# define SERVO_ROLL_D 0.0 +#endif +#ifndef SERVO_ROLL_INT_MAX +# define SERVO_ROLL_INT_MAX 5 +#endif +#ifndef ROLL_SLEW_LIMIT +# define ROLL_SLEW_LIMIT 0 +#endif +#ifndef SERVO_PITCH_P +# define SERVO_PITCH_P 0.6 +#endif +#ifndef SERVO_PITCH_I +# define SERVO_PITCH_I 0.0 +#endif +#ifndef SERVO_PITCH_D +# define SERVO_PITCH_D 0.0 +#endif +#ifndef SERVO_PITCH_INT_MAX +# define SERVO_PITCH_INT_MAX 5 +#endif +#ifndef PITCH_COMP +# define PITCH_COMP 0.2 +#endif +#ifndef SERVO_YAW_P +# define SERVO_YAW_P 0.5 +#endif +#ifndef SERVO_YAW_I +# define SERVO_YAW_I 0.0 +#endif +#ifndef SERVO_YAW_D +# define SERVO_YAW_D 0.0 +#endif +#ifndef SERVO_YAW_INT_MAX +# define SERVO_YAW_INT_MAX 5 +#endif +#ifndef RUDDER_MIX +# define RUDDER_MIX 0.5 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Navigation control gains +// +#ifndef NAV_ROLL_P +# define NAV_ROLL_P 0.7 +#endif +#ifndef NAV_ROLL_I +# define NAV_ROLL_I 0.01 +#endif +#ifndef NAV_ROLL_D +# define NAV_ROLL_D 0.02 +#endif +#ifndef NAV_ROLL_INT_MAX +# define NAV_ROLL_INT_MAX 5 +#endif +#ifndef NAV_PITCH_ASP_P +# define NAV_PITCH_ASP_P 0.65 +#endif +#ifndef NAV_PITCH_ASP_I +# define NAV_PITCH_ASP_I 0.0 +#endif +#ifndef NAV_PITCH_ASP_D +# define NAV_PITCH_ASP_D 0.0 +#endif +#ifndef NAV_PITCH_ASP_INT_MAX +# define NAV_PITCH_ASP_INT_MAX 5 +#endif +#ifndef NAV_PITCH_ALT_P +# define NAV_PITCH_ALT_P 0.65 +#endif +#ifndef NAV_PITCH_ALT_I +# define NAV_PITCH_ALT_I 0.0 +#endif +#ifndef NAV_PITCH_ALT_D +# define NAV_PITCH_ALT_D 0.0 +#endif +#ifndef NAV_PITCH_ALT_INT_MAX +# define NAV_PITCH_ALT_INT_MAX 5 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Energy/Altitude control gains +// +#ifndef THROTTLE_TE_P +# define THROTTLE_TE_P 0.50 +#endif +#ifndef THROTTLE_TE_I +# define THROTTLE_TE_I 0.0 +#endif +#ifndef THROTTLE_TE_D +# define THROTTLE_TE_D 0.0 +#endif +#ifndef THROTTLE_TE_INT_MAX +# define THROTTLE_TE_INT_MAX 20 +#endif +#ifndef THROTTLE_SLEW_LIMIT +# define THROTTLE_SLEW_LIMIT 0 +#endif +#ifndef P_TO_T +# define P_TO_T 2.5 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Crosstrack compensation +// +#ifndef XTRACK_GAIN +# define XTRACK_GAIN 0.01 +#endif +#ifndef XTRACK_ENTRY_ANGLE +# define XTRACK_ENTRY_ANGLE 30 +#endif + + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// DEBUGGING +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////// +// DEBUG_SUBSYSTEM +// +#ifndef DEBUG_SUBSYSTEM +# define DEBUG_SUBSYSTEM 0 +#endif + + +////////////////////////////////////////////////////////////////////////////// +// DEBUG_LEVEL +// +#ifndef DEBUG_LEVEL +# define DEBUG_LEVEL SEVERITY_LOW +#endif + + +////////////////////////////////////////////////////////////////////////////// +// Dataflash logging control +// +#ifndef LOG_ATTITUDE_FAST +# define LOG_ATTITUDE_FAST DISABLED +#endif +#ifndef LOG_ATTITUDE_MED +# define LOG_ATTITUDE_MED ENABLED +#endif +#ifndef LOG_GPS +# define LOG_GPS ENABLED +#endif +#ifndef LOG_PM +# define LOG_PM ENABLED +#endif +#ifndef LOG_CTUN +# define LOG_CTUN DISABLED +#endif +#ifndef LOG_NTUN +# define LOG_NTUN DISABLED +#endif +#ifndef LOG_MODE +# define LOG_MODE ENABLED +#endif +#ifndef LOG_RAW +# define LOG_RAW DISABLED +#endif +#ifndef LOG_CMD +# define LOG_CMD ENABLED +#endif + +#ifndef DEBUG_PORT +# define DEBUG_PORT 0 +#endif + +#if DEBUG_PORT == 0 +# define SendDebug Serial.print +# define SendDebugln Serial.println +#elif DEBUG_PORT == 1 +# define SendDebug Serial1.print +# define SendDebugln Serial1.println +#elif DEBUG_PORT == 2 +# define SendDebug Serial2.print +# define SendDebugln Serial2.println +#elif DEBUG_PORT == 3 +# define SendDebug Serial3.print +# define SendDebugln Serial3.println +#endif + +////////////////////////////////////////////////////////////////////////////// +// Navigation defaults +// +#ifndef WP_RADIUS_DEFAULT +# define WP_RADIUS_DEFAULT 20 +#endif +#ifndef LOITER_RADIUS_DEFAULT +# define LOITER_RADIUS_DEFAULT 30 +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde new file mode 100644 index 0000000000..d2d348e67f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde @@ -0,0 +1,58 @@ +void read_control_switch() +{ + byte switchPosition = readSwitch(); + if (oldSwitchPosition != switchPosition){ + + set_mode(flight_modes[switchPosition]); + + oldSwitchPosition = switchPosition; + + // reset navigation integrators + // ------------------------- + reset_I(); + } +} +/* +byte readSwitch(void){ + int pulsewidth = APM_RC.InputCh(FLIGHT_MODE_CHANNEL - 1); + if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; + if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; + if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; + if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual + if (pulsewidth >= 1750) return 5; // Hardware Manual + return 0; +} +*/ +byte readSwitch(void){ + int pulsewidth = APM_RC.InputCh(FLIGHT_MODE_CHANNEL - 1); + if (pulsewidth > FLIGHT_MODE_5_BOUNDARY) return 5; + if (pulsewidth > FLIGHT_MODE_4_BOUNDARY) return 4; + if (pulsewidth > FLIGHT_MODE_3_BOUNDARY) return 3; + if (pulsewidth > FLIGHT_MODE_2_BOUNDARY) return 2; + if (pulsewidth > FLIGHT_MODE_1_BOUNDARY) return 1; + return 0; +} + +void reset_control_switch() +{ + oldSwitchPosition = 0; + read_control_switch(); + SendDebug("MSG: reset_control_switch"); + SendDebugln(oldSwitchPosition , DEC); +} + +void update_servo_switches() +{ + // up is reverse + // up is elevon + mix_mode = (PINL & 128) ? 1 : 0; + if (mix_mode == 0) { + reverse_roll = (PINE & 128) ? 1 : -1; + reverse_pitch = (PINE & 64) ? 1 : -1; + reverse_rudder = (PINL & 64) ? 1 : -1; + } else { + reverse_elevons = (PINE & 128) ? 1 : -1; + reverse_ch1_elevon = (PINE & 64) ? 1 : -1; + reverse_ch2_elevon = (PINL & 64) ? 1 : -1; + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde new file mode 100644 index 0000000000..e090a763db --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde @@ -0,0 +1,70 @@ +#if DEBUG_SUBSYSTEM == 1 +void debug_subsystem() +{ + Serial.println("GPS Subsystem, RAW OUTPUT"); + + while(1){ + if(Serial1.available() > 0) // Ok, let me see, the buffer is empty? + { + delay(60); // wait for it all + while(Serial1.available() > 0){ + byte incoming = Serial1.read(); + //Serial.print(incoming,DEC); + Serial.print(incoming, BYTE); // will output Hex values + //Serial.print(","); + } + Serial.println(" "); + } + + } +} +#endif + +#if DEBUG_SUBSYSTEM == 2 +void debug_subsystem() +{ + Serial.println("Control Switch"); + + Serial.print("Control CH "); + Serial.println(flight_mode_channel, DEC); + + while(1){ + delay(20); + byte switchPosition = readSwitch(); + if (oldSwitchPosition != switchPosition){ + Serial.printf_P(PSTR("Position %d\n"), i, switchPosition); + oldSwitchPosition = switchPosition; + } + } +} +#endif + +#if DEBUG_SUBSYSTEM == 3 +void debug_subsystem() +{ + Serial.println("DIP Switch Test"); + + while(1){ + delay(100); + update_servo_switches(); + if (mix_mode == 0) { + Serial.print("Mix:standard "); + Serial.print("\t reverse_roll: "); + Serial.print(reverse_roll, DEC); + Serial.print("\t reverse_pitch: "); + Serial.print(reverse_pitch, DEC); + Serial.print("\t reverse_rudder: "); + Serial.println(reverse_rudder, DEC); + } else { + Serial.print("Mix:elevons "); + Serial.print("\t reverse_elevons: "); + Serial.print(reverse_elevons, DEC); + Serial.print("\t reverse_ch1_elevon: "); + Serial.print(reverse_ch1_elevon, DEC); + Serial.print("\t reverse_ch2_elevon: "); + Serial.println(reverse_ch2_elevon, DEC); + } + } +} +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h new file mode 100644 index 0000000000..878fdadf0a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h @@ -0,0 +1,345 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Internal defines, don't edit and expect things to work +// ------------------------------------------------------- + +#define DEBUG 0 +#define LOITER_RANGE 30 // for calculating power outside of loiter radius + +// GPS baud rates +// -------------- +#define NO_GPS 38400 +#define NMEA_GPS 38400 +#define EM406_GPS 57600 +#define UBLOX_GPS 38400 +#define ARDU_IMU 38400 +#define MTK_GPS 38400 +#define SIM_GPS 38400 + +// GPS type codes - use the names, not the numbers +#define GPS_PROTOCOL_NONE -1 +#define GPS_PROTOCOL_NMEA 0 +#define GPS_PROTOCOL_SIRF 1 +#define GPS_PROTOCOL_UBLOX 2 +#define GPS_PROTOCOL_IMU 3 +#define GPS_PROTOCOL_MTK 4 + +// Radio channels +// Note channels are from 0! +// +// XXX these should be CH_n defines from RC.h at some point. +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + +#define WP_START_BYTE 0x130 // where in memory home WP is stored + all other WP +#define WP_SIZE 14 + +// GCS enumeration +#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol +#define GCS_PROTOCOL_SPECIAL 1 // special test protocol (?) +#define GCS_PROTOCOL_LEGACY 2 // legacy ArduPilot protocol +#define GCS_PROTOCOL_XPLANE 3 // X-Plane HIL simulation +#define GCS_PROTOCOL_IMU 4 // ArdiPilot IMU output +#define GCS_PROTOCOL_JASON 5 // Jason's special secret GCS protocol +#define GCS_PROTOCOL_DEBUGTERMINAL 6 //Human-readable debug interface for use with a dumb terminal +#define GCS_PROTOCOL_XDIY 7 // X-DIY custom protocol +#define GCS_PROTOCOL_NONE -1 // No GCS output + +// PID enumeration +// --------------- +#define CASE_SERVO_ROLL 0 +#define CASE_SERVO_PITCH 1 +#define CASE_SERVO_RUDDER 2 +#define CASE_NAV_ROLL 3 +#define CASE_NAV_PITCH_ASP 4 +#define CASE_NAV_PITCH_ALT 5 +#define CASE_TE_THROTTLE 6 +#define CASE_ALT_THROTTLE 7 + +// Feedforward cases +// ---------------- +#define CASE_PITCH_COMP 0 +#define CASE_RUDDER_MIX 1 +#define CASE_P_TO_T 2 + +// Auto Pilot modes +// ---------------- +#define MANUAL 0 +#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle +#define STABILIZE 2 + +#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle +#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed + // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 +#define AUTO 10 +#define RTL 11 +#define LOITER 12 +#define TAKEOFF 13 +#define LAND 14 + + +// Command IDs - Must +#define CMD_BLANK 0x00 // there is no command stored in the mem location requested +#define CMD_WAYPOINT 0x10 +#define CMD_LOITER 0x11 +#define CMD_LOITER_N_TURNS 0x12 +#define CMD_LOITER_TIME 0x13 +#define CMD_RTL 0x14 +#define CMD_LAND 0x15 +#define CMD_TAKEOFF 0x16 + +// Command IDs - May +#define CMD_DELAY 0x20 +#define CMD_CLIMB 0x21 // NOT IMPLEMENTED +#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 + +// Command IDs - Now +//#define CMD_AP_MODE 0x30 +#define CMD_RESET_INDEX 0x31 +#define CMD_GOTO_INDEX 0x32 // NOT IMPLEMENTED +#define CMD_GETVAR_INDEX 0x33 +#define CMD_SENDVAR_INDEX 0x34 +#define CMD_TELEMETRY 0x35 + +#define CMD_THROTTLE_CRUISE 0x40 +#define CMD_AIRSPEED_CRUISE 0x41 +#define CMD_RESET_HOME 0x44 + +#define CMD_KP_GAIN 0x60 +#define CMD_KI_GAIN 0x61 +#define CMD_KD_GAIN 0x62 +#define CMD_KI_MAX 0x63 +#define CMD_KFF_GAIN 0x64 + +#define CMD_RADIO_TRIM 0x70 +#define CMD_RADIO_MAX 0x71 +#define CMD_RADIO_MIN 0x72 +#define CMD_RADIO_MIN 0x72 +#define CMD_ELEVON_TRIM 0x73 + +#define CMD_INDEX 0x75 // sets the current Must index +#define CMD_REPEAT 0x80 +#define CMD_RELAY 0x81 +#define CMD_SERVO 0x82 // move servo N to PWM value + +//repeating events +#define NO_REPEAT 0 +#define CH_4_TOGGLE 1 +#define CH_5_TOGGLE 2 +#define CH_6_TOGGLE 3 +#define CH_7_TOGGLE 4 +#define RELAY_TOGGLE 5 +#define STOP_REPEAT 10 + +// GCS Message ID's +#define MSG_ACKNOWLEDGE 0x00 +#define MSG_HEARTBEAT 0x01 +#define MSG_ATTITUDE 0x02 +#define MSG_LOCATION 0x03 +#define MSG_PRESSURE 0x04 +#define MSG_STATUS_TEXT 0x05 +#define MSG_PERF_REPORT 0x06 +#define MSG_COMMAND 0x22 +#define MSG_VALUE 0x32 +#define MSG_PID 0x42 +#define MSG_TRIMS 0x50 +#define MSG_MINS 0x51 +#define MSG_MAXS 0x52 +#define MSG_IMU_OUT 0x53 + +#define SEVERITY_LOW 1 +#define SEVERITY_MEDIUM 2 +#define SEVERITY_HIGH 3 +#define SEVERITY_CRITICAL 4 + +// Logging parameters +#define LOG_ATTITUDE_MSG 0x01 +#define LOG_GPS_MSG 0x02 +#define LOG_MODE_MSG 0X03 +#define LOG_CONTROL_TUNING_MSG 0X04 +#define LOG_NAV_TUNING_MSG 0X05 +#define LOG_PERFORMANCE_MSG 0X06 +#define LOG_RAW_MSG 0x07 +#define LOG_CMD_MSG 0x08 +#define LOG_STARTUP_MSG 0x09 +#define TYPE_AIRSTART_MSG 0x00 +#define TYPE_GROUNDSTART_MSG 0x01 + +#define MASK_LOG_ATTITUDE_FAST 0 +#define MASK_LOG_ATTITUDE_MED 2 +#define MASK_LOG_GPS 4 +#define MASK_LOG_PM 8 +#define MASK_LOG_CTUN 16 +#define MASK_LOG_NTUN 32 +#define MASK_LOG_MODE 64 +#define MASK_LOG_RAW 128 +#define MASK_LOG_CMD 256 + +// Yaw modes +#define YAW_MODE_COORDINATE_TURNS 0 +#define YAW_MODE_HOLD_HEADING 1 +#define YAW_MODE_SLIP 2 + +// Waypoint Modes +// ---------------- +#define ABS_WP 0 +#define REL_WP 1 + +// Command Queues +// --------------- +#define COMMAND_MUST 0 +#define COMMAND_MAY 1 +#define COMMAND_NOW 2 + +// Events +// ------ +#define EVENT_WILL_REACH_WAYPOINT 1 +#define EVENT_SET_NEW_WAYPOINT_INDEX 2 +#define EVENT_LOADED_WAYPOINT 3 +#define EVENT_LOOP 4 + +//GPS_fix +#define VALID_GPS 0x00 +#define BAD_GPS 0x01 +#define FAILED_GPS 0x03 + + + +#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO + +#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor +#define BATTERY_PIN1 0 // These are the pins for the voltage dividers +#define BATTERY_PIN2 1 +#define BATTERY_PIN3 2 +#define BATTERY_PIN4 3 +#define RELAY_PIN 47 + +// Hardware Parameters +#define SLIDE_SWITCH_PIN 40 +#define PUSHBUTTON_PIN 41 + +#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C +#define B_LED_PIN 36 +#define C_LED_PIN 35 + +#define HOLD_ALT_ABOVE_HOME 8 // bitmask value + +// IMU Parameters + +#define ADC_CONSTRAINT 900 +#define TRUE 1 +#define FALSE 0 +#define ADC_WARM_CYCLES 200 +#define SPEEDFILT 400 // centimeters/second + +#define GYRO_TEMP_CH 3 // The ADC channel reading the gyro temperature + +// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step +// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 +// Tested value : 418 +#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer +#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square + +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 +// Tested values : 0.4026, ?, 0.4192 +#define Gyro_Gain_X 0.4 //X axis Gyro gain +#define Gyro_Gain_Y 0.41 //Y axis Gyro gain +#define Gyro_Gain_Z 0.41 //Z axis Gyro gain +#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second +#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second +#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second + +#define Kp_ROLLPITCH 0.0014 // Pitch&Roll Drift Correction Proportional Gain +#define Ki_ROLLPITCH 0.0000003 // Pitch&Roll Drift Correction Integrator Gain +#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain +#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain + +/*For debugging purposes*/ +#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data + + +#define EEPROM_MAX_ADDR 4096 + +// Radio setup +#define EE_TRIM 0x00 +#define EE_MIN 0x10 +#define EE_MAX 0x20 +#define EE_ELEVON1_TRIM 0x30 +#define EE_ELEVON2_TRIM 0x32 + +// user gains +#define EE_XTRACK_GAIN 0x34 +#define EE_XTRACK_ANGLE 0x36 +#define EE_ALT_MIX 0x3B +#define EE_HEAD_MAX 0x38 +#define EE_PITCH_MAX 0x39 +#define EE_PITCH_MIN 0x3A +#define EE_KP 0x40 +#define EE_KI 0x60 +#define EE_KD 0x80 +#define EE_IMAX 0xA0 +#define EE_KFF 0xC0 +#define EE_AN_OFFSET 0xE0 + +//mission specific +#define EE_CONFIG 0X0F8 +#define EE_WP_MODE 0x0F9 +#define EE_YAW_MODE 0x0FA // not used +#define EE_WP_TOTAL 0x0FB +#define EE_WP_INDEX 0x0FC +#define EE_WP_RADIUS 0x0FD +#define EE_LOITER_RADIUS 0x0FE +#define EE_ALT_HOLD_HOME 0x0FF + +// user configs +#define EE_AIRSPEED_CRUISE 0x103 +#define EE_AIRSPEED_RATIO 0x104 +#define EE_AIRSPEED_FBW_MIN 0x108 +#define EE_AIRSPEED_FBW_MAX 0x109 +#define EE_THROTTLE_MIN 0x10A +#define EE_THROTTLE_CRUISE 0x10B +#define EE_THROTTLE_MAX 0x10C +#define EE_THROTTLE_FAILSAFE 0x10D +#define EE_THROTTLE_FS_VALUE 0x10E +#define EE_THROTTLE_FAILSAFE_ACTION 0x110 +#define EE_FLIGHT_MODE_CHANNEL 0x112 +#define EE_AUTO_TRIM 0x113 +#define EE_LOG_BITMASK 0x114 +#define EE_REVERSE_SWITCH 0x120 +#define EE_FLIGHT_MODES 0x121 + +// sensors +#define EE_ABS_PRESS_GND 0x116 +#define EE_GND_TEMP 0x11A +#define EE_GND_ALT 0x11C +#define EE_AP_OFFSET 0x11E + +// log +#define EE_LAST_LOG_PAGE 0xE00 +#define EE_LAST_LOG_NUM 0xE02 +#define EE_LOG_1_START 0xE04 + +// bits in log_bitmask +#define LOGBIT_ATTITUDE_FAST (1<<0) +#define LOGBIT_ATTITUDE_MED (1<<1) +#define LOGBIT_GPS (1<<2) +#define LOGBIT_PM (1<<3) +#define LOGBIT_CTUN (1<<4) +#define LOGBIT_NTUN (1<<5) +#define LOGBIT_MODE (1<<6) +#define LOGBIT_RAW (1<<7) +#define LOGBIT_CMD (1<<8) + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/events.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/events.pde new file mode 100644 index 0000000000..c8f94d4f23 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/events.pde @@ -0,0 +1,226 @@ +/* + This event will be called when the failsafe changes + boolean failsafe reflects the current state +*/ +void failsafe_event() +{ + if (failsafe == true){ + + // This is how to handle a failsafe. + switch(control_mode) + { + case MANUAL: // First position + set_mode(STABILIZE); + break; + + case STABILIZE: + case FLY_BY_WIRE_A: // middle position + case FLY_BY_WIRE_B: // middle position + set_mode(RTL); + throttle_cruise = THROTTLE_CRUISE; + + case CIRCLE: // middle position + break; + + case AUTO: // middle position + case LOITER: // middle position + if (throttle_failsafe_action == 1){ + set_mode(RTL); + throttle_cruise = THROTTLE_CRUISE; + } + // 2 = Stay in AUTO and ignore failsafe + break; + + case RTL: // middle position + break; + + default: + set_mode(RTL); + throttle_cruise = THROTTLE_CRUISE; + break; + } + }else{ + reset_I(); + } +} + +void low_battery_event(void) +{ + send_message(SEVERITY_HIGH,"Low Battery!"); + set_mode(RTL); + throttle_cruise = THROTTLE_CRUISE; +} + + +/* +4 simultaneous events +int event_original - original time in seconds +int event_countdown - count down to zero +byte event_countdown - ID for what to do +byte event_countdown - how many times to loop, 0 = indefinite +byte event_value - specific information for an event like PWM value +byte counterEvent - undo the event if nescessary + +count down each one + + +new event +undo_event +*/ + +void new_event(struct Location *cmd) +{ + SendDebug("New Event Loaded "); + SendDebugln(cmd->p1,DEC); + + + if(cmd->p1 == STOP_REPEAT){ + SendDebugln("STOP repeat "); + event_id = NO_REPEAT; + event_timer = -1; + undo_event = 0; + event_value = 0; + event_delay = 0; + event_repeat = 0; // convert seconds to millis + event_undo_value = 0; + repeat_forever = 0; + }else{ + // reset the event timer + event_timer = millis(); + event_id = cmd->p1; + event_value = cmd->alt; + event_delay = cmd->lat; + event_repeat = cmd->lng; // convert seconds to millis + event_undo_value = 0; + repeat_forever = (event_repeat == 0) ? 1:0; + } + + /* + Serial.print("event_id: "); + Serial.println(event_id,DEC); + Serial.print("event_value: "); + Serial.println(event_value,DEC); + Serial.print("event_delay: "); + Serial.println(event_delay,DEC); + Serial.print("event_repeat: "); + Serial.println(event_repeat,DEC); + Serial.print("event_undo_value: "); + Serial.println(event_undo_value,DEC); + Serial.print("repeat_forever: "); + Serial.println(repeat_forever,DEC); + Serial.print("Event_timer: "); + Serial.println(event_timer,DEC); + */ + perform_event(); +} + +void perform_event() +{ + if (event_repeat > 0){ + event_repeat --; + } + switch(event_id) { + case CH_4_TOGGLE: + event_undo_value = readOutputCh(4); + + APM_RC.OutputCh(4, event_value); // send to Servos + undo_event = 2; + break; + case CH_5_TOGGLE: + event_undo_value = readOutputCh(5); + APM_RC.OutputCh(5, event_value); // send to Servos + undo_event = 2; + break; + case CH_6_TOGGLE: + event_undo_value = readOutputCh(6); + APM_RC.OutputCh(6, event_value); // send to Servos + undo_event = 2; + break; + case CH_7_TOGGLE: + event_undo_value = readOutputCh(7); + APM_RC.OutputCh(7, event_value); // send to Servos + undo_event = 2; + event_undo_value = 1; + break; + case RELAY_TOGGLE: + + event_undo_value = PORTL & B00000100 ? 1:0; + if(event_undo_value == 1){ + relay_A(); + }else{ + relay_B(); + } + SendDebug("toggle relay "); + SendDebugln(PORTL,BIN); + undo_event = 2; + break; + + } +} + +void relay_A() +{ + PORTL |= B00000100; +} + +void relay_B() +{ + PORTL ^= B00000100; +} + +void update_events() +{ + // repeating events + if(undo_event == 1){ + perform_event_undo(); + undo_event = 0; + }else if(undo_event > 1){ + undo_event --; + } + + if(event_timer == -1) + return; + + if((millis() - event_timer) > event_delay){ + perform_event(); + + if(event_repeat > 0 || repeat_forever == 1){ + event_repeat--; + event_timer = millis(); + }else{ + event_timer = -1; + } + } +} + +void perform_event_undo() +{ + switch(event_id) { + case CH_4_TOGGLE: + APM_RC.OutputCh(4, event_undo_value); // send to Servos + break; + + case CH_5_TOGGLE: + APM_RC.OutputCh(5, event_undo_value); // send to Servos + break; + + case CH_6_TOGGLE: + APM_RC.OutputCh(6, event_undo_value); // send to Servos + break; + + case CH_7_TOGGLE: + APM_RC.OutputCh(7, event_undo_value); // send to Servos + break; + + case RELAY_TOGGLE: + + if(event_undo_value == 1){ + relay_A(); + }else{ + relay_B(); + } + SendDebug("untoggle relay "); + SendDebugln(PORTL,BIN); + break; + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde new file mode 100644 index 0000000000..e7dbf3d2df --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde @@ -0,0 +1,230 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +//**************************************************************** +// Function that will calculate the desired direction to fly and altitude error +//**************************************************************** +void navigate() +{ + // do not navigate with corrupt data + // --------------------------------- + if (GPS.fix == 0) + { + GPS.new_data = false; + return; + } + if(next_WP.lat == 0){ + return; + } + + // We only perform most nav computations if we have new gps data to work with + // -------------------------------------------------------------------------- + if(GPS.new_data){ + GPS.new_data = false; + + // target_bearing is where we should be heading + // -------------------------------------------- + target_bearing = get_bearing(¤t_loc, &next_WP); + + // nav_bearing will includes xtrac correction + // ------------------------------------------ + nav_bearing = target_bearing; + + // waypoint distance from plane + // ---------------------------- + wp_distance = getDistance(¤t_loc, &next_WP); + + if (wp_distance < 0){ + send_message(SEVERITY_HIGH," WP error - distance < 0"); + //Serial.println(wp_distance,DEC); + //print_current_waypoints(); + return; + } + + // check if we have missed the WP + loiter_delta = (target_bearing - old_target_bearing)/100; + + // reset the old value + old_target_bearing = target_bearing; + + // wrap values + if (loiter_delta > 180) loiter_delta -= 360; + if (loiter_delta < -180) loiter_delta += 360; + loiter_sum += abs(loiter_delta); + + calc_bearing_error(); + + // control mode specific updates to nav_bearing + update_navigation(); + } +} + +void update_navigation() +{ + // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS + // ------------------------------------------------------------------------ + + // distance and bearing calcs only + if(control_mode == AUTO){ + verify_must(); + verify_may(); + }else{ + + switch(control_mode){ + case LOITER: + float power; + if (wp_distance <= loiter_radius){ + nav_bearing -= 9000; + + }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ + power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0, 1); + nav_bearing -= power * 9000; + + }else{ + update_crosstrack(); + } + nav_bearing = wrap_360(nav_bearing); + calc_bearing_error(); + break; + + case RTL: + if(wp_distance <= (loiter_radius + 10)) { // + 10 meters + set_mode(LOITER); + }else{ + update_crosstrack(); + } + break; + } + } +} + + +/* +Disabled for now +void calc_distance_error() +{ + //distance_estimate += (float)GPS.ground_speed * .0002 * cos(radians(bearing_error * .01)); + //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); + //wp_distance = max(distance_estimate,10); +} +*/ + +void calc_airspeed_errors() +{ + airspeed_error = airspeed_cruise - airspeed; + airspeed_energy_error = (long)(((long)airspeed_cruise * (long)airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation +} + +void calc_bearing_error() +{ + bearing_error = nav_bearing - yaw_sensor; + bearing_error = wrap_180(bearing_error); +} + +void calc_altitude_error() +{ + // limit climb rates + target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30)); + if(prev_WP.alt > next_WP.alt){ + target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); + }else{ + target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); + } + + /* + // Disabled for now + #if AIRSPEED_SENSOR == 1 + // special thanks to Ryan Beall for this one + float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) + pitch_angle = constrain(pitch_angle, -2000, 2000); + float scale = sin(radians(pitch_angle * .01)); + altitude_estimate += (float)airspeed * .0002 * scale; + altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); + + // compute altitude error for throttle control + altitude_error = target_altitude - altitude_estimate; + #else + altitude_error = target_altitude - current_loc.alt; + #endif + */ + + altitude_error = target_altitude - current_loc.alt; +} + + +long wrap_360(long error) +{ + if (error > 36000) error -= 36000; + if (error < 0) error += 36000; + return error; +} + +long wrap_180(long error) +{ + if (error > 18000) error -= 36000; + if (error < -18000) error += 36000; + return error; +} + +/* +// disabled for now +void update_loiter() +{ + loiter_delta = (target_bearing - old_target_bearing)/100; + // reset the old value + old_target_bearing = target_bearing; + // wrap values + if (loiter_delta > 170) loiter_delta -= 360; + if (loiter_delta < -170) loiter_delta += 360; + loiter_sum += loiter_delta; +}*/ + +void update_crosstrack(void) +{ + // Crosstrack Error + // ---------------- + if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line + nav_bearing += constrain(crosstrack_error * x_track_gain, -x_track_angle, x_track_angle); + nav_bearing = wrap_360(nav_bearing); + } +} + +void reset_crosstrack() +{ + crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following +} + +int get_altitude_above_home(void) +{ + // This is the altitude above the home location + // The GPS gives us altitude at Sea Level + // if you slope soar, you should see a negative number sometimes + // ------------------------------------------------------------- + return current_loc.alt - home.alt; +} + +long getDistance(struct Location *loc1, struct Location *loc2) +{ + if(loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; + return sqrt(sq(dlat) + sq(dlong)) * .01113195; +} + +long get_alt_distance(struct Location *loc1, struct Location *loc2) +{ + return abs(loc1->alt - loc2->alt); +} + +long get_bearing(struct Location *loc1, struct Location *loc2) +{ + long off_x = loc2->lng - loc1->lng; + long off_y = (loc2->lat - loc1->lat) * scaleLongUp; + long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) bearing += 36000; + return bearing; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde new file mode 100644 index 0000000000..89b81648e5 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde @@ -0,0 +1,180 @@ +//Function that will read the radio data, limit servos and trigger a failsafe +// ---------------------------------------------------------------------------- +byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling + +void read_radio() +{ + ch1_temp = APM_RC.InputCh(CH_ROLL); + ch2_temp = APM_RC.InputCh(CH_PITCH); + + if(mix_mode == 0){ + radio_in[CH_ROLL] = ch1_temp; + radio_in[CH_PITCH] = ch2_temp; + }else{ + radio_in[CH_ROLL] = reverse_elevons * (reverse_ch2_elevon * (ch2_temp - elevon2_trim) - reverse_ch1_elevon * (ch1_temp - elevon1_trim)) / 2 + 1500; + radio_in[CH_PITCH] = (reverse_ch2_elevon * (ch2_temp - elevon2_trim) + reverse_ch1_elevon * (ch1_temp - elevon1_trim)) / 2 + 1500; + } + + for (int y = 2; y < 8; y++) + radio_in[y] = APM_RC.InputCh(y); + + #if THROTTLE_REVERSE == 1 + radio_in[CH_THROTTLE] = radio_max[CH_THROTTLE] + radio_min[CH_THROTTLE] - radio_in[CH_THROTTLE]; + #endif + + throttle_failsafe(radio_in[CH_THROTTLE]); + servo_out[CH_THROTTLE] = ((float)(radio_in[CH_THROTTLE] - radio_min[CH_THROTTLE]) / (float)(radio_max[CH_THROTTLE] - radio_min[CH_THROTTLE])) * 100; + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); +} + +void throttle_failsafe(int pwm) +{ + if(throttle_failsafe_enabled == 0) + return; + + //check for failsafe and debounce funky reads + // ------------------------------------------ + if (pwm < throttle_failsafe_value){ + // we detect a failsafe from radio + // throttle has dropped below the mark + failsafeCounter++; + if (failsafeCounter == 9){ + SendDebug("MSG FS ON "); + SendDebugln(pwm, DEC); + }else if(failsafeCounter == 10) { + ch3_failsafe = true; + //set_failsafe(true); + //failsafeCounter = 10; + }else if (failsafeCounter > 10){ + failsafeCounter = 11; + } + + }else if(failsafeCounter > 0){ + // we are no longer in failsafe condition + // but we need to recover quickly + failsafeCounter--; + if (failsafeCounter > 3){ + failsafeCounter = 3; + } + if (failsafeCounter == 1){ + SendDebug("MSG FS OFF "); + SendDebugln(pwm, DEC); + }else if(failsafeCounter == 0) { + ch3_failsafe = false; + //set_failsafe(false); + //failsafeCounter = -1; + }else if (failsafeCounter <0){ + failsafeCounter = -1; + } + } +} + +void trim_control_surfaces() +{ + // Store control surface trim values + // --------------------------------- + if(mix_mode == 0){ + radio_trim[CH_ROLL] = radio_in[CH_ROLL]; + radio_trim[CH_PITCH] = radio_in[CH_PITCH]; + radio_trim[CH_RUDDER] = radio_in[CH_RUDDER]; + }else{ + elevon1_trim = ch1_temp; + elevon2_trim = ch2_temp; + //Recompute values here using new values for elevon1_trim and elevon2_trim + //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed + radio_trim[CH_ROLL] = 1500; + radio_trim[CH_PITCH] = 1500; + } + // disabled for now + //save_EEPROM_trims(); +} + +void trim_radio() +{ + for (int y = 0; y < 50; y++) { + read_radio(); + } + + // Store the trim values + // --------------------- + if(mix_mode == 0){ + for (int y = 0; y < 8; y++) radio_trim[y] = radio_in[y]; + }else{ + elevon1_trim = ch1_temp; + elevon2_trim = ch2_temp; + radio_trim[CH_ROLL] = 1500; + radio_trim[CH_PITCH] = 1500; + for (int y = 2; y < 8; y++) radio_trim[y] = radio_in[y]; + } + save_EEPROM_trims(); +} + + +#if SET_RADIO_LIMITS == 1 +void read_radio_limits() +{ + // set initial servo limits for calibration routine + // ------------------------------------------------- + radio_min[CH_ROLL] = radio_in[CH_ROLL] - 150; + radio_max[CH_ROLL] = radio_in[CH_ROLL] + 150; + + radio_min[CH_PITCH] = radio_in[CH_PITCH] - 150; + radio_max[CH_PITCH] = radio_in[CH_PITCH] + 150; + + // vars for the radio config routine + // --------------------------------- + int counter = 0; + long reminder; + reminder = millis() - 10000; + + // Allows user to set stick limits and calibrate the IR + // ---------------------------------------------------- + while(counter < 50){ + + if (millis() - reminder >= 10000) { // Remind user every 10 seconds what is going on + send_message(SEVERITY_LOW,"Reading radio limits:"); + send_message(SEVERITY_LOW,"Move sticks to: upper right and lower Left"); + send_message(SEVERITY_LOW,"To Continue, hold the stick in the corner for 2 seconds."); + send_message(SEVERITY_LOW," "); + //print_radio(); + demo_servos(1); + reminder = millis(); + } + + delay(40); + read_radio(); + + // AutoSet servo limits + // -------------------- + if (radio_in[CH_ROLL] > 1000 && radio_in[CH_ROLL] < 2000){ + radio_min[CH_ROLL] = min(radio_in[CH_ROLL], radio_min[CH_ROLL]); + radio_max[CH_ROLL] = max(radio_in[CH_ROLL], radio_max[CH_ROLL]); + } + + if (radio_in[CH_PITCH] > 1000 && radio_in[CH_PITCH]< 2000){ + radio_min[CH_PITCH] = min(radio_in[CH_PITCH], radio_min[CH_PITCH]); + radio_max[CH_PITCH] = max(radio_in[CH_PITCH], radio_max[CH_PITCH]); + } + if(radio_in[CH_PITCH] < (radio_min[CH_PITCH] + 30) || radio_in[CH_PITCH] > (radio_max[CH_PITCH] -30)){ + SendDebug("."); + counter++; + }else{ + if (counter > 0) + counter--; + } + } + + // contstrain min values + // --------------------- + radio_min[CH_ROLL] = constrain(radio_min[CH_ROLL], 800, 2200); + radio_max[CH_ROLL] = constrain(radio_max[CH_ROLL], 800, 2200); + radio_min[CH_PITCH] = constrain(radio_min[CH_PITCH], 800, 2200); + radio_max[CH_PITCH] = constrain(radio_max[CH_PITCH], 800, 2200); + + SendDebugln(" "); +} +#endif + + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde new file mode 100644 index 0000000000..c0b12e618e --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde @@ -0,0 +1,65 @@ +void ReadSCP1000(void) {} + + +void read_airpressure(void){ + double x; + + APM_BMP085.Read(); // Get new data from absolute pressure sensor + abs_press = APM_BMP085.Press; + abs_press_filt = (abs_press); // + 2l * abs_press_filt) / 3l; // Light filtering + //temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation + + double p = (double)abs_press_gnd / (double)abs_press_filt; + double temp = (float)ground_temperature / 10.f + 273.15f; + x = log(p) * temp * 29271.267f; + //x = log(p) * temp * 29.271267 * 1000; + press_alt = (long)(x / 10) + ground_alt; // Pressure altitude in centimeters + // Need to add comments for theory..... +} + +// in M/S * 100 +void read_airspeed(void) +{ + #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed + airpressure_raw = ((float)APM_ADC.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75); + airpressure = (int)airpressure_raw - airpressure_offset; + airpressure = max(airpressure, 0); + airspeed = sqrt((float)airpressure * airspeed_ratio) * 100; + #endif + + calc_airspeed_errors(); +} + +#if BATTERY_EVENT == 1 +void read_battery(void) +{ + battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; + battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; + battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; + battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; + + #if BATTERY_TYPE == 0 + if(battery_voltage3 < LOW_VOLTAGE) + low_battery_event(); + battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream + #endif + + #if BATTERY_TYPE == 1 + if(battery_voltage4 < LOW_VOLTAGE) + low_battery_event(); + battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream + #endif +} +#endif + +void zero_airspeed(void) +{ + airpressure_raw = (float)APM_ADC.Ch(AIRSPEED_CH); + for(int c = 0; c < 50; c++){ + delay(20); + airpressure_raw = (airpressure_raw * .90) + ((float)APM_ADC.Ch(AIRSPEED_CH) * .10); + } + airpressure_offset = airpressure_raw; +} + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde new file mode 100644 index 0000000000..4f29457ee1 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde @@ -0,0 +1,410 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Functions called from the setup menu +static int8_t setup_radio(uint8_t argc, const Menu::arg *argv); +static int8_t setup_show(uint8_t argc, const Menu::arg *argv); +static int8_t setup_factory(uint8_t argc, const Menu::arg *argv); +static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv); + +// Command/function table for the setup menu +const struct Menu::command setup_menu_commands[] PROGMEM = { + // command function called + // ======= =============== + {"reset", setup_factory}, + {"radio", setup_radio}, + {"modes", setup_flightmodes}, + {"show", setup_show}, +}; + +// Create the setup menu object. +MENU(setup_menu, "setup", setup_menu_commands); + +// Called from the top-level menu to run the setup menu. +int8_t +setup_mode(uint8_t argc, const Menu::arg *argv) +{ + // Give the user some guidance + Serial.printf_P(PSTR("Setup Mode\n" + "\n" + "IMPORTANT: if you have not previously set this system up, use the\n" + "'reset' command to initialize the EEPROM to sensible default values\n" + "and then the 'radio' command to configure for your radio.\n" + "\n")); + + // Run the setup menu. When the menu exits, we will return to the main menu. + setup_menu.run(); +} + +// Print the current configuration. +// Called by the setup menu 'show' command. +static int8_t +setup_show(uint8_t argc, const Menu::arg *argv) +{ + uint8_t i; + + Serial.printf_P(PSTR("\nRadio:\n")); + read_EEPROM_radio_minmax(); + for(i = 0; i < 8; i++) + Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min[i], radio_max[i]); + + Serial.printf_P(PSTR("\nGains:\n")); + read_EEPROM_gains(); + + for(i = 0; i < 8; i++){ + Serial.printf_P(PSTR("P,I,D,iMax:")); + Serial.print(kp[i],3); + Serial.print(","); + Serial.print(ki[i],3); + Serial.print(","); + Serial.print(kd[i],3); + Serial.print(","); + Serial.println(integrator_max[i]/100,DEC); + } + + Serial.printf_P(PSTR("kff:")); + Serial.print(kff[0],3); + Serial.print(","); + Serial.print(kff[1],3); + Serial.print(","); + Serial.println(kff[2],3); + + Serial.printf_P(PSTR("XTRACK_GAIN:")); + Serial.println(x_track_gain,DEC); + + Serial.printf_P(PSTR("XTRACK_ENTRY_ANGLE: %d\n"), x_track_angle); + Serial.printf_P(PSTR("HEAD_MAX: %d\n"), head_max); + Serial.printf_P(PSTR("PITCH_MAX: %d\n"), pitch_max); + Serial.printf_P(PSTR("PITCH_MIN: %d\n"), pitch_min); + + read_user_configs(); + Serial.printf_P(PSTR("\nUser config:\n")); + Serial.printf_P(PSTR("airspeed_cruise: %d\n"), airspeed_cruise); + Serial.printf_P(PSTR("airspeed_fbw_min: %d\n"), airspeed_fbw_min); + Serial.printf_P(PSTR("airspeed_fbw_max: %d\n"), airspeed_fbw_max); + Serial.printf_P(PSTR("airspeed_ratio: ")); + Serial.println(airspeed_ratio, 4); + + Serial.printf_P(PSTR("throttle_min: %d\n"), throttle_min); + Serial.printf_P(PSTR("throttle_max: %d\n"), throttle_max); + Serial.printf_P(PSTR("throttle_cruise: %d\n"), throttle_cruise); + Serial.printf_P(PSTR("throttle_failsafe_enabled: %d\n"), throttle_failsafe_enabled); + Serial.printf_P(PSTR("throttle_failsafe_value: %d\n"), throttle_failsafe_value); + Serial.printf_P(PSTR("flight_mode_channel: %d\n"), flight_mode_channel+1); //Add 1 to flight_mode_channel to change 0-based channels to 1-based channels + Serial.printf_P(PSTR("auto_trim: %d\n"), auto_trim); + Serial.printf_P(PSTR("log_bitmask: %d\n\n"), log_bitmask); + //Serial.printf_P(PSTR("Switch settings:\n")); + //for(i = 0; i < 6; i++){ + // print_switch(i+1, flight_modes[i]); + //} + + return(0); +} + +// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). +// Called by the setup menu 'factoryreset' command. +static int8_t +setup_factory(uint8_t argc, const Menu::arg *argv) +{ + uint8_t i; + int c; + + Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); + + do { + c = Serial.read(); + } while (-1 == c); + + if (('y' != c) && ('Y' != c)) + return(-1); + + Serial.printf_P(PSTR("\nFACTORY RESET\n\n")); + + wp_radius = WP_RADIUS_DEFAULT; + loiter_radius = LOITER_RADIUS_DEFAULT; + + save_EEPROM_waypoint_info(); + + radio_min[CH_1] = 0; + radio_min[CH_2] = 0; + radio_min[CH_3] = 0; + radio_min[CH_4] = 0; + radio_min[CH_5] = CH5_MIN; + radio_min[CH_6] = CH6_MIN; + radio_min[CH_7] = CH7_MIN; + radio_min[CH_8] = CH8_MIN; + + radio_max[CH_1] = 0; + radio_max[CH_2] = 0; + radio_max[CH_3] = 0; + radio_max[CH_4] = 0; + radio_max[CH_5] = CH5_MAX; + radio_max[CH_6] = CH6_MAX; + radio_max[CH_7] = CH7_MAX; + radio_max[CH_8] = CH8_MAX; + + save_EEPROM_radio_minmax(); + + kp[0] = SERVO_ROLL_P; + kp[1] = SERVO_PITCH_P; + kp[2] = SERVO_YAW_P; + kp[3] = NAV_ROLL_P; + kp[4] = NAV_PITCH_ASP_P; + kp[5] = NAV_PITCH_ALT_P; + kp[6] = THROTTLE_TE_P; + kp[7] = THROTTLE_ALT_P; + + ki[0] = SERVO_ROLL_I; + ki[1] = SERVO_PITCH_I; + ki[2] = SERVO_YAW_I; + ki[3] = NAV_ROLL_I; + ki[4] = NAV_PITCH_ASP_I; + ki[5] = NAV_PITCH_ALT_I; + ki[6] = THROTTLE_TE_I; + ki[7] = THROTTLE_ALT_I; + + kd[0] = SERVO_ROLL_D; + kd[1] = SERVO_PITCH_D; + kd[2] = SERVO_YAW_D; + kd[3] = NAV_ROLL_D; + kd[4] = NAV_PITCH_ASP_D; + kd[5] = NAV_PITCH_ALT_D; + kd[6] = THROTTLE_TE_D; + kd[7] = THROTTLE_ALT_D; + + integrator_max[0] = SERVO_ROLL_INT_MAX; + integrator_max[1] = SERVO_PITCH_INT_MAX; + integrator_max[2] = SERVO_YAW_INT_MAX; + integrator_max[3] = NAV_ROLL_INT_MAX; + integrator_max[4] = NAV_PITCH_ASP_INT_MAX; + integrator_max[5] = NAV_PITCH_ALT_INT_MAX; + integrator_max[6] = THROTTLE_TE_INT_MAX; + integrator_max[7] = THROTTLE_ALT_INT_MAX; + + kff[0] = PITCH_COMP; + kff[1] = RUDDER_MIX; + kff[2] = P_TO_T; + + for(i = 0; i < 8; i++){ + // scale input to deg * 100; + integrator_max[i] *= 100; + } + x_track_gain = XTRACK_GAIN * 100; + x_track_angle = XTRACK_ENTRY_ANGLE * 100; + altitude_mix = ALTITUDE_MIX; + + head_max = HEAD_MAX * 100; + pitch_max = PITCH_MAX * 100; + pitch_min = PITCH_MIN * 100; + + save_EEPROM_gains(); + + airspeed_cruise = AIRSPEED_CRUISE*100; + airspeed_fbw_min = AIRSPEED_FBW_MIN; + airspeed_fbw_max = AIRSPEED_FBW_MAX; + airspeed_ratio = AIRSPEED_RATIO; + throttle_min = THROTTLE_MIN; + throttle_max = THROTTLE_MAX; + throttle_cruise = THROTTLE_CRUISE; + throttle_failsafe_enabled = THROTTLE_FAILSAFE; + throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; + throttle_failsafe_value = THROTTLE_FS_VALUE; + //flight_mode_channel = FLIGHT_MODE_CHANNEL - 1; + auto_trim = AUTO_TRIM; + + flight_modes[0] = FLIGHT_MODE_1; + flight_modes[1] = FLIGHT_MODE_2; + flight_modes[2] = FLIGHT_MODE_3; + flight_modes[3] = FLIGHT_MODE_4; + flight_modes[4] = FLIGHT_MODE_5; + flight_modes[5] = FLIGHT_MODE_6; + save_EEPROM_flight_modes(); + + // convenience macro for testing LOG_* and setting LOGBIT_* +#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) + log_bitmask = + LOGBIT(ATTITUDE_FAST) | + LOGBIT(ATTITUDE_MED) | + LOGBIT(GPS) | + LOGBIT(PM) | + LOGBIT(CTUN) | + LOGBIT(NTUN) | + LOGBIT(MODE) | + LOGBIT(RAW) | + LOGBIT(CMD); +#undef LOGBIT + + save_user_configs(); + + return(0); +} + +// Perform radio setup. +// Called by the setup menu 'radio' command. +static int8_t +setup_radio(uint8_t argc, const Menu::arg *argv) +{ + Serial.println("\n\nRadio Setup:"); + uint8_t i; + + for(i = 0; i < 100;i++){ + delay(20); + read_radio(); + } + + if(radio_in[CH_ROLL] < 500){ + while(1){ + Serial.print("Radio error"); + delay(1000); + // stop here + } + } + + for(i = 0; i < 4; i++){ + radio_min[i] = radio_in[i]; + radio_max[i] = radio_in[i]; + } + + Serial.printf_P(PSTR("\nMove both sticks to each corner. Hit Enter to save: ")); + while(1){ + + delay(20); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + for(i = 0; i < 4; i++){ + radio_min[i] = min(radio_min[i], radio_in[i]); + radio_max[i] = max(radio_max[i], radio_in[i]); + } + + if(Serial.available() > 0){ + Serial.flush(); + Serial.println("Saving:"); + + save_EEPROM_radio_minmax(); + delay(100); + read_EEPROM_radio_minmax(); + + for(i = 0; i < 8; i++) + Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min[i], radio_max[i]); + break; + } + } + + return(0); +} + + +static int8_t +setup_flightmodes(uint8_t argc, const Menu::arg *argv) +{ + byte switchPosition, oldSwitchPosition, mode; + + Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); + print_hit_enter(); + trim_radio(); + + while(1){ + delay(20); + read_radio(); + switchPosition = readSwitch(); + + + // look for control switch change + if (oldSwitchPosition != switchPosition){ + + // Override position 5 + if(switchPosition > 4){ + mode = flight_modes[switchPosition] = MANUAL; + }else{ + // update our current mode + mode = flight_modes[switchPosition]; + } + + // update the user + print_switch(switchPosition, mode); + + // Remember switch position + oldSwitchPosition = switchPosition; + } + + // look for stick input + if (radio_input_switch() == true){ + switch(mode){ + case MANUAL: + mode = STABILIZE; + break; + + case STABILIZE: + mode = FLY_BY_WIRE_A; + break; + + case FLY_BY_WIRE_A: + mode = FLY_BY_WIRE_B; + break; + + case FLY_BY_WIRE_B: + mode = AUTO; + break; + + case AUTO: + mode = RTL; + break; + + case RTL: + mode = LOITER; + break; + + case LOITER: + mode = MANUAL; + break; + + default: + mode = MANUAL; + break; + } + + // Override position 5 + if(switchPosition > 4) + mode = MANUAL; + + // save new mode + flight_modes[switchPosition] = mode; + + // print new mode + print_switch(switchPosition, mode); + } + + // escape hatch + if(Serial.available() > 0){ + save_EEPROM_flight_modes(); + return (0); + } + } +} + +void +print_switch(byte p, byte m) +{ + Serial.printf_P(PSTR("Pos %d: "),p); + Serial.println(flight_mode_strings[m]); +} + +boolean +radio_input_switch(void) +{ + static byte bouncer; + + if (abs(radio_in[0] - radio_trim[0]) > 200) + bouncer = 10; + + if (bouncer > 0) + bouncer--; + + if (bouncer == 1){ + return true; + }else{ + return false; + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/system.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/system.pde new file mode 100644 index 0000000000..56a44067da --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/system.pde @@ -0,0 +1,491 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +/***************************************************************************** +The init_ardupilot function processes everything we need for an in - air restart + We will determine later if we are actually on the ground and process a + ground start in that case. + +*****************************************************************************/ + +// Functions called from the top-level menu +extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Commands:\n" + " logs log readback/setup mode\n" + " setup setup mode\n" + " test test mode\n" + "\n" + "Move the slide switch and reset to FLY.\n" + "\n")); + return(0); +} + +// Command/function table for the top-level menu. +const struct Menu::command main_menu_commands[] PROGMEM = { +// command function called +// ======= =============== + {"logs", process_logs}, + {"setup", setup_mode}, + {"test", test_mode}, + {"help", main_menu_help} +}; + +// Create the top-level menu object. +MENU(main_menu, "ArduPilotMega", main_menu_commands); + +void init_ardupilot() +{ + + byte last_log_num; + int last_log_start; + int last_log_end; + + // Console serial port + // + // The console port buffers are defined to be sufficiently large to support + // the console's use as a logging device, optionally as the GPS port when + // GPS_PROTOCOL_IMU is selected, and as the telemetry port. + // + // XXX This could be optimised to reduce the buffer sizes in the cases + // where they are not otherwise required. + // + Serial.begin(SERIAL0_BAUD, 128, 128); + + // GPS serial port. + // + // Not used if the IMU/X-Plane GPS is in use. + // + // XXX currently the EM406 (SiRF receiver) is nominally configured + // at 57600, however it's not been supported to date. We should + // probably standardise on 38400. + // + // XXX the 128 byte receive buffer may be too small for NMEA, depending + // on the message set configured. + // +#if GPS_PROTOCOL != GPS_PROTOCOL_IMU + Serial1.begin(38400, 128, 16); +#endif + + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // +#if GCS_PORT == 3 + Serial3.begin(SERIAL3_BAUD, 128, 128); +#endif + + Serial.printf_P(PSTR("\n\n" + "Init ArduPilotMega X-DIY\n\n" +#if TELEMETRY_PORT == 3 + "Telemetry is on the xbee port\n" +#endif + "freeRAM: %d\n"), freeRAM()); + + APM_RC.Init(); // APM Radio initialization + read_EEPROM_startup(); // Read critical config information to start + + APM_ADC.Init(); // APM ADC library initialization + APM_BMP085.Init(); // APM Abs Pressure sensor initialization + DataFlash.Init(); // DataFlash log initialization + GPS.init(); // GPS Initialization + + #if MAGNETOMETER == ENABLED + APM_Compass.Init(); // I2C initialization + #endif + + APM_RC.OutputCh(CH_ROLL, radio_trim[CH_ROLL]); // Initialization of servo outputs + APM_RC.OutputCh(CH_PITCH, radio_trim[CH_PITCH]); + APM_RC.OutputCh(CH_THROTTLE, radio_trim[CH_THROTTLE]); + APM_RC.OutputCh(CH_RUDDER, radio_trim[CH_RUDDER]); + + pinMode(C_LED_PIN, OUTPUT); // GPS status LED + pinMode(A_LED_PIN, OUTPUT); // GPS status LED + pinMode(B_LED_PIN, OUTPUT); // GPS status LED + pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode + pinMode(PUSHBUTTON_PIN, INPUT); // unused + + + // If the switch is in 'menu' mode, run the main menu. + // + // Since we can't be sure that the setup or test mode won't leave + // the system in an odd state, we don't let the user exit the top + // menu; they must reset in order to fly. + // + if (digitalRead(SLIDE_SWITCH_PIN) == 0) { + digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED + Serial.printf_P(PSTR("\n" + "Entering interactive setup mode...\n" + "\n" + "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" + "Type 'help' to list commands, 'exit' to leave a submenu.\n" + "Visit the 'setup' menu for first-time configuration.\n")); + for (;;) { + Serial.printf_P(PSTR("\n" + "Move the slide switch and reset to FLY.\n" + "\n")); + main_menu.run(); + } + } + + + if(log_bitmask > 0){ + // Here we will check on the length of the last log + // We don't want to create a bunch of little logs due to powering on and off + last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); + last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02)); + last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); + + if(last_log_num == 0) { + // The log space is empty. Start a write session on page 1 + DataFlash.StartWrite(1); + eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1)); + eeprom_write_word((uint16_t *) EE_LOG_1_START, (1)); + + } else if (last_log_end <= last_log_start + 10) { + // The last log is small. We consider it junk. Overwrite it. + DataFlash.StartWrite(last_log_start); + + } else { + // The last log is valid. Start a new log + if(last_log_num >= 19) { + Serial.println("Number of log files exceeds max. Log 19 will be overwritten."); + last_log_num --; + } + DataFlash.StartWrite(last_log_end + 1); + eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1)); + eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1)); + } + } + + // read in the flight switches + update_servo_switches(); + + if(DEBUG_SUBSYSTEM > 0){ + debug_subsystem(); + + } else if (ENABLE_AIR_START == 1) { + // Perform an air start and get back to flying + send_message(SEVERITY_LOW," AIR START"); + + // Get necessary data from EEPROM + //---------------- + read_EEPROM_airstart_critical(); + + // This delay is important for the APM_RC library to work. We need some time for the comm between the 328 and 1280 to be established. + int old_pulse = 0; + while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(flight_mode_channel)) > 5 || APM_RC.InputCh(flight_mode_channel) == 1000 || APM_RC.InputCh(flight_mode_channel) == 1200)) { + old_pulse = APM_RC.InputCh(flight_mode_channel); + delay(25); + } + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_AIRSTART_MSG); + reload_commands(); // Get set to resume AUTO from where we left off + + }else { + startup_ground(); + if (log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + } + + // set the correct flight mode + // --------------------------- + reset_control_switch(); +} + +//******************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//******************************************************************************** +void startup_ground(void) +{ + send_message(SEVERITY_LOW," GROUND START"); + + #if(GROUND_START_DELAY > 0) + send_message(SEVERITY_LOW," With Delay"); + delay(GROUND_START_DELAY * 1000); + #endif + + // Output waypoints for confirmation + // -------------------------------- + for(int i = 1; i < wp_total + 1; i++) { + send_message(MSG_COMMAND, i); + } + + // Makes the servos wiggle + // step 1 = 1 wiggle + // ----------------------- + demo_servos(1); + + //IMU ground start + //------------------------ +#if GPS_PROTOCOL != GPS_PROTOCOL_IMU + startup_IMU_ground(); +#endif + + + // read the radio to set trims + // --------------------------- + trim_radio(); + + #if AIRSPEED_SENSOR == 1 + // initialize airspeed sensor + // -------------------------- + zero_airspeed(); + send_message(SEVERITY_LOW," zero airspeed calibrated"); + #else + send_message(SEVERITY_LOW," NO airspeed"); + #endif + + // Save the settings for in-air restart + // ------------------------------------ + save_EEPROM_groundstart(); + + // initialize commands + // ------------------- + init_commands(); + +} + +void set_mode(byte mode) +{ + if(control_mode == mode){ + // don't switch modes if we are already in the correct mode. + return; + } + if(auto_trim > 0 || control_mode == MANUAL) + trim_control_surfaces(); + + control_mode = mode; + crash_timer = 0; + + switch(control_mode) + { + case MANUAL: + break; + + case STABILIZE: + break; + + case FLY_BY_WIRE_A: + break; + + case FLY_BY_WIRE_B: + break; + + case AUTO: + update_auto(); + break; + + case RTL: + return_to_launch(); + break; + + case LOITER: + loiter_at_location(); + break; + + case TAKEOFF: + break; + + case LAND: + break; + + default: + return_to_launch(); + break; + } + + // output control mode to the ground station + send_message(MSG_HEARTBEAT); + + if (log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); +} + +void set_failsafe(boolean mode) +{ + // only act on changes + // ------------------- + if(failsafe != mode){ + + // store the value so we don't trip the gate twice + // ----------------------------------------------- + failsafe = mode; + + if (failsafe == false){ + // We're back in radio contact + // --------------------------- + + // re-read the switch so we can return to our preferred mode + reset_control_switch(); + + // Reset control integrators + // --------------------- + reset_I(); + + }else{ + // We've lost radio contact + // ------------------------ + // nothing to do right now + } + + // Let the user know what's up so they can override the behavior + // ------------------------------------------------------------- + failsafe_event(); + } +} + + +void startup_IMU_ground(void) +{ + uint16_t store = 0; + int flashcount = 0; + // 12345678901234567890123456789012 + jeti_status(" **** INIT **** Warming up ADC"); + jeti_update(); + SendDebugln(" Warming up ADC..."); + + for(int c = 0; c < ADC_WARM_CYCLES; c++) + { + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, LOW); + delay(50); + Read_adc_raw(); + digitalWrite(C_LED_PIN, HIGH); + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, HIGH); + delay(50); + } + + // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! + // ----------------------- + demo_servos(2); + SendDebugln(" Beginning IMU calibration; do not move plane"); + // 12345678901234567890123456789012 + jeti_status(" **** INIT **** Do not move!!!"); + jeti_update(); + + + for(int y = 0; y <= 5; y++){ // Read first initial ADC values for offset. + AN_OFFSET[y] = AN[y]; + } + + APM_BMP085.Read(); // Get initial data from absolute pressure sensor + abs_press_gnd = APM_BMP085.Press; + ground_temperature = APM_BMP085.Temp; + delay(20); + // *********** + + for(int i = 0; i < 400; i++){ // We take some readings... + Read_adc_raw(); + for(int y = 0; y <= 5; y++) // Read initial ADC values for offset (averaging). + AN_OFFSET[y] = AN_OFFSET[y] * 0.9 + AN[y] * 0.1; + + APM_BMP085.Read(); // Get initial data from absolute pressure sensor + abs_press_gnd = (abs_press_gnd * 9l + APM_BMP085.Press) / 10l; + ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10; + + delay(20); + if(flashcount == 5) { + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, LOW); + } + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(C_LED_PIN, HIGH); + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, HIGH); + } + flashcount++; + + } + SendDebugln(" Calibration complete."); + digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + + AN_OFFSET[5] -= GRAVITY * SENSOR_SIGN[5]; +/* + Serial.print ("Offsets "); + Serial.print (AN_OFFSET[0]); + Serial.print (" "); + Serial.print (AN_OFFSET[1]); + Serial.print (" "); + Serial.print (AN_OFFSET[2]); + Serial.print (" "); + Serial.print (AN_OFFSET[3]); + Serial.print (" "); + Serial.print (AN_OFFSET[4]); + Serial.print (" "); + Serial.println (AN_OFFSET[5]); +*/ +} + + +void update_GPS_light(void) +{ + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + switch (GPS.status()) { + case(2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case(1): + if (GPS.valid_read == true){ + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light){ + digitalWrite(C_LED_PIN, LOW); + } else { + digitalWrite(C_LED_PIN, HIGH); + } + GPS.valid_read = false; + } + break; + + default: + digitalWrite(C_LED_PIN, LOW); + break; + } +} + + + +void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; + gyro_sat_count = 0; + adc_constraints = 0; + renorm_sqrt_count = 0; + renorm_blowup_count = 0; + gps_fix_count = 0; + perf_mon_timer = millis(); +} + + +/* This function gets the current value of the heap and stack pointers. +* The stack pointer starts at the top of RAM and grows downwards. The heap pointer +* starts just above the static variables etc. and grows upwards. SP should always +* be larger than HP or you'll be in big trouble! The smaller the gap, the more +* careful you need to be. Julian Gall 6 - Feb - 2009. +*/ +unsigned long freeRAM() { + uint8_t * heapptr, * stackptr; + stackptr = (uint8_t *)malloc(4); // use stackptr temporarily + heapptr = stackptr; // save value of heap pointer + free(stackptr); // free up the memory again (sets stackptr to 0) + stackptr = (uint8_t *)(SP); // save value of stack pointer + return stackptr - heapptr; +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde new file mode 100644 index 0000000000..d6791b4291 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde @@ -0,0 +1,433 @@ +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t test_radio(uint8_t argc, const Menu::arg *argv); +static int8_t test_gps(uint8_t argc, const Menu::arg *argv); +static int8_t test_imu(uint8_t argc, const Menu::arg *argv); +static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); +static int8_t test_battery(uint8_t argc, const Menu::arg *argv); +static int8_t test_relay(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp(uint8_t argc, const Menu::arg *argv); +static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); +static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); +static int8_t test_mag(uint8_t argc, const Menu::arg *argv); +static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); +static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); +static int8_t test_jeti(uint8_t argc, const Menu::arg *argv); + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("\n" + "Commands:\n" + " radio\n" + " servos\n" + " gps\n" + " imu\n" + " battery\n" + "\n")); +}*/ + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +const struct Menu::command test_menu_commands[] PROGMEM = { + {"radio", test_radio}, + {"gps", test_gps}, + {"imu", test_imu}, + {"gyro", test_gyro}, + {"battery", test_battery}, + {"relay", test_relay}, + {"waypoints", test_wp}, + {"airspeed", test_airspeed}, + {"airpressure", test_pressure}, + {"compass", test_mag}, + {"xbee", test_xbee}, + {"eedump", test_eedump}, + {"jeti", test_jeti}, +}; + +// A Macro to create the Menu +MENU(test_menu, "test", test_menu_commands); + +int8_t +test_mode(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Test Mode\n\n")); + test_menu.run(); +} + +static int8_t +test_eedump(uint8_t argc, const Menu::arg *argv) +{ + int i, j; + + // hexdump the EEPROM + for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { + Serial.printf_P(PSTR("%04x:"), i); + for (j = 0; j < 16; j++) + Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); + Serial.println(); + } + return(0); +} + +static int8_t +test_radio(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + #if THROTTLE_REVERSE == 1 + Serial.println("Throttle is reversed in config: "); + delay(1000); + #endif + + // read the radio to set trims + // --------------------------- + trim_radio(); + + while(1){ + delay(20); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + update_servo_switches(); + + servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; + servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; + servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; + + // write out the servo PWM values + // ------------------------------ + set_servos_4(); + + + for(int y = 4; y < 8; y++) { + radio_out[y] = constrain(radio_in[y], radio_min[y], radio_max[y]); + APM_RC.OutputCh(y, radio_out[y]); // send to Servos + } + Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\t"), radio_in[CH_1], radio_in[CH_2], radio_in[CH_3], radio_in[CH_4], radio_in[CH_5], radio_in[CH_6], radio_in[CH_7], radio_in[CH_8]); + Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (servo_out[CH_ROLL]/100), (servo_out[CH_PITCH]/100), servo_out[CH_THROTTLE], (servo_out[CH_RUDDER]/100)); + + if(Serial.available() > 0){ + return (0); + } + } +} + + +static int8_t +test_gps(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(333); + + // Blink GPS LED if we don't have a fix + // ------------------------------------ + update_GPS_light(); + + GPS.update(); + + if (GPS.new_data){ + Serial.print("Lat:"); + Serial.print((float)GPS.latitude/10000000, 10); + Serial.print(" Lon:"); + Serial.print((float)GPS.longitude/10000000, 10); + Serial.printf_P(PSTR("alt %dm, #sats: %d\n"), GPS.altitude/100, GPS.num_sats); + }else{ + Serial.print("."); + } + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_imu(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Calibrating.")); + startup_IMU_ground(); + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + read_AHRS(); + + // We are using the IMU + // --------------------- + Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_battery(uint8_t argc, const Menu::arg *argv) +{ +#if BATTERY_EVENT == 1 + for (int i = 0; i < 20; i++){ + delay(20); + read_battery(); + } + Serial.printf_P(PSTR("Volts: 1:")); + Serial.print(battery_voltage1, 4); + Serial.print(" 2:"); + Serial.print(battery_voltage2, 4); + Serial.print(" 3:"); + Serial.print(battery_voltage3, 4); + Serial.print(" 4:"); + Serial.println(battery_voltage4, 4); +#else + Serial.printf_P(PSTR("Not enabled\n")); + +#endif + return (0); +} + + +static int8_t +test_relay(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + //DDRL |= B00000100; + + while(1){ + Serial.println(int DDRL); + Serial.println(int PORTL); + Serial.println("Relay A"); + relay_A(); + delay(500); + if(Serial.available() > 0){ + return (0); + } + + Serial.println("Relay B"); + relay_B(); + delay(500); + if(Serial.available() > 0){ + return (0); + } + } +} + + +static int8_t +test_gyro(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Gyro | Accel\n")); + delay(200); + + while(1){ + for (int i = 0; i < 6; i++) { + AN[i] = APM_ADC.Ch(sensors[i]); + } + Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)AN[0], (int)AN[1], (int)AN[2], (int)AN[3], (int)AN[4], (int)AN[5]); + delay(100); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_wp(uint8_t argc, const Menu::arg *argv) +{ + delay(1000); + ground_alt = 0; + read_EEPROM_waypoint_info(); + + uint8_t options = eeprom_read_byte((const uint8_t *) EE_CONFIG); + int32_t hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME); + + // save the alitude above home option + if(options & HOLD_ALT_ABOVE_HOME){ + Serial.printf_P(PSTR("Hold altitude of %dm\n"), hold/100); + }else{ + Serial.printf_P(PSTR("Hold current altitude\n")); + } + + Serial.printf_P(PSTR("%d waypoints\n"), wp_total); + Serial.printf_P(PSTR("Hit radius: %d\n"), wp_radius); + Serial.printf_P(PSTR("Loiter radius: %d\n\n"), loiter_radius); + + for(byte i = 0; i <= wp_total; i++){ + struct Location temp = get_wp_with_index(i); + print_waypoint(&temp, i); + } + + return (0); +} + + +static int8_t +test_airspeed(uint8_t argc, const Menu::arg *argv) +{ +#if AIRSPEED_SENSOR == DISABLED + Serial.printf_P(PSTR("airspeed disabled\n")); + return (0); +#else + print_hit_enter(); + zero_airspeed(); + + while(1){ + delay(20); + read_airspeed(); + Serial.printf_P(PSTR("%dm/s\n"),airspeed/100); + + if(Serial.available() > 0){ + return (0); + } + } +#endif +} + +static int8_t +test_xbee(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); + while(1){ + delay(250); + // Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200 + //Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n")); + //Serial.print("X"); + // Default 32bit data from X-CTU Range Test + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_pressure(uint8_t argc, const Menu::arg *argv) +{ + uint32_t sum = 0; + Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n")); + Serial.printf_P(PSTR("Note - the altitude displayed is relative to the start of this test\n")); + print_hit_enter(); + Serial.printf_P(PSTR("\nCalibrating....\n")); + for (int i=1;i<301;i++) { + read_airpressure(); + if(i>200)sum += abs_press_filt; + delay(10); + } + abs_press_gnd = (double)sum/100.0; + + ground_alt = 0; + while(1){ + delay(100); + read_airpressure(); + //Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), press_alt / 100, abs_press_filt); // Someone needs to fix the formatting here for long integers +Serial.print("Altitude: "); +Serial.print(press_alt/100.0,2); +Serial.print(" Raw pressure value: "); +Serial.println(abs_press_filt); + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_mag(uint8_t argc, const Menu::arg *argv) +{ +#if MAGNETOMETER == DISABLED + Serial.printf_P(PSTR("Compass disabled\n")); + return (0); +#else + print_hit_enter(); + + while(1){ + delay(250); + APM_Compass.Read(); + APM_Compass.Calculate(0,0); + Serial.printf_P(PSTR("Heading: (")); + Serial.print(ToDeg(APM_Compass.Heading)); + Serial.printf_P(PSTR(") XYZ: (")); + Serial.print(APM_Compass.Mag_X); + Serial.print(comma); + Serial.print(APM_Compass.Mag_Y); + Serial.print(comma); + Serial.print(APM_Compass.Mag_Z); + Serial.println(")"); + if(Serial.available() > 0){ + return (0); + } + } +#endif +} + +static int8_t +test_jeti(uint8_t argc, const Menu::arg *argv) +{ + uint8_t switchPosition, m; + print_hit_enter(); + + JB.begin(); + //JB.print(" X-DIY"); + //JB.print(" Test mode"); + //JB.checkvalue(20000); + Serial.print("Jeti Box test: press any buttons\n\n"); + while(1){ + delay(200); + //Serial.println(JB.checkvalue(0)); + read_radio(); + switchPosition = readSwitch(); + m = flight_modes[switchPosition]; + JB.clear(1); + JB.print("Mode: "); + JB.print(flight_mode_strings[m]); + JB.clear(2); + switch (JB.readbuttons()) + { + case JB_key_right: + JB.print("rechts"); + Serial.print("rechts\n"); + break; + + case JB_key_left: + JB.print("links"); + Serial.print("links\n"); + break; + + case JB_key_up: + JB.print("hoch"); + Serial.print("hoch\n"); + break; + + case JB_key_down: + JB.print("runter"); + Serial.print("runter\n"); + break; + + case JB_key_left | JB_key_right: + JB.print("links-rechts"); + Serial.print("links-rechts\n"); + break; + } + + if(Serial.available() > 0){ + return (0); + } + } +} + +void print_hit_enter() +{ + Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp new file mode 100644 index 0000000000..2543002d76 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp @@ -0,0 +1,149 @@ +/* + APM_ADC.cpp - ADC ADS7844 Library for Ardupilot Mega + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Modified by John Ihlein 6/19/2010 to: + 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably + only an issue on initial read of ADC at program start. + 2)Reorder analog read order as follows: + p, q, r, ax, ay, az + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + External ADC ADS7844 is connected via Serial port 2 (in SPI mode) + TXD2 = MOSI = pin PH1 + RXD2 = MISO = pin PH0 + XCK2 = SCK = pin PH2 + Chip Select pin is PC4 (33) [PH6 (9)] + We are using the 16 clocks per conversion timming to increase efficiency (fast) + The sampling frequency is 400Hz (Timer2 overflow interrupt) + So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so + we have an 4x oversampling and averaging. + + Methods: + Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) + Ch(ch_num) : Return the ADC channel value + + // HJI - Input definitions. USB connector assumed to be on the left, Rx and servo + // connector pins to the rear. IMU shield components facing up. These are board + // referenced sensor inputs, not device referenced. + On Ardupilot Mega Hardware, oriented as described above: + Channel 0 : yaw rate, r + Channel 1 : roll rate, p + Channel 2 : pitch rate, q + Channel 3 : x/y gyro temperature + Channel 4 : x acceleration, aX + Channel 5 : y acceleration, aY + Channel 6 : z acceleration, aZ + Channel 7 : Differential pressure sensor port + +*/ +extern "C" { + // AVR LibC Includes + #include + #include + #include "WConstants.h" +} + +#include "APM_ADC.h" + + +// Commands for reading ADC channels on ADS7844 +const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; +volatile long adc_value[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; +volatile unsigned char adc_counter[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + +unsigned char ADC_SPI_transfer(unsigned char data) +{ + /* Wait for empty transmit buffer */ + while ( !( UCSR2A & (1<= 17) // HJI - Added this to prevent + { // overflow of adc_value + adc_value[ch] = 0; + adc_counter[ch] = 0; + } + adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte + adc_tmp |= ADC_SPI_transfer(adc_cmd[ch+1]); // Read second byte and send next command + adc_value[ch] += adc_tmp>>3; // Shift to 12 bits + adc_counter[ch]++; // Number of samples + } + bit_set(PORTC,4); // Disable Chip Select (PIN PC4) + //bit_clear(PORTL,6); // To test performance + TCNT2 = 104; // 400 Hz +} + + +// Constructors //////////////////////////////////////////////////////////////// +APM_ADC_Class::APM_ADC_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_ADC_Class::Init(void) +{ + unsigned char tmp; + + pinMode(ADC_CHIP_SELECT,OUTPUT); + + digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low) + + // Setup Serial Port2 in SPI mode + UBRR2 = 0; + DDRH |= (1<0) + result = adc_value[ch_num]/adc_counter[ch_num]; + else + result = 0; + adc_value[ch_num] = 0; // Initialize for next reading + adc_counter[ch_num] = 0; + sei(); + return(result); +} + +// make one instance for the user to use +APM_ADC_Class APM_ADC; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h new file mode 100644 index 0000000000..cea6102eb2 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h @@ -0,0 +1,24 @@ +#ifndef APM_ADC_h +#define APM_ADC_h + +#define bit_set(p,m) ((p) |= ( 1< // ArduPilot Mega ADC Library + +unsigned long timer; + +void setup() +{ + APM_ADC.Init(); // APM ADC initialization + Serial.begin(57600); + Serial.println("ArduPilot Mega ADC library test"); + delay(1000); + timer = millis(); +} + +void loop() +{ + int ch; + + if((millis()- timer) > 20) + { + timer = millis(); + for (ch=0;ch<7;ch++) + { + Serial.print(APM_ADC.Ch(ch)); + Serial.print(","); + } + Serial.println(); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt new file mode 100644 index 0000000000..c82cb5d786 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt @@ -0,0 +1,3 @@ +APM_ADC KEYWORD1 +Init KEYWORD2 +Ch KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp new file mode 100644 index 0000000000..72f84b7161 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp @@ -0,0 +1,243 @@ +/* + APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor is conected to I2C port + Sensor End of Conversion (EOC) pin is PC7 (30) + + Variables: + RawTemp : Raw temperature data + RawPress : Raw pressure data + + Temp : Calculated temperature (in 0.1ºC units) + Press : Calculated pressure (in Pa units) + + Methods: + Init() : Initialization of I2C and read sensor calibration data + Read() : Read sensor data and calculate Temperature and Pressure + This function is optimized so the main host don´t need to wait + You can call this function in your main loop + It returns a 1 if there are new data. + + Internal functions: + Command_ReadTemp(): Send commando to read temperature + Command_ReadPress(): Send commando to read Pressure + ReadTemp() : Read temp register + ReadPress() : Read press register + Calculate() : Calculate Temperature and Pressure in real units + + +*/ +extern "C" { + // AVR LibC Includes + #include + #include + #include "WConstants.h" +} + +#include +#include "APM_BMP085.h" + +#define BMP085_ADDRESS 0x77 //(0xEE >> 1) +#define BMP085_EOC 30 // End of conversion pin PC7 + +// Constructors //////////////////////////////////////////////////////////////// +APM_BMP085_Class::APM_BMP085_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_BMP085_Class::Init(void) +{ + unsigned char tmp; + byte buff[22]; + int i=0; + + pinMode(BMP085_EOC,INPUT); // End Of Conversion (PC7) input + Wire.begin(); + oss = 3; // Over Sampling setting 3 = High resolution + BMP085_State = 0; // Initial state + + // We read the calibration data registers + Wire.beginTransmission(BMP085_ADDRESS); + Wire.send(0xAA); + Wire.endTransmission(); + + Wire.requestFrom(BMP085_ADDRESS, 22); + //Wire.endTransmission(); + while(Wire.available()) + { + buff[i] = Wire.receive(); // receive one byte + i++; + } + ac1 = ((int)buff[0] << 8) | buff[1]; + ac2 = ((int)buff[2] << 8) | buff[3]; + ac3 = ((int)buff[4] << 8) | buff[5]; + ac4 = ((int)buff[6] << 8) | buff[7]; + ac5 = ((int)buff[8] << 8) | buff[9]; + ac6 = ((int)buff[10] << 8) | buff[11]; + b1 = ((int)buff[12] << 8) | buff[13]; + b2 = ((int)buff[14] << 8) | buff[15]; + mb = ((int)buff[16] << 8) | buff[17]; + mc = ((int)buff[18] << 8) | buff[19]; + md = ((int)buff[20] << 8) | buff[21]; + + //Send a command to read Temp + Command_ReadTemp(); + BMP085_State=1; +} + + +// Read the sensor. This is a state machine +// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) +uint8_t APM_BMP085_Class::Read() +{ +uint8_t result=0; + + if (BMP085_State==1) + { + if (digitalRead(BMP085_EOC)) + { + ReadTemp(); // On state 1 we read temp + BMP085_State++; + Command_ReadPress(); + } + } + else + { + if (BMP085_State==5) + { + if (digitalRead(BMP085_EOC)) + { + ReadPress(); + Calculate(); + BMP085_State = 1; // Start again from state=1 + Command_ReadTemp(); // Read Temp + result=1; // New pressure reading + } + } + else + { + if (digitalRead(BMP085_EOC)) + { + ReadPress(); + Calculate(); + BMP085_State++; + Command_ReadPress(); + result=1; // New pressure reading + } + } + } + return(result); +} + + +// Send command to Read Pressure +void APM_BMP085_Class::Command_ReadPress() +{ + Wire.beginTransmission(BMP085_ADDRESS); + Wire.send(0xF4); + Wire.send(0x34+(oss<<6)); //write_register(0xF4,0x34+(oversampling_setting<<6)); + Wire.endTransmission(); +} + +// Read Raw Pressure values +void APM_BMP085_Class::ReadPress() +{ + byte msb; + byte lsb; + byte xlsb; + + Wire.beginTransmission(BMP085_ADDRESS); + Wire.send(0xF6); + Wire.endTransmission(); + + Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte + while(!Wire.available()) { + // waiting + } + msb = Wire.receive(); + while(!Wire.available()) { + // waiting + } + lsb = Wire.receive(); + while(!Wire.available()) { + // waiting + } + xlsb = Wire.receive(); + RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss); +} + +// Send Command to Read Temperature +void APM_BMP085_Class::Command_ReadTemp() +{ + Wire.beginTransmission(BMP085_ADDRESS); + Wire.send(0xF4); + Wire.send(0x2E); + Wire.endTransmission(); +} + +// Read Raw Temperature values +void APM_BMP085_Class::ReadTemp() +{ + byte tmp; + + Wire.beginTransmission(BMP085_ADDRESS); + Wire.send(0xF6); + Wire.endTransmission(); + + Wire.beginTransmission(BMP085_ADDRESS); + Wire.requestFrom(BMP085_ADDRESS,2); + while(!Wire.available()); // wait + RawTemp = Wire.receive(); + while(!Wire.available()); // wait + tmp = Wire.receive(); + RawTemp = RawTemp<<8 | tmp; +} + +// Calculate Temperature and Pressure in real units. +void APM_BMP085_Class::Calculate() +{ + long x1, x2, x3, b3, b5, b6, p; + unsigned long b4, b7; + int32_t tmp; + + // See Datasheet page 13 for this formulas + // Based also on Jee Labs BMP085 example code. Thanks for share. + // Temperature calculations + x1 = ((long)RawTemp - ac6) * ac5 >> 15; + x2 = ((long) mc << 11) / (x1 + md); + b5 = x1 + x2; + Temp = (b5 + 8) >> 4; + + // Pressure calculations + b6 = b5 - 4000; + x1 = (b2 * (b6 * b6 >> 12)) >> 11; + x2 = ac2 * b6 >> 11; + x3 = x1 + x2; + //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD + //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0 + tmp = ac1; + tmp = (tmp*4 + x3)<> 13; + x2 = (b1 * (b6 * b6 >> 12)) >> 16; + x3 = ((x1 + x2) + 2) >> 2; + b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; + b7 = ((uint32_t) RawPress - b3) * (50000 >> oss); + p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; + + x1 = (p >> 8) * (p >> 8); + x1 = (x1 * 3038) >> 16; + x2 = (-7357 * p) >> 16; + Press = p + ((x1 + x2 + 3791) >> 4); +} + + +// make one instance for the user to use +APM_BMP085_Class APM_BMP085; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h new file mode 100644 index 0000000000..d5ad04932c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h @@ -0,0 +1,34 @@ +#ifndef APM_BMP085_h +#define APM_BMP085_h + + +class APM_BMP085_Class +{ + private: + // State machine + uint8_t BMP085_State; + // Internal calibration registers + int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; + uint16_t ac4, ac5, ac6; + void Command_ReadPress(); + void Command_ReadTemp(); + void ReadPress(); + void ReadTemp(); + void Calculate(); + public: + int32_t RawPress; + int32_t RawTemp; + int16_t Temp; + int32_t Press; + //int Altitude; + uint8_t oss; + //int32_t Press0; // Pressure at sea level + + APM_BMP085_Class(); // Constructor + void Init(); + uint8_t Read(); +}; + +extern APM_BMP085_Class APM_BMP085; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde new file mode 100644 index 0000000000..1ed59977b8 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde @@ -0,0 +1,41 @@ +/* + Example of APM_BMP085 (absolute pressure sensor) library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com +*/ + +#include +#include // ArduPilot Mega BMP085 Library + +unsigned long timer; + +void setup() +{ + APM_BMP085.Init(); // APM ADC initialization + Serial.begin(57600); + Serial.println("ArduPilot Mega BMP085 library test"); + delay(1000); + timer = millis(); +} + +void loop() +{ + int ch; + float tmp_float; + float Altitude; + + if((millis()- timer) > 50) + { + timer=millis(); + APM_BMP085.Read(); + Serial.print("Pressure:"); + Serial.print(APM_BMP085.Press); + Serial.print(" Temperature:"); + Serial.print(APM_BMP085.Temp/10.0); + Serial.print(" Altitude:"); + tmp_float = (APM_BMP085.Press/101325.0); + tmp_float = pow(tmp_float,0.190295); + Altitude = 44330*(1.0-tmp_float); + Serial.print(Altitude); + Serial.println(); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt new file mode 100644 index 0000000000..0d26768c39 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt @@ -0,0 +1,5 @@ +APM_BMP085 KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Press KEYWORD2 +Temp KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp new file mode 100644 index 0000000000..8d5107a5fc --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp @@ -0,0 +1,206 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- +// +// Copyright (c) 2010 Michael Smith. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +// SUCH DAMAGE. + +/// @file BinComm.cpp +/// @brief Implementation of the ArduPilot Mega binary communications +/// library. + +#include "APM_BinComm.h" +#include "WProgram.h" + +/// @name decoder state machine phases +//@{ +#define DEC_WAIT_P1 0 +#define DEC_WAIT_P2 1 +#define DEC_WAIT_HEADER 2 +#define DEC_WAIT_MESSAGE 3 +#define DEC_WAIT_SUM_A 4 +#define DEC_WAIT_SUM_B 5 +//@} + + +/// inter-byte timeout for decode (ms) +#define DEC_MESSAGE_TIMEOUT 100 + +BinComm::BinComm(const BinComm::MessageHandler *handlerTable, + Stream *interface) : + _handlerTable(handlerTable), + _interface(interface) +{ +}; + +void +BinComm::_sendMessage(void) +{ + uint8_t bytesToSend; + uint8_t sumA, sumB; + const uint8_t *p; + + // send the preamble first + _interface->write((uint8_t)MSG_PREAMBLE_1); + _interface->write((uint8_t)MSG_PREAMBLE_2); + + // set up to send the payload + bytesToSend = _encodeBuf.header.length + sizeof(_encodeBuf.header); + sumA = sumB = 0; + p = (const uint8_t *)&_encodeBuf; + + // send message bytes and compute checksum on the fly + while (bytesToSend--) { + sumA += *p; + sumB += sumA; + _interface->write(*p++); + } + + // send the checksum + _interface->write(sumA); + _interface->write(sumB); +} + +void +BinComm::update(void) +{ + uint8_t count; + + // Ensure that we don't spend too long here by only processing + // the bytes that were available when we started. + // + // XXX we might want to further constrain this count + count = _interface->available(); + while (count--) + _decode(_interface->read()); +} + +void +BinComm::_decode(uint8_t inByte) +{ + uint8_t tableIndex; + + // handle inter-byte timeouts (resync after link loss, etc.) + // + if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT) + _decodePhase = DEC_WAIT_P1; + + // run the decode state machine + // + switch (_decodePhase) { + + // Preamble detection + // + // Note the fallthrough from P2 to P1 deals with the case where + // we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or + // a SUM_B byte we never looked at. + // + case DEC_WAIT_P2: + if (MSG_PREAMBLE_2 == inByte) { + _decodePhase++; + + // prepare for the header + _bytesIn = 0; + _bytesExpected = sizeof(MessageHeader); + + // intialise the checksum accumulators + _sumA = _sumB = 0; + + break; + } + _decodePhase = DEC_WAIT_P1; + // FALLTHROUGH + case DEC_WAIT_P1: + if (MSG_PREAMBLE_1 == inByte) { + _decodePhase++; + } + break; + + // receiving the header + // + case DEC_WAIT_HEADER: + // do checksum accumulation + _sumA += inByte; + _sumB += _sumA; + + // store the byte + _decodeBuf.bytes[_bytesIn++] = inByte; + + // check for complete header received + if (_bytesIn == _bytesExpected) { + _decodePhase++; + + // prepare for the payload + // variable-length data? + _bytesIn = 0; + _bytesExpected = _decodeBuf.header.length; + _messageID = _decodeBuf.header.messageID; + _messageVersion = _decodeBuf.header.messageVersion; + + // sanity check to avoid buffer overflow - revert back to waiting + if (_bytesExpected > sizeof(_decodeBuf)) + _decodePhase = DEC_WAIT_P1; + } + break; + + // receiving payload data + // + case DEC_WAIT_MESSAGE: + // do checksum accumulation + _sumA += inByte; + _sumB += _sumA; + + // store the byte + _decodeBuf.bytes[_bytesIn++] = inByte; + + // check for complete payload received + if (_bytesIn == _bytesExpected) { + _decodePhase++; + } + break; + + // waiting for the checksum bytes + // + case DEC_WAIT_SUM_A: + if (inByte != _sumA) { + badMessagesReceived++; + _decodePhase = DEC_WAIT_P1; + } else { + _decodePhase++; + } + break; + case DEC_WAIT_SUM_B: + if (inByte == _sumB) { + // if we got this far, we have a message + messagesReceived++; + + // call any handler interested in this message + for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++) + if ((_handlerTable[tableIndex].messageID == _messageID) || + (_handlerTable[tableIndex].messageID == MSG_ANY)) + _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_decodeBuf); + } else { + badMessagesReceived++; + } + _decodePhase = DEC_WAIT_P1; + break; + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h new file mode 100644 index 0000000000..22708f2dd7 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h @@ -0,0 +1,267 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Copyright (c) 2010 Michael Smith. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +// SUCH DAMAGE. + +/// @file BinComm.h +/// @brief Definitions for the ArduPilot Mega binary communications +/// library. + +#ifndef APM_BinComm_h +#define APM_BinComm_h + +#include +#include +#include "WProgram.h" + +/// +/// @class BinComm +/// @brief Class providing protocol en/decoding services for the ArduPilot +/// Mega binary telemetry protocol. +/// +class BinComm { +public: + struct MessageHandler; + + ////////////////////////////////////////////////////////////////////// + /// Constructor. + /// + /// @param handlerTable Array of callout functions to which + /// received messages will be sent. More than + /// one handler for a given messageID may be + /// registered; handlers are called in the order + /// they appear in the table. + /// + /// @param interface The stream that will be used + /// for telemetry communications. + /// + BinComm(const MessageHandler *handlerTable, + Stream *interface); + +private: + /// OTA message header + struct MessageHeader { + uint8_t length; + uint8_t messageID; + uint8_t messageVersion; + }; + + /// Incoming header/packet buffer + /// XXX we could make this smaller + union { + uint8_t bytes[0]; + MessageHeader header; + uint8_t payload[256]; + } _decodeBuf; + + /// Outgoing header/packet buffer + /// XXX we could make this smaller + struct { + MessageHeader header; + uint8_t payload[256 - sizeof(MessageHeader)]; + } _encodeBuf; + + + ////////////////////////////////////////////////////////////////////// + /// @name Message pack/unpack utility functions + /// + //@{ + inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); } + inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); } + + inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; } + inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; } + inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; } + + inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); } + inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); } + + inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; } + inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; } + inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; } + //@} + +public: + ////////////////////////////////////////////////////////////////////// + /// @name Protocol definition + /// + /// The protocol definition, including structures describing messages, + /// MessageID values and helper functions for packing messages are + /// automatically generated. + //@{ +#include "protocol/protocol.h" + //@} + + ////////////////////////////////////////////////////////////////////// + /// @name Protocol magic numbers + /// + /// @note The MessageID enum is automatically generated and thus not described here. + /// + //@{ + + /// Variables defined + /// XXX these should probably be handled by the database/MIB? + enum variableID { + MSG_VAR_ROLL_MODE = 0x00, + MSG_VAR_PITCH_MODE = 0x01, + MSG_VAR_THROTTLE_MODE = 0x02, + MSG_VAR_YAW_MODE = 0x03, + MSG_VAR_ELEVON_TRIM_1 = 0x04, + MSG_VAR_ELEVON_TRIM_2 = 0x05, + + MSG_VAR_INTEGRATOR_0 = 0x10, + MSG_VAR_INTEGRATOR_1 = 0x11, + MSG_VAR_INTEGRATOR_2 = 0x12, + MSG_VAR_INTEGRATOR_3 = 0x13, + MSG_VAR_INTEGRATOR_4 = 0x14, + MSG_VAR_INTEGRATOR_5 = 0x15, + MSG_VAR_INTEGRATOR_6 = 0x16, + MSG_VAR_INTEGRATOR_7 = 0x17, + + MSG_VAR_KFF_0 = 0x1a, + MSG_VAR_KFF_1 = 0x1b, + MSG_VAR_KFF_2 = 0x1c, + + MSG_VAR_TARGET_BEARING = 0x20, + MSG_VAR_NAV_BEARING = 0x21, + MSG_VAR_BEARING_ERROR = 0x22, + MSG_VAR_CROSSTRACK_BEARING = 0x23, + MSG_VAR_CROSSTRACK_ERROR = 0x24, + MSG_VAR_ALTITUDE_ERROR = 0x25, + MSG_VAR_WP_RADIUS = 0x26, + MSG_VAR_LOITER_RADIUS = 0x27, + MSG_VAR_WP_MODE = 0x28, + MSG_VAR_LOOP_COMMANDS = 0x29, + MSG_VAR_NAV_GAIN_SCALER = 0x2a, + }; + + /// PID sets defined + enum PIDSet { + MSG_SERVO_ROLL = 0, + MSG_SERVO_PITCH = 1, + MSG_SERVO_RUDDER = 2, + MSG_SERVO_NAV_ROLL = 3, + MSG_SERVO_NAV_PITCH_ASP = 4, + MSG_SERVO_NAV_PITCH_ALT = 5, + MSG_SERVO_TE_THROTTLE = 6, + MSG_SERVO_ALT_THROTTLE = 7, + MSG_SERVO_ELEVATOR = 8 // Added by Randy + }; + + //@} + + ////////////////////////////////////////////////////////////////////// + /// Message reception callout descriptor + /// + /// An array of these handlers is passed to the constructor to delegate + /// processing for received messages. + /// + struct MessageHandler { + MessageID messageID; ///< messageID for which the handler will be called + void (* handler)(void *arg, + uint8_t messageId, + uint8_t messageVersion, + void *messageData); ///< function to be called + void *arg; ///< argument passed to function + }; + + ////////////////////////////////////////////////////////////////////// + /// @name Decoder interface + //@{ + + /// Consume bytes from the interface and feed them to the decoder. + /// + /// If a packet is completed, then any callbacks associated + /// with the packet's messageID will be called. + /// + /// If no bytes are passed to the decoder for a period determined + /// by DEC_MESSAGE_TIMEOUT, the decode state machine will reset + /// before processing the next byte. This can help re-synchronise + /// after a link loss or in-flight failure. + /// + + void update(void); + + uint32_t messagesReceived; ///< statistics + uint32_t badMessagesReceived; ///< statistics + + //@} + + ////////////////////////////////////////////////////////////////////// + /// @name Encoder interface + /// + /// Messages are normally encoded and sent using the + /// send_msg_* functions defined in protocol/protocol.h. + /// For each message type MSG_* there is a corresponding send_msg_* + /// function which will construct and transmit the message. + /// + //@{ + uint32_t messagesSent; ///< statistics + //@} + + +private: + const MessageHandler *_handlerTable; ///< callout table + Stream *_interface; ///< Serial port we send/receive using. + + /// Various magic numbers + enum MagicNumbers { + MSG_PREAMBLE_1 = 0x34, + MSG_PREAMBLE_2 = 0x44, + MSG_VERSION_1 = 1, + MSG_VARIABLE_LENGTH = 0xff + }; + + ////////////////////////////////////////////////////////////////////// + /// @name Decoder state + //@{ + uint8_t _decodePhase; ///< decoder state machine phase + uint8_t _bytesIn; ///< bytes received in the current phase + uint8_t _bytesExpected; ///< bytes expected in the current phase + uint8_t _sumA; ///< sum of incoming bytes + uint8_t _sumB; ///< sum of _sumA values + + uint8_t _messageID; ///< messageID from the packet being received + uint8_t _messageVersion;///< messageVersion from the packet being received + + unsigned long _lastReceived; ///< timestamp of last byte reception + //@} + + /// Decoder state machine. + /// + /// @param inByte The byte to process. + /// + void _decode(uint8_t inByte); + + /// Send the packet in the encode buffer. + /// + void _sendMessage(void); +}; + +#endif // BinComm_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt new file mode 100644 index 0000000000..3d75c5372d --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt @@ -0,0 +1,6 @@ +APM_BinComm KEYWORD1 +update KEYWORD2 +messagesReceived KEYWORD2 +badMessagesReceived KEYWORD2 +messagesSent KEYWORD2 + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def new file mode 100644 index 0000000000..d1e465cc6f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def @@ -0,0 +1,224 @@ +# +# Message definitions for the ArduPilot Mega binary communications protocol. +# +# Process this file using: +# +# awk -f protogen.awk protocol.def > protocol.h +# +# Messages are declared with +# +# message +# +# is a valid member of BinComm::MessageID. +# is the message ID byte +# +# Following a message declaration the fields of the message are +# defined in the format: +# +# [] +# +# is a C type corresponding to the field. The type must be a single +# word, either an integer from or "char". +# is the name of the field; it should be unique within the message +# is an optional array count for fields that are arrays +# + +# +# Acknowledge message +# +message 0x00 MSG_ACKNOWLEDGE + uint8_t msgID + uint16_t msgSum + +# +# System heartbeat +# +message 0x01 MSG_HEARTBEAT + uint8_t flightMode + uint16_t timeStamp + uint16_t batteryVoltage + uint16_t commandIndex + +# +# Attitude report +# +message 0x02 MSG_ATTITUDE + int16_t roll + int16_t pitch + int16_t yaw + +# +# Location report +# +message 0x03 MSG_LOCATION + int32_t latitude + int32_t longitude + int16_t altitude + int16_t groundSpeed + int16_t groundCourse + uint16_t timeOfWeek + +# +# Optional pressure-based location report +# +message 0x04 MSG_PRESSURE + uint16_t pressureAltitude + uint16_t airSpeed + +# +# Text status message +# +message 0x05 MSG_STATUS_TEXT + uint8_t severity + char text 50 + +# +# Algorithm performance report +# +message 0x06 MSG_PERF_REPORT + uint32_t interval + uint16_t mainLoopCycles + uint8_t mainLoopTime + uint8_t gyroSaturationCount + uint8_t adcConstraintCount + uint16_t imuHealth + uint16_t gcsMessageCount + +# +# System version messages +# +message 0x07 MSG_VERSION_REQUEST + uint8_t systemType + uint8_t systemID + +message 0x08 MSG_VERSION + uint8_t systemType + uint8_t systemID + uint8_t firmwareVersion 3 + +# +# Flight command operations +# +message 0x20 MSG_COMMAND_REQUEST + uint16_t UNSPECIFIED + +message 0x21 MSG_COMMAND_UPLOAD + uint8_t action + uint16_t itemNumber + int listLength + uint8_t commandID + uint8_t p1 + uint16_t p2 + uint32_t p3 + uint32_t p4 + +message 0x22 MSG_COMMAND_LIST + int itemNumber + int listLength + uint8_t commandID + uint8_t p1 + uint16_t p2 + uint32_t p3 + uint32_t p4 + +message 0x23 MSG_COMMAND_MODE_CHANGE + uint16_t UNSPECIFIED + +# +# Parameter operations +# +message 0x30 MSG_VALUE_REQUEST + uint8_t valueID + uint8_t broadcast + + +message 0x31 MSG_VALUE_SET + uint8_t valueID + uint32_t value + +message 0x32 MSG_VALUE + uint8_t valueID + uint32_t value + +# +# PID adjustments +# +message 0x40 MSG_PID_REQUEST + uint8_t pidSet + +message 0x41 MSG_PID_SET + uint8_t pidSet + int32_t p + int32_t i + int32_t d + int16_t integratorMax + +message 0x42 MSG_PID + uint8_t pidSet + int32_t p + int32_t i + int32_t d + int16_t integratorMax + + +# +# Radio settings and values +# +message 0x50 MSG_TRIM_STARTUP + uint16_t value 8 + +message 0x51 MSG_TRIM_MIN + uint16_t value 8 + +message 0x52 MSG_TRIM_MAX + uint16_t value 8 + +message 0x53 MSG_SERVOS + int16_t ch1 + int16_t ch2 + int16_t ch3 + int16_t ch4 + int16_t ch5 + int16_t ch6 + int16_t ch7 + int16_t ch8 + +# +# Direct sensor access +# +message 0x60 MSG_SENSOR + uint16_t UNSPECIFIED + +# +# Simulation-related messages +# +message 0x70 MSG_SIM + uint16_t UNSPECIFIED + +# +# Direct I/O pin control +# +message 0x80 MSG_PIN_REQUEST + uint16_t UNSPECIFIED + +message 0x81 MSG_PIN_SET + uint16_t UNSPECIFIED + +# +# Dataflash operations +# +message 0x90 MSG_DATAFLASH_REQUEST + uint16_t UNSPECIFIED + +message 0x91 MSG_DATAFLASH_SET + uint16_t UNSPECIFIED + +# +# EEPROM operations +# +message 0xa0 MSG_EEPROM_REQUEST + uint16_t UNSPECIFIED + +message 0xa1 MSG_EEPROM_SET + uint16_t UNSPECIFIED + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h new file mode 100644 index 0000000000..2da8e0f7c3 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h @@ -0,0 +1,1301 @@ +// +// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT +// +/// @file protocol.h +#pragma pack(1) + +////////////////////////////////////////////////////////////////////// +/// @name MSG_ACKNOWLEDGE +//@{ + +/// Structure describing the payload section of the MSG_ACKNOWLEDGE message +struct msg_acknowledge { + uint8_t msgID; + uint16_t msgSum; +}; + +/// Send a MSG_ACKNOWLEDGE message +inline void +send_msg_acknowledge( + const uint8_t msgID, + const uint16_t msgSum) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, msgID); + _pack(__p, msgSum); + _encodeBuf.header.length = 3; + _encodeBuf.header.messageID = MSG_ACKNOWLEDGE; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_ACKNOWLEDGE message +inline void +unpack_msg_acknowledge( + uint8_t &msgID, + uint16_t &msgSum) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, msgID); + _unpack(__p, msgSum); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_HEARTBEAT +//@{ + +/// Structure describing the payload section of the MSG_HEARTBEAT message +struct msg_heartbeat { + uint8_t flightMode; + uint16_t timeStamp; + uint16_t batteryVoltage; + uint16_t commandIndex; +}; + +/// Send a MSG_HEARTBEAT message +inline void +send_msg_heartbeat( + const uint8_t flightMode, + const uint16_t timeStamp, + const uint16_t batteryVoltage, + const uint16_t commandIndex) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, flightMode); + _pack(__p, timeStamp); + _pack(__p, batteryVoltage); + _pack(__p, commandIndex); + _encodeBuf.header.length = 7; + _encodeBuf.header.messageID = MSG_HEARTBEAT; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_HEARTBEAT message +inline void +unpack_msg_heartbeat( + uint8_t &flightMode, + uint16_t &timeStamp, + uint16_t &batteryVoltage, + uint16_t &commandIndex) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, flightMode); + _unpack(__p, timeStamp); + _unpack(__p, batteryVoltage); + _unpack(__p, commandIndex); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_ATTITUDE +//@{ + +/// Structure describing the payload section of the MSG_ATTITUDE message +struct msg_attitude { + int16_t roll; + int16_t pitch; + int16_t yaw; +}; + +/// Send a MSG_ATTITUDE message +inline void +send_msg_attitude( + const int16_t roll, + const int16_t pitch, + const int16_t yaw) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, roll); + _pack(__p, pitch); + _pack(__p, yaw); + _encodeBuf.header.length = 6; + _encodeBuf.header.messageID = MSG_ATTITUDE; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_ATTITUDE message +inline void +unpack_msg_attitude( + int16_t &roll, + int16_t &pitch, + int16_t &yaw) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, roll); + _unpack(__p, pitch); + _unpack(__p, yaw); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_LOCATION +//@{ + +/// Structure describing the payload section of the MSG_LOCATION message +struct msg_location { + int32_t latitude; + int32_t longitude; + int16_t altitude; + int16_t groundSpeed; + int16_t groundCourse; + uint16_t timeOfWeek; +}; + +/// Send a MSG_LOCATION message +inline void +send_msg_location( + const int32_t latitude, + const int32_t longitude, + const int16_t altitude, + const int16_t groundSpeed, + const int16_t groundCourse, + const uint16_t timeOfWeek) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, latitude); + _pack(__p, longitude); + _pack(__p, altitude); + _pack(__p, groundSpeed); + _pack(__p, groundCourse); + _pack(__p, timeOfWeek); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_LOCATION; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_LOCATION message +inline void +unpack_msg_location( + int32_t &latitude, + int32_t &longitude, + int16_t &altitude, + int16_t &groundSpeed, + int16_t &groundCourse, + uint16_t &timeOfWeek) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, latitude); + _unpack(__p, longitude); + _unpack(__p, altitude); + _unpack(__p, groundSpeed); + _unpack(__p, groundCourse); + _unpack(__p, timeOfWeek); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PRESSURE +//@{ + +/// Structure describing the payload section of the MSG_PRESSURE message +struct msg_pressure { + uint16_t pressureAltitude; + uint16_t airSpeed; +}; + +/// Send a MSG_PRESSURE message +inline void +send_msg_pressure( + const uint16_t pressureAltitude, + const uint16_t airSpeed) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, pressureAltitude); + _pack(__p, airSpeed); + _encodeBuf.header.length = 4; + _encodeBuf.header.messageID = MSG_PRESSURE; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PRESSURE message +inline void +unpack_msg_pressure( + uint16_t &pressureAltitude, + uint16_t &airSpeed) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, pressureAltitude); + _unpack(__p, airSpeed); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_STATUS_TEXT +//@{ + +/// Structure describing the payload section of the MSG_STATUS_TEXT message +struct msg_status_text { + uint8_t severity; + char text[50]; +}; + +/// Send a MSG_STATUS_TEXT message +inline void +send_msg_status_text( + const uint8_t severity, + const char (&text)[50]) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, severity); + _pack(__p, text, 50); + _encodeBuf.header.length = 51; + _encodeBuf.header.messageID = MSG_STATUS_TEXT; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_STATUS_TEXT message +inline void +unpack_msg_status_text( + uint8_t &severity, + char (&text)[50]) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, severity); + _unpack(__p, text, 50); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PERF_REPORT +//@{ + +/// Structure describing the payload section of the MSG_PERF_REPORT message +struct msg_perf_report { + uint32_t interval; + uint16_t mainLoopCycles; + uint8_t mainLoopTime; + uint8_t gyroSaturationCount; + uint8_t adcConstraintCount; + uint16_t imuHealth; + uint16_t gcsMessageCount; +}; + +/// Send a MSG_PERF_REPORT message +inline void +send_msg_perf_report( + const uint32_t interval, + const uint16_t mainLoopCycles, + const uint8_t mainLoopTime, + const uint8_t gyroSaturationCount, + const uint8_t adcConstraintCount, + const uint16_t imuHealth, + const uint16_t gcsMessageCount) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, interval); + _pack(__p, mainLoopCycles); + _pack(__p, mainLoopTime); + _pack(__p, gyroSaturationCount); + _pack(__p, adcConstraintCount); + _pack(__p, imuHealth); + _pack(__p, gcsMessageCount); + _encodeBuf.header.length = 13; + _encodeBuf.header.messageID = MSG_PERF_REPORT; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PERF_REPORT message +inline void +unpack_msg_perf_report( + uint32_t &interval, + uint16_t &mainLoopCycles, + uint8_t &mainLoopTime, + uint8_t &gyroSaturationCount, + uint8_t &adcConstraintCount, + uint16_t &imuHealth, + uint16_t &gcsMessageCount) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, interval); + _unpack(__p, mainLoopCycles); + _unpack(__p, mainLoopTime); + _unpack(__p, gyroSaturationCount); + _unpack(__p, adcConstraintCount); + _unpack(__p, imuHealth); + _unpack(__p, gcsMessageCount); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_VERSION_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_VERSION_REQUEST message +struct msg_version_request { + uint8_t systemType; + uint8_t systemID; +}; + +/// Send a MSG_VERSION_REQUEST message +inline void +send_msg_version_request( + const uint8_t systemType, + const uint8_t systemID) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, systemType); + _pack(__p, systemID); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_VERSION_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_VERSION_REQUEST message +inline void +unpack_msg_version_request( + uint8_t &systemType, + uint8_t &systemID) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, systemType); + _unpack(__p, systemID); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_VERSION +//@{ + +/// Structure describing the payload section of the MSG_VERSION message +struct msg_version { + uint8_t systemType; + uint8_t systemID; + uint8_t firmwareVersion[3]; +}; + +/// Send a MSG_VERSION message +inline void +send_msg_version( + const uint8_t systemType, + const uint8_t systemID, + const uint8_t (&firmwareVersion)[3]) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, systemType); + _pack(__p, systemID); + _pack(__p, firmwareVersion, 3); + _encodeBuf.header.length = 5; + _encodeBuf.header.messageID = MSG_VERSION; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_VERSION message +inline void +unpack_msg_version( + uint8_t &systemType, + uint8_t &systemID, + uint8_t (&firmwareVersion)[3]) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, systemType); + _unpack(__p, systemID); + _unpack(__p, firmwareVersion, 3); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_COMMAND_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_COMMAND_REQUEST message +struct msg_command_request { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_COMMAND_REQUEST message +inline void +send_msg_command_request( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_COMMAND_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_COMMAND_REQUEST message +inline void +unpack_msg_command_request( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_COMMAND_UPLOAD +//@{ + +/// Structure describing the payload section of the MSG_COMMAND_UPLOAD message +struct msg_command_upload { + uint8_t action; + uint16_t itemNumber; + int listLength; + uint8_t commandID; + uint8_t p1; + uint16_t p2; + uint32_t p3; + uint32_t p4; +}; + +/// Send a MSG_COMMAND_UPLOAD message +inline void +send_msg_command_upload( + const uint8_t action, + const uint16_t itemNumber, + const int listLength, + const uint8_t commandID, + const uint8_t p1, + const uint16_t p2, + const uint32_t p3, + const uint32_t p4) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, action); + _pack(__p, itemNumber); + _pack(__p, listLength); + _pack(__p, commandID); + _pack(__p, p1); + _pack(__p, p2); + _pack(__p, p3); + _pack(__p, p4); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_COMMAND_UPLOAD; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_COMMAND_UPLOAD message +inline void +unpack_msg_command_upload( + uint8_t &action, + uint16_t &itemNumber, + int &listLength, + uint8_t &commandID, + uint8_t &p1, + uint16_t &p2, + uint32_t &p3, + uint32_t &p4) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, action); + _unpack(__p, itemNumber); + _unpack(__p, listLength); + _unpack(__p, commandID); + _unpack(__p, p1); + _unpack(__p, p2); + _unpack(__p, p3); + _unpack(__p, p4); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_COMMAND_LIST +//@{ + +/// Structure describing the payload section of the MSG_COMMAND_LIST message +struct msg_command_list { + int itemNumber; + int listLength; + uint8_t commandID; + uint8_t p1; + uint16_t p2; + uint32_t p3; + uint32_t p4; +}; + +/// Send a MSG_COMMAND_LIST message +inline void +send_msg_command_list( + const int itemNumber, + const int listLength, + const uint8_t commandID, + const uint8_t p1, + const uint16_t p2, + const uint32_t p3, + const uint32_t p4) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, itemNumber); + _pack(__p, listLength); + _pack(__p, commandID); + _pack(__p, p1); + _pack(__p, p2); + _pack(__p, p3); + _pack(__p, p4); + _encodeBuf.header.length = 14; + _encodeBuf.header.messageID = MSG_COMMAND_LIST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_COMMAND_LIST message +inline void +unpack_msg_command_list( + int &itemNumber, + int &listLength, + uint8_t &commandID, + uint8_t &p1, + uint16_t &p2, + uint32_t &p3, + uint32_t &p4) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, itemNumber); + _unpack(__p, listLength); + _unpack(__p, commandID); + _unpack(__p, p1); + _unpack(__p, p2); + _unpack(__p, p3); + _unpack(__p, p4); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_COMMAND_MODE_CHANGE +//@{ + +/// Structure describing the payload section of the MSG_COMMAND_MODE_CHANGE message +struct msg_command_mode_change { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_COMMAND_MODE_CHANGE message +inline void +send_msg_command_mode_change( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_COMMAND_MODE_CHANGE; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_COMMAND_MODE_CHANGE message +inline void +unpack_msg_command_mode_change( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_VALUE_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_VALUE_REQUEST message +struct msg_value_request { + uint8_t valueID; + uint8_t broadcast; +}; + +/// Send a MSG_VALUE_REQUEST message +inline void +send_msg_value_request( + const uint8_t valueID, + const uint8_t broadcast) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, valueID); + _pack(__p, broadcast); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_VALUE_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_VALUE_REQUEST message +inline void +unpack_msg_value_request( + uint8_t &valueID, + uint8_t &broadcast) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, valueID); + _unpack(__p, broadcast); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_VALUE_SET +//@{ + +/// Structure describing the payload section of the MSG_VALUE_SET message +struct msg_value_set { + uint8_t valueID; + uint32_t value; +}; + +/// Send a MSG_VALUE_SET message +inline void +send_msg_value_set( + const uint8_t valueID, + const uint32_t value) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, valueID); + _pack(__p, value); + _encodeBuf.header.length = 5; + _encodeBuf.header.messageID = MSG_VALUE_SET; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_VALUE_SET message +inline void +unpack_msg_value_set( + uint8_t &valueID, + uint32_t &value) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, valueID); + _unpack(__p, value); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_VALUE +//@{ + +/// Structure describing the payload section of the MSG_VALUE message +struct msg_value { + uint8_t valueID; + uint32_t value; +}; + +/// Send a MSG_VALUE message +inline void +send_msg_value( + const uint8_t valueID, + const uint32_t value) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, valueID); + _pack(__p, value); + _encodeBuf.header.length = 5; + _encodeBuf.header.messageID = MSG_VALUE; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_VALUE message +inline void +unpack_msg_value( + uint8_t &valueID, + uint32_t &value) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, valueID); + _unpack(__p, value); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PID_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_PID_REQUEST message +struct msg_pid_request { + uint8_t pidSet; +}; + +/// Send a MSG_PID_REQUEST message +inline void +send_msg_pid_request( + const uint8_t pidSet) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, pidSet); + _encodeBuf.header.length = 1; + _encodeBuf.header.messageID = MSG_PID_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PID_REQUEST message +inline void +unpack_msg_pid_request( + uint8_t &pidSet) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, pidSet); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PID_SET +//@{ + +/// Structure describing the payload section of the MSG_PID_SET message +struct msg_pid_set { + uint8_t pidSet; + int32_t p; + int32_t i; + int32_t d; + int16_t integratorMax; +}; + +/// Send a MSG_PID_SET message +inline void +send_msg_pid_set( + const uint8_t pidSet, + const int32_t p, + const int32_t i, + const int32_t d, + const int16_t integratorMax) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, pidSet); + _pack(__p, p); + _pack(__p, i); + _pack(__p, d); + _pack(__p, integratorMax); + _encodeBuf.header.length = 15; + _encodeBuf.header.messageID = MSG_PID_SET; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PID_SET message +inline void +unpack_msg_pid_set( + uint8_t &pidSet, + int32_t &p, + int32_t &i, + int32_t &d, + int16_t &integratorMax) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, pidSet); + _unpack(__p, p); + _unpack(__p, i); + _unpack(__p, d); + _unpack(__p, integratorMax); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PID +//@{ + +/// Structure describing the payload section of the MSG_PID message +struct msg_pid { + uint8_t pidSet; + int32_t p; + int32_t i; + int32_t d; + int16_t integratorMax; +}; + +/// Send a MSG_PID message +inline void +send_msg_pid( + const uint8_t pidSet, + const int32_t p, + const int32_t i, + const int32_t d, + const int16_t integratorMax) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, pidSet); + _pack(__p, p); + _pack(__p, i); + _pack(__p, d); + _pack(__p, integratorMax); + _encodeBuf.header.length = 15; + _encodeBuf.header.messageID = MSG_PID; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PID message +inline void +unpack_msg_pid( + uint8_t &pidSet, + int32_t &p, + int32_t &i, + int32_t &d, + int16_t &integratorMax) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, pidSet); + _unpack(__p, p); + _unpack(__p, i); + _unpack(__p, d); + _unpack(__p, integratorMax); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_TRIM_STARTUP +//@{ + +/// Structure describing the payload section of the MSG_TRIM_STARTUP message +struct msg_trim_startup { + uint16_t value[8]; +}; + +/// Send a MSG_TRIM_STARTUP message +inline void +send_msg_trim_startup( + const uint16_t (&value)[8]) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, value, 8); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_TRIM_STARTUP; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_TRIM_STARTUP message +inline void +unpack_msg_trim_startup( + uint16_t (&value)[8]) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, value, 8); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_TRIM_MIN +//@{ + +/// Structure describing the payload section of the MSG_TRIM_MIN message +struct msg_trim_min { + uint16_t value[8]; +}; + +/// Send a MSG_TRIM_MIN message +inline void +send_msg_trim_min( + const uint16_t (&value)[8]) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, value, 8); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_TRIM_MIN; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_TRIM_MIN message +inline void +unpack_msg_trim_min( + uint16_t (&value)[8]) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, value, 8); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_TRIM_MAX +//@{ + +/// Structure describing the payload section of the MSG_TRIM_MAX message +struct msg_trim_max { + uint16_t value[8]; +}; + +/// Send a MSG_TRIM_MAX message +inline void +send_msg_trim_max( + const uint16_t (&value)[8]) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, value, 8); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_TRIM_MAX; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_TRIM_MAX message +inline void +unpack_msg_trim_max( + uint16_t (&value)[8]) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, value, 8); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_SERVOS +//@{ + +/// Structure describing the payload section of the MSG_SERVOS message +struct msg_servos { + int16_t ch1; + int16_t ch2; + int16_t ch3; + int16_t ch4; + int16_t ch5; + int16_t ch6; + int16_t ch7; + int16_t ch8; +}; + +/// Send a MSG_SERVOS message +inline void +send_msg_servos( + const int16_t ch1, + const int16_t ch2, + const int16_t ch3, + const int16_t ch4, + const int16_t ch5, + const int16_t ch6, + const int16_t ch7, + const int16_t ch8) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, ch1); + _pack(__p, ch2); + _pack(__p, ch3); + _pack(__p, ch4); + _pack(__p, ch5); + _pack(__p, ch6); + _pack(__p, ch7); + _pack(__p, ch8); + _encodeBuf.header.length = 16; + _encodeBuf.header.messageID = MSG_SERVOS; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_SERVOS message +inline void +unpack_msg_servos( + int16_t &ch1, + int16_t &ch2, + int16_t &ch3, + int16_t &ch4, + int16_t &ch5, + int16_t &ch6, + int16_t &ch7, + int16_t &ch8) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, ch1); + _unpack(__p, ch2); + _unpack(__p, ch3); + _unpack(__p, ch4); + _unpack(__p, ch5); + _unpack(__p, ch6); + _unpack(__p, ch7); + _unpack(__p, ch8); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_SENSOR +//@{ + +/// Structure describing the payload section of the MSG_SENSOR message +struct msg_sensor { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_SENSOR message +inline void +send_msg_sensor( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_SENSOR; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_SENSOR message +inline void +unpack_msg_sensor( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_SIM +//@{ + +/// Structure describing the payload section of the MSG_SIM message +struct msg_sim { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_SIM message +inline void +send_msg_sim( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_SIM; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_SIM message +inline void +unpack_msg_sim( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PIN_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_PIN_REQUEST message +struct msg_pin_request { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_PIN_REQUEST message +inline void +send_msg_pin_request( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_PIN_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PIN_REQUEST message +inline void +unpack_msg_pin_request( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_PIN_SET +//@{ + +/// Structure describing the payload section of the MSG_PIN_SET message +struct msg_pin_set { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_PIN_SET message +inline void +send_msg_pin_set( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_PIN_SET; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_PIN_SET message +inline void +unpack_msg_pin_set( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_DATAFLASH_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_DATAFLASH_REQUEST message +struct msg_dataflash_request { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_DATAFLASH_REQUEST message +inline void +send_msg_dataflash_request( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_DATAFLASH_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_DATAFLASH_REQUEST message +inline void +unpack_msg_dataflash_request( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_DATAFLASH_SET +//@{ + +/// Structure describing the payload section of the MSG_DATAFLASH_SET message +struct msg_dataflash_set { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_DATAFLASH_SET message +inline void +send_msg_dataflash_set( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_DATAFLASH_SET; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_DATAFLASH_SET message +inline void +unpack_msg_dataflash_set( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_EEPROM_REQUEST +//@{ + +/// Structure describing the payload section of the MSG_EEPROM_REQUEST message +struct msg_eeprom_request { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_EEPROM_REQUEST message +inline void +send_msg_eeprom_request( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_EEPROM_REQUEST; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_EEPROM_REQUEST message +inline void +unpack_msg_eeprom_request( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// @name MSG_EEPROM_SET +//@{ + +/// Structure describing the payload section of the MSG_EEPROM_SET message +struct msg_eeprom_set { + uint16_t UNSPECIFIED; +}; + +/// Send a MSG_EEPROM_SET message +inline void +send_msg_eeprom_set( + const uint16_t UNSPECIFIED) +{ + uint8_t *__p = &_encodeBuf.payload[0]; + _pack(__p, UNSPECIFIED); + _encodeBuf.header.length = 2; + _encodeBuf.header.messageID = MSG_EEPROM_SET; + _encodeBuf.header.messageVersion = MSG_VERSION_1; + _sendMessage(); +}; + +/// Unpack a MSG_EEPROM_SET message +inline void +unpack_msg_eeprom_set( + uint16_t &UNSPECIFIED) +{ + uint8_t *__p = &_decodeBuf.payload[0]; + _unpack(__p, UNSPECIFIED); +}; +//@} + +////////////////////////////////////////////////////////////////////// +/// Message ID values +enum MessageID { + MSG_PID = 0x42, + MSG_DATAFLASH_REQUEST = 0x90, + MSG_DATAFLASH_SET = 0x91, + MSG_SENSOR = 0x60, + MSG_VALUE_REQUEST = 0x30, + MSG_VALUE_SET = 0x31, + MSG_VALUE = 0x32, + MSG_PIN_REQUEST = 0x80, + MSG_PIN_SET = 0x81, + MSG_ACKNOWLEDGE = 0x0, + MSG_HEARTBEAT = 0x1, + MSG_ATTITUDE = 0x2, + MSG_LOCATION = 0x3, + MSG_PRESSURE = 0x4, + MSG_TRIM_STARTUP = 0x50, + MSG_STATUS_TEXT = 0x5, + MSG_TRIM_MIN = 0x51, + MSG_PERF_REPORT = 0x6, + MSG_TRIM_MAX = 0x52, + MSG_VERSION_REQUEST = 0x7, + MSG_SERVOS = 0x53, + MSG_VERSION = 0x8, + MSG_COMMAND_REQUEST = 0x20, + MSG_COMMAND_UPLOAD = 0x21, + MSG_COMMAND_LIST = 0x22, + MSG_COMMAND_MODE_CHANGE = 0x23, + MSG_SIM = 0x70, + MSG_EEPROM_REQUEST = 0xa0, + MSG_EEPROM_SET = 0xa1, + MSG_PID_REQUEST = 0x40, + MSG_PID_SET = 0x41, + MSG_ANY = 0xfe, + MSG_NULL = 0xff +}; +#pragma pack(pop) diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk new file mode 100644 index 0000000000..3b5e0b65e4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk @@ -0,0 +1,174 @@ +# +# Process the protocol specification and emit functions to pack and unpack buffers. +# +# See protocol.def for a description of the definition format. +# + +BEGIN { + printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") + printf("/// @file protocol.h\n") + printf("#pragma pack(1)\n"); + + currentMessage = "" +} + +END { + # finalise the last message definition + EMIT_MESSAGE() + + # + # emit the MessageID enum + # + # XXX it would be elegant to sort the array here, but not + # everyone has GNU awk. + # + printf("\n//////////////////////////////////////////////////////////////////////\n") + printf("/// Message ID values\n") + printf("enum MessageID {\n") + for (opcode in opcodes) { + printf("\t%s = 0x%x,\n", opcodes[opcode], opcode) + } + printf("\tMSG_ANY = 0xfe,\n") + printf("\tMSG_NULL = 0xff\n") + printf("};\n") + + printf("#pragma pack(pop)\n") +} + +# +# Emit definitions for one message +# +function EMIT_MESSAGE(payloadSize) +{ + if (currentMessage != "") { + printf("\n//////////////////////////////////////////////////////////////////////\n") + printf("/// @name %s \n//@{\n\n", currentMessage) + + # + # emit a structure defining the message payload + # + printf("/// Structure describing the payload section of the %s message\n", currentMessage) + printf("struct %s {\n", tolower(currentMessage)) + for (i = 0; i < fieldCount; i++) { + printf("\t%s %s", types[i], names[i]) + if (counts[i]) + printf("[%s]", counts[i]) + printf(";\n") + } + printf("};\n\n") + + # + # emit a routine to pack the message payload from a set of variables and send it + # + printf("/// Send a %s message\n", currentMessage) + printf("inline void\nsend_%s(\n", tolower(currentMessage)) + for (i = 0; i < fieldCount; i++) { + if (counts[i]) { + printf("\tconst %s (&%s)[%d]", types[i], names[i], counts[i]) + } else { + printf("\tconst %s %s", types[i], names[i]) + } + if (i < (fieldCount -1)) + printf(",\n"); + } + printf(")\n{\n") + printf("\tuint8_t *__p = &_encodeBuf.payload[0];\n") + payloadSize = 0; + for (i = 0; i < fieldCount; i++) { + if (counts[i]) { + printf("\t_pack(__p, %s, %s);\n", names[i], counts[i]) + payloadSize += sizes[i] * counts[i] + } else { + printf("\t_pack(__p, %s);\n", names[i]) + payloadSize += sizes[i] + } + } + printf("\t_encodeBuf.header.length = %s;\n", payloadSize) + printf("\t_encodeBuf.header.messageID = %s;\n", currentMessage) + printf("\t_encodeBuf.header.messageVersion = MSG_VERSION_1;\n") + printf("\t_sendMessage();\n") + printf("};\n\n") + + # + # emit a routine to unpack the current message into a set of variables + # + printf("/// Unpack a %s message\n", currentMessage) + printf("inline void\nunpack_%s(\n", tolower(currentMessage)) + for (i = 0; i < fieldCount; i++) { + if (counts[i]) { + printf("\t%s (&%s)[%d]", types[i], names[i], counts[i]) + } else { + printf("\t%s &%s", types[i], names[i]) + } + if (i < (fieldCount -1)) + printf(",\n"); + } + printf(")\n{\n") + printf("\tuint8_t *__p = &_decodeBuf.payload[0];\n") + for (i = 0; i < fieldCount; i++) { + if (counts[i]) { + printf("\t_unpack(__p, %s, %s);\n", names[i], counts[i]) + payloadSize += sizes[i] * counts[i] + } else { + printf("\t_unpack(__p, %s);\n", names[i]) + payloadSize += sizes[i] + } + } + printf("};\n") + + # close the Doxygen group + printf("//@}\n") + } +} + +# skip lines containing comments +$1=="#" { + next +} + +# +# process a new message declaration +# +$1=="message" { + + # emit any previous message + EMIT_MESSAGE() + + # save the current opcode and message name + currentOpcode = $2 + currentMessage = $3 + opcodes[$2] = $3 + + # set up for the coming fields + fieldCount = 0 + delete types + delete names + delete sizes + delete counts + + next +} + +# +# process a field inside a message definition +# +NF >= 2 { + + # save the field definition + types[fieldCount] = $1 + names[fieldCount] = $2 + + # guess the field size, note that we only support and "char" + sizes[fieldCount] = 1 + if ($1 ~ ".*16.*") + sizes[fieldCount] = 2 + if ($1 ~ ".*32.*") + sizes[fieldCount] = 4 + + # if an array size was supplied, save it + if (NF >= 3) { + counts[fieldCount] = $3 + } + + fieldCount++ +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile new file mode 100644 index 0000000000..a484d5a004 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile @@ -0,0 +1,14 @@ + +PROG := BinCommTest +SRCS := test.cpp ../APM_BinComm.cpp +OBJS := $(SRCS:.cpp=.o) + +BinCommTest: $(OBJS) + c++ -g -o $@ $(OBJS) + +.cpp.o: + @echo C++ $< -> $@ + c++ -g -c -I. -o $@ $< + +clean: + rm $(PROG) $(OBJS) diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h new file mode 100644 index 0000000000..539db09c5d --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h @@ -0,0 +1,15 @@ + +#ifndef WPROGRAM_H +#define WPROGRAM_H + +class Stream { +public: + void write(uint8_t val); + int available(void); + int read(void); +}; + + +extern unsigned int millis(void); + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp new file mode 100644 index 0000000000..e15f1827f0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp @@ -0,0 +1,110 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- + +// test harness for the APM_BinComm bits + +#include +#include +#include +#include +#include +#include + +#include "WProgram.h" + +#include "../APM_BinComm.h" + +static void handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData); + +BinComm::MessageHandler handlers[] = { + {BinComm::MSG_ANY, handler, NULL}, + {BinComm::MSG_NULL, 0, 0} +}; + +Stream port; +BinComm comm(handlers, &port); + +int port_fd; + +unsigned int +millis(void) +{ + return 0; +} + +void +Stream::write(uint8_t val) +{ + ::write(port_fd, &val, 1); +} + +int +Stream::available(void) +{ + return(1); +} + +int +Stream::read(void) +{ + int ret; + uint8_t c; + + switch(::read(port_fd, &c, 1)) { + case 1: + return c; + case 0: + errx(1, "device disappeared"); + + default: + // almost certainly EWOULDBLOCK + return -1; + } +} + +void +handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData) +{ + + if (messageId == BinComm::MSG_HEARTBEAT) { + struct BinComm::msg_heartbeat *m = (struct BinComm::msg_heartbeat *)messageData; + printf("Heartbeat: mode %u time %u voltage %u command %u\n", + m->flightMode, m->timeStamp, m->batteryVoltage, m->commandIndex); + } else + if (messageId == BinComm::MSG_ATTITUDE) { + struct BinComm::msg_attitude *m = (struct BinComm::msg_attitude *)messageData; + printf("Attitude: pitch %d roll %d yaw %d\n", + m->pitch, m->roll, m->yaw); + } else + if (messageId == BinComm::MSG_LOCATION) { + struct BinComm::msg_location *m = (struct BinComm::msg_location *)messageData; + printf("Location: lat %d long %d altitude %d groundspeed %d groundcourse %d time %u\n", + m->latitude, m->longitude, m->altitude, m->groundSpeed, m->groundCourse, m->timeOfWeek); + } else + if (messageId == BinComm::MSG_STATUS_TEXT) { + struct BinComm::msg_status_text *m = (struct BinComm::msg_status_text *)messageData; + printf("Message %d: %-50s\n", m->severity, m->text); + } else { + warnx("received message %d,%d", messageId, messageVersion); + } +} + +int +main(int argc, char *argv[]) +{ + struct termios t; + + if (2 > argc) + errx(1, "BinCommTest "); + if (0 >= (port_fd = open(argv[1], O_RDWR | O_NONBLOCK))) + err(1, "could not open port %s", argv[1]); + if (tcgetattr(port_fd, &t)) + err(1, "tcgetattr"); + cfsetspeed(&t, 115200); + if (tcsetattr(port_fd, TCSANOW, &t)) + err(1, "tcsetattr"); + + // spin listening + for (;;) { + comm.update(); + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp new file mode 100644 index 0000000000..4b83191ef9 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp @@ -0,0 +1,235 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + APM_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor is conected to I2C port + Sensor is initialized in Continuos mode (10Hz) + + Variables: + Heading : Magnetic heading + Heading_X : Magnetic heading X component + Heading_Y : Magnetic heading Y component + Mag_X : Raw X axis magnetometer data + Mag_Y : Raw Y axis magnetometer data + Mag_Z : Raw Z axis magnetometer data + lastUpdate : the time of the last successful reading + + Methods: + Init() : Initialization of I2C and sensor + Read() : Read Sensor data + Calculate(float roll, float pitch) : Calculate tilt adjusted heading + SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass + SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances + SetDeclination(float radians) : Set heading adjustment between true north and magnetic north + + To do : code optimization + Mount position : UPDATED + Big capacitor pointing backward, connector forward + +*/ +extern "C" { + // AVR LibC Includes + #include + #include "WConstants.h" +} + +#include +#include "APM_Compass.h" + +#define CompassAddress 0x1E +#define ConfigRegA 0x00 +#define ConfigRegB 0x01 +#define MagGain 0x20 +#define PositiveBiasConfig 0x11 +#define NegativeBiasConfig 0x12 +#define NormalOperation 0x10 +#define ModeRegister 0x02 +#define ContinuousConversion 0x00 +#define SingleConversion 0x01 + +// Constructors //////////////////////////////////////////////////////////////// +APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0) +{ + // mag x y z offset initialisation + offset[0] = 0; + offset[1] = 0; + offset[2] = 0; + + // initialise orientation matrix + orientationMatrix = ROTATION_NONE; +} + +// Public Methods ////////////////////////////////////////////////////////////// +bool APM_Compass_Class::Init(void) +{ + unsigned long currentTime = millis(); // record current time + int numAttempts = 0; + int success = 0; + + Wire.begin(); + + delay(10); + + // calibration initialisation + calibration[0] = 1.0; + calibration[1] = 1.0; + calibration[2] = 1.0; + + while( success == 0 && numAttempts < 5 ) + { + // record number of attempts at initialisation + numAttempts++; + + // force positiveBias (compass should return 715 for all channels) + Wire.beginTransmission(CompassAddress); + Wire.send(ConfigRegA); + Wire.send(PositiveBiasConfig); + if (0 != Wire.endTransmission()) + continue; // compass not responding on the bus + delay(50); + + // set gains + Wire.beginTransmission(CompassAddress); + Wire.send(ConfigRegB); + Wire.send(MagGain); + Wire.endTransmission(); + delay(10); + + Wire.beginTransmission(CompassAddress); + Wire.send(ModeRegister); + Wire.send(SingleConversion); + Wire.endTransmission(); + delay(10); + + // read values from the compass + Read(); + delay(10); + + // calibrate + if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000) + { + calibration[0] = abs(715.0 / Mag_X); + calibration[1] = abs(715.0 / Mag_Y); + calibration[2] = abs(715.0 / Mag_Z); + + // mark success + success = 1; + } + + // leave test mode + Wire.beginTransmission(CompassAddress); + Wire.send(ConfigRegA); + Wire.send(NormalOperation); + Wire.endTransmission(); + delay(50); + + Wire.beginTransmission(CompassAddress); + Wire.send(ModeRegister); + Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) + Wire.endTransmission(); // End transmission + delay(50); + } + return(success); +} + +// Read Sensor data +void APM_Compass_Class::Read() +{ + int i = 0; + byte buff[6]; + + Wire.beginTransmission(CompassAddress); + Wire.send(0x03); //sends address to read from + Wire.endTransmission(); //end transmission + + //Wire.beginTransmission(CompassAddress); + Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device + while(Wire.available()) + { + buff[i] = Wire.receive(); // receive one byte + i++; + } + Wire.endTransmission(); //end transmission + + if (i==6) // All bytes received? + { + // MSB byte first, then LSB, X,Y,Z + Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis + Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis + Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis + lastUpdate = millis(); // record time of update + } +} + +void APM_Compass_Class::Calculate(float roll, float pitch) +{ + float Head_X; + float Head_Y; + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + Vector3f rotMagVec; + + cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? + sin_roll = sin(roll); + cos_pitch = cos(pitch); + sin_pitch = sin(pitch); + + // rotate the magnetometer values depending upon orientation + if( orientation == 0 ) + rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); + else + rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); + + // Tilt compensated Magnetic field X component: + Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; + // Tilt compensated Magnetic field Y component: + Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; + // Magnetic Heading + Heading = atan2(-Head_Y,Head_X); + + // Declination correction (if supplied) + if( declination != 0.0 ) + { + Heading = Heading + declination; + if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) + Heading -= (2.0 * M_PI); + else if (Heading < -M_PI) + Heading += (2.0 * M_PI); + } + + // Optimization for external DCM use. Calculate normalized components + Heading_X = cos(Heading); + Heading_Y = sin(Heading); +} + +void APM_Compass_Class::SetOrientation(const Matrix3f &rotationMatrix) +{ + orientationMatrix = rotationMatrix; + if( orientationMatrix == ROTATION_NONE ) + orientation = 0; + else + orientation = 1; +} + +void APM_Compass_Class::SetOffsets(int x, int y, int z) +{ + offset[0] = x; + offset[1] = y; + offset[2] = z; +} + +void APM_Compass_Class::SetDeclination(float radians) +{ + declination = radians; +} + +// make one instance for the user to use +APM_Compass_Class APM_Compass; diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h new file mode 100644 index 0000000000..a8d0282a85 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h @@ -0,0 +1,69 @@ +#ifndef APM_Compass_h +#define APM_Compass_h + +#include "../AP_Math/AP_Math.h" + +// Rotation matrices +#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) +#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) +#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) +#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) +#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) +#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) +#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) +#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) +#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) + +#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE +#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45 +#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 +#define APM_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135 +#define APM_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 +#define APM_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225 +#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 +#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 +#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90 +#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 +#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225 +#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270 +#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 + +class APM_Compass_Class +{ + private: + int orientation; + Matrix3f orientationMatrix; + float calibration[3]; + int offset[3]; + float declination; + public: + int Mag_X; + int Mag_Y; + int Mag_Z; + float Heading; + float Heading_X; + float Heading_Y; + unsigned long lastUpdate; + + APM_Compass_Class(); // Constructor + bool Init(); + void Read(); + void Calculate(float roll, float pitch); + void SetOrientation(const Matrix3f &rotationMatrix); + void SetOffsets(int x, int y, int z); + void SetDeclination(float radians); +}; + +extern APM_Compass_Class APM_Compass; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde new file mode 100644 index 0000000000..3216f89f55 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde @@ -0,0 +1,87 @@ +/* + Example of APM_Compass library (HMC5843 sensor). + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + offsets displayed are calculated from the min/max values from each axis. + rotate the compass so as to produce the maximum and minimum values + after the offsets stop changing, edit the code to pass these offsets into + APM_Compass.SetOffsets. +*/ + +#include +#include // Compass Library +#include // Math library + +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +unsigned long timer; + +void setup() +{ + Serial.begin(38400); + Serial.println("Compass library test (HMC5843)"); + + APM_Compass.Init(); // Initialization + APM_Compass.SetOrientation(APM_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft + APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference + APM_Compass.SetDeclination(ToRad(0.0)); // set local difference between magnetic north and true north + + delay(1000); + timer = millis(); +} + +void loop() +{ + static float min[3], max[3], offset[3]; + + if((millis()- timer) > 100) + { + timer = millis(); + APM_Compass.Read(); + APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example + + // capture min + if( APM_Compass.Mag_X < min[0] ) + min[0] = APM_Compass.Mag_X; + if( APM_Compass.Mag_Y < min[1] ) + min[1] = APM_Compass.Mag_Y; + if( APM_Compass.Mag_Z < min[2] ) + min[2] = APM_Compass.Mag_Z; + + // capture max + if( APM_Compass.Mag_X > max[0] ) + max[0] = APM_Compass.Mag_X; + if( APM_Compass.Mag_Y > max[1] ) + max[1] = APM_Compass.Mag_Y; + if( APM_Compass.Mag_Z > max[2] ) + max[2] = APM_Compass.Mag_Z; + + // calculate offsets + offset[0] = -(max[0]+min[0])/2; + offset[1] = -(max[1]+min[1])/2; + offset[2] = -(max[2]+min[2])/2; + + // display all to user + Serial.print("Heading:"); + Serial.print(ToDeg(APM_Compass.Heading)); + Serial.print(" ("); + Serial.print(APM_Compass.Mag_X); + Serial.print(","); + Serial.print(APM_Compass.Mag_Y); + Serial.print(","); + Serial.print(APM_Compass.Mag_Z); + Serial.print(")"); + + // display offsets + Serial.print("\t offsets("); + Serial.print(offset[0]); + Serial.print(","); + Serial.print(offset[1]); + Serial.print(","); + Serial.print(offset[2]); + Serial.print(")"); + + Serial.println(); + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt new file mode 100644 index 0000000000..617c9aa805 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt @@ -0,0 +1,19 @@ +Compass KEYWORD1 +AP_Compass KEYWORD1 +APM_Compass KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Calculate KEYWORD2 +SetOrientation KEYWORD2 +SetOffsets KEYWORDS2 +SetDeclination KEYWORDS2 +Heading KEYWORD2 +Heading_X KEYWORD2 +Heading_Y KEYWORD2 +Mag_X KEYWORD2 +Mag_Y KEYWORD2 +Mag_Z KEYWORD2 +lastUpdate KEYWORD2 + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp new file mode 100644 index 0000000000..6b811caa5b --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp @@ -0,0 +1,150 @@ +/* + APM_FastSerial.cpp - Fast Serial Output for Ardupilot Mega Hardware (atmega1280) + It´s also compatible with standard Arduino boards (atmega 168 and 328) + Interrupt driven Serial output with intermediate buffer + Code Jose Julio and Jordi Muñoz. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library works as a complement of the standard Arduino Serial + library. So user must initialize Standard Serial Arduino library first. + This library works in Serial port 0 and Serial port3(telemetry port)[APM] + Methods: (the same as standard arduino library, inherits from Print) + write() for bytes or array of bytes (binary output) + print() for chars, strings, numbers and floats + println() +*/ + +//#include "WProgram.h" +#include "APM_FastSerial.h" +extern "C" { + // AVR LibC Includes + #include + #include + #include + #include "WConstants.h" +} +#define TX_BUFFER_SIZE 80 // Serial output buffer size + +// Serial0 buffer +uint8_t tx_buffer0[TX_BUFFER_SIZE]; +volatile int tx_buffer0_head=0; +volatile int tx_buffer0_tail=0; + +#if defined(__AVR_ATmega1280__) +// Serial3 buffer +uint8_t tx_buffer3[TX_BUFFER_SIZE]; +volatile int tx_buffer3_head=0; +volatile int tx_buffer3_tail=0; +#endif + +#if defined(__AVR_ATmega1280__) // For atmega1280 we use Serial port 0 and 3 +// Serial0 interrupt +ISR(SIG_USART0_DATA) +{ + uint8_t data; + + if (tx_buffer0_tail == tx_buffer0_head) + UCSR0B &= ~(_BV(UDRIE0)); // Disable interrupt + else { + data = tx_buffer0[tx_buffer0_tail]; + tx_buffer0_tail = (tx_buffer0_tail + 1) % TX_BUFFER_SIZE; + UDR0 = data; + } +} + +// Serial3 interrupt +ISR(SIG_USART3_DATA) +{ + uint8_t data; + + if (tx_buffer3_tail == tx_buffer3_head) + UCSR3B &= ~(_BV(UDRIE3)); // Disable interrupt + else { + data = tx_buffer3[tx_buffer3_tail]; + tx_buffer3_tail = (tx_buffer3_tail + 1) % TX_BUFFER_SIZE; + UDR3 = data; + } +} +#else + +// Serial interrupt +ISR(USART_UDRE_vect) +{ + uint8_t data; + + if (tx_buffer0_tail == tx_buffer0_head) + UCSR0B &= ~(_BV(UDRIE0)); // Disable interrupt + else { + data = tx_buffer0[tx_buffer0_tail]; + tx_buffer0_tail = (tx_buffer0_tail + 1) % TX_BUFFER_SIZE; + UDR0 = data; + } +} +#endif + +// Constructors //////////////////////////////////////////////////////////////// +APM_FastSerial_Class::APM_FastSerial_Class(uint8_t SerialPort) +{ + SerialPortNumber=SerialPort; // This could be serial port 0 or 3 +} + +// Public Methods ////////////////////////////////////////////////////////////// + +// This is the important function (basic funtion: send a byte) +void APM_FastSerial_Class::write(uint8_t b) +{ + uint8_t Enable_tx_int=0; + uint8_t new_head; + + if (SerialPortNumber==0) // Serial Port 0 + { + // if buffer was empty then we enable Serial TX interrupt + if (tx_buffer0_tail == tx_buffer0_head) + Enable_tx_int=1; + + new_head = (tx_buffer0_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer + if (new_head==tx_buffer0_tail) + return; // This is an Error : BUFFER OVERFLOW. We lost this character!! + + tx_buffer0[tx_buffer0_head] = b; // Add data to the tx buffer + tx_buffer0_head = new_head; // Update head pointer + + if (Enable_tx_int) + UCSR0B |= _BV(UDRIE0); // Enable Serial TX interrupt + } +#if defined(__AVR_ATmega1280__) + else // Serial Port 3 + { + // if buffer was empty then we enable Serial TX interrupt + if (tx_buffer3_tail == tx_buffer3_head) + Enable_tx_int=1; + + new_head = (tx_buffer3_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer + if (new_head==tx_buffer3_tail) + return; // This is an Error : BUFFER OVERFLOW. We lost this character!! + + tx_buffer3[tx_buffer3_head] = b; // Add data to the tx buffer + tx_buffer3_head = new_head; // Update head pointer + + if (Enable_tx_int) + UCSR3B |= _BV(UDRIE3); // Enable Serial TX interrupt + } +#endif +} + +// Send a buffer of bytes (this is util for binary protocols) +void APM_FastSerial_Class::write(const uint8_t *buffer, int size) +{ + while (size--) + write(*buffer++); +} + +// We create this two instances +APM_FastSerial_Class APM_FastSerial(0); // For Serial port 0 +#if defined(__AVR_ATmega1280__) + APM_FastSerial_Class APM_FastSerial3(3); // For Serial port 3 (only Atmega1280) +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h new file mode 100644 index 0000000000..319ec523dd --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h @@ -0,0 +1,25 @@ +#ifndef APM_FastSerial_h +#define APM_FastSerial_h + +#include + +#include "Print.h" + +class APM_FastSerial_Class : public Print // Inherit from Print +{ + private: + uint8_t SerialPortNumber; + + public: + APM_FastSerial_Class(uint8_t SerialPort); // Constructor + // we overwrite the write methods + void write(uint8_t b); // basic funtion : send a byte + void write(const uint8_t *buffer, int size); +}; + +extern APM_FastSerial_Class APM_FastSerial; +#if defined(__AVR_ATmega1280__) + extern APM_FastSerial_Class APM_FastSerial3; +#endif +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde new file mode 100644 index 0000000000..97616237ed --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde @@ -0,0 +1,91 @@ +/* + Example of APM_FastSerial library. + Code by Jose Julio and Jordi Muñoz. DIYDrones.com +*/ + +// FastSerial is implemented for Serial port 0 (connection to PC) and 3 (telemetry) +#include // ArduPilot Mega Fast Serial Output Library + +#if defined(__AVR_ATmega1280__) + #define LED 35 +#else + #define LED 13 +#endif + +unsigned long timer; +unsigned long counter; + +void setup() +{ + int myint=14235; // Examples of data tytpes + long mylong=-23456432; + float myfloat=-26.669; + byte mybyte=0xD1; + byte bc_bufIn[50]; + + for (int i=0;i<10;i++) + bc_bufIn[i]=i*10+30; // we fill the byte array + + pinMode(LED,OUTPUT); + + // We use the standard serial port initialization + Serial.begin(57600); + //Serial3.begin(57600); // if we want to use port3 also (Mega boards)... + delay(100); + // We can use both methods to write to serial port: + Serial.println("ArduPilot Mega FastSerial library test"); // Standard serial output + APM_FastSerial.println("FAST_SERIAL : ArduPilot Mega FastSerial library test"); // Fast serial output + // We can use the same on serial port3 (telemetry) + // APM_FastSerial3.println("Serial Port3 : ArduPilot Mega FastSerial library test"); + delay(1000); + // Examples of data types (same result as standard arduino library) + APM_FastSerial.println("Numbers:"); + APM_FastSerial.println(myint); + APM_FastSerial.println(mylong); + APM_FastSerial.println(myfloat); + APM_FastSerial.println("Byte:"); + APM_FastSerial.write(mybyte); + APM_FastSerial.println(); + APM_FastSerial.println("Bytes:"); + APM_FastSerial.write(bc_bufIn,10); + APM_FastSerial.println(); + delay(4000); +} + +void loop() +{ + if((millis()- timer) > 20) // 50Hz loop + { + timer = millis(); + if (counter < 250) // we use the Normal Serial output for 5 seconds + { + // Example of typical telemetry output + digitalWrite(LED,HIGH); + Serial.println("!ATT:14.2;-5.5;-26.8"); // 20 bytes + digitalWrite(LED,LOW); + if ((counter%5)==0) // GPS INFO at 5Hz + { + digitalWrite(LED,HIGH); + Serial.println("!LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes + digitalWrite(LED,LOW); + } + } + else // and Fast Serial Output for other 5 seconds + { + // The same info... + digitalWrite(LED,HIGH); + APM_FastSerial.println("#ATT:14.2;-5.5;-26.8"); // 20 bytes + digitalWrite(LED,LOW); + if ((counter%5)==0) // GPS INFO at 5Hz + { + digitalWrite(LED,HIGH); + + APM_FastSerial.println("#LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes + digitalWrite(LED,LOW); + } + if (counter>500) // Counter reset + counter=0; + } + counter++; + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt new file mode 100644 index 0000000000..91cfc02dea --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt @@ -0,0 +1,2 @@ +APM_FastSerial KEYWORD1 +APM_FastSerial3 KEYWORD1 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp new file mode 100644 index 0000000000..7099b44e26 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp @@ -0,0 +1,193 @@ +/* + APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + RC Input : PPM signal on IC4 pin + RC Output : 11 Servo outputs (standard 20ms frame) + + Methods: + Init() : Initialization of interrupts an Timers + OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 + InputCh(ch) : Read a channel input value. ch=0..7 + GetState() : Returns the state of the input. 1 => New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ +#include "APM_RC.h" + +#include +#include "WProgram.h" + +#if !defined(__AVR_ATmega1280__) +# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. +#else + +// Variable definition for Input Capture interrupt +volatile unsigned int ICR4_old; +volatile unsigned char PPM_Counter=0; +volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400}; +volatile unsigned char radio_status=0; + +/**************************************************** + Input Capture Interrupt ICP4 => PPM signal read + ****************************************************/ +ISR(TIMER4_CAPT_vect) +{ + unsigned int Pulse; + unsigned int Pulse_Width; + + Pulse=ICR4; + if (Pulse8000) // SYNC pulse? + PPM_Counter=0; + else + { + PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) + PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse. + if (PPM_Counter >= NUM_CHANNELS) + radio_status = 1; + } + ICR4_old = Pulse; +} + + +// Constructors //////////////////////////////////////////////////////////////// + +APM_RC_Class::APM_RC_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_RC_Class::Init(void) +{ + // Init PWM Timer 1 + pinMode(11,OUTPUT); // (PB5/OC1A) + pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) + pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1<>1; // Because timer runs at 0.5us we need to do value/2 + result2 = PWM_RAW[ch]>>1; + if (result != result2) + result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) + + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + radio_status=0; // Radio channel read + return(result); +} + +unsigned char APM_RC_Class::GetState(void) +{ + return(radio_status); +} + +// InstantPWM implementation +// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use +void APM_RC_Class::Force_Out0_Out1(void) +{ + if (TCNT5>5000) // We take care that there are not a pulse in the output + TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 +} +// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use +void APM_RC_Class::Force_Out2_Out3(void) +{ + if (TCNT1>5000) + TCNT1=39990; +} +// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use +void APM_RC_Class::Force_Out6_Out7(void) +{ + if (TCNT3>5000) + TCNT3=39990; +} + +// make one instance for the user to use +APM_RC_Class APM_RC; + +#endif // defined(ATMega1280) diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h new file mode 100644 index 0000000000..3874e0a319 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h @@ -0,0 +1,24 @@ +#ifndef APM_RC_h +#define APM_RC_h + +#define NUM_CHANNELS 8 +#define MIN_PULSEWIDTH 900 +#define MAX_PULSEWIDTH 2100 + +class APM_RC_Class +{ + private: + public: + APM_RC_Class(); + void Init(); + void OutputCh(unsigned char ch, int pwm); + int InputCh(unsigned char ch); + unsigned char GetState(); + void Force_Out0_Out1(void); + void Force_Out2_Out3(void); + void Force_Out6_Out7(void); +}; + +extern APM_RC_Class APM_RC; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde new file mode 100644 index 0000000000..3edc3d223a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde @@ -0,0 +1,31 @@ +/* + Example of APM_RC library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library + +void setup() +{ + APM_RC.Init(); // APM Radio initialization + Serial.begin(57600); + Serial.println("ArduPilot Mega RC library test"); + delay(1000); +} +void loop() +{ + if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20) + { + Serial.print("CH:"); + for(int i=0;i<8;i++) + { + Serial.print(APM_RC.InputCh(i)); // Print channel values + Serial.print(","); + APM_RC.OutputCh(i,APM_RC.InputCh(i)); // Copy input to Servos + } + Serial.println(); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt new file mode 100644 index 0000000000..0fba1dd8a0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt @@ -0,0 +1,8 @@ +APM_RC KEYWORD1 +begin KEYWORD2 +InputCh KEYWORD2 +OutputCh KEYWORD2 +GetState KEYWORD2 +Force_Out0_Out1 KEYWORD2 +Force_Out2_Out3 KEYWORD2 +Force_Out6_Out7 KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp new file mode 100644 index 0000000000..da5f4dd4a9 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp @@ -0,0 +1,169 @@ +/* + APM_RC_QUAD.cpp - Radio Control Library for Ardupilot Mega. Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This is a special version of the library for use in Quadcopters + Because we have modified servo outputs 0..3 to have a higher rate (300Hz) + + RC Input : PPM signal on IC4 pin + RC Output : 8 servo Outputs, + OUT0..OUT3 : 300Hz Servo output (for Quad ESC´s) + OUT4..OUT7 : standard 50Hz servo outputs + + Methods: + Init() : Initialization of interrupts an Timers + OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 + InputCh(ch) : Read a channel input value. ch=0..7 + GetState() : Returns the state of the input. 1 => New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ +#include "APM_RC_QUAD.h" + +#include +#include "WProgram.h" + +// Variable definition for Input Capture interrupt +volatile unsigned int ICR4_old; +volatile unsigned char PPM_Counter=0; +volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400}; +volatile unsigned char radio_status=0; + +/**************************************************** + Input Capture Interrupt ICP4 => PPM signal read + ****************************************************/ +ISR(TIMER4_CAPT_vect) +{ + unsigned int Pulse; + unsigned int Pulse_Width; + + Pulse=ICR4; + if (Pulse8000) // SYNC pulse? + PPM_Counter=0; + else + { + PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) + PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse. + if (PPM_Counter >= NUM_CHANNELS) + radio_status = 1; + } + ICR4_old = Pulse; +} + + +// Constructors //////////////////////////////////////////////////////////////// + +APM_RC_QUAD_Class::APM_RC_QUAD_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_RC_QUAD_Class::Init(void) +{ + // Init PWM Timer 1 + //pinMode(11,OUTPUT); // (PB5/OC1A) + pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) + pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1<>1; // Because timer runs at 0.5us we need to do value/2 + result2 = PWM_RAW[ch]>>1; + if (result != result2) + result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) + + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + radio_status=0; // Radio channel read + return(result); +} + +unsigned char APM_RC_QUAD_Class::GetState(void) +{ + return(radio_status); +} + +// make one instance for the user to use +APM_RC_QUAD_Class APM_RC_QUAD; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h new file mode 100644 index 0000000000..421089d7af --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h @@ -0,0 +1,21 @@ +#ifndef APM_RC_QUAD_h +#define APM_RC_QUAD_h + +#define NUM_CHANNELS 8 +#define MIN_PULSEWIDTH 900 +#define MAX_PULSEWIDTH 2100 + +class APM_RC_QUAD_Class +{ + private: + public: + APM_RC_QUAD_Class(); + void Init(); + void OutputCh(unsigned char ch, int pwm); + int InputCh(unsigned char ch); + unsigned char GetState(); +}; + +extern APM_RC_QUAD_Class APM_RC_QUAD; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde new file mode 100644 index 0000000000..3edc3d223a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde @@ -0,0 +1,31 @@ +/* + Example of APM_RC library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library + +void setup() +{ + APM_RC.Init(); // APM Radio initialization + Serial.begin(57600); + Serial.println("ArduPilot Mega RC library test"); + delay(1000); +} +void loop() +{ + if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20) + { + Serial.print("CH:"); + for(int i=0;i<8;i++) + { + Serial.print(APM_RC.InputCh(i)); // Print channel values + Serial.print(","); + APM_RC.OutputCh(i,APM_RC.InputCh(i)); // Copy input to Servos + } + Serial.println(); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt new file mode 100644 index 0000000000..762966e545 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt @@ -0,0 +1,5 @@ +APM_RC_QUAD KEYWORD1 +begin KEYWORD2 +InputCh KEYWORD2 +OutputCh KEYWORD2 +GetState KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp new file mode 100644 index 0000000000..5b476c4f09 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp @@ -0,0 +1,17 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// @file AP_Common.cpp +/// @brief Common utility routines for the ArduPilot libraries. +/// +/// @note Exercise restraint adding code here; everything in this +/// library will be linked with any sketch using it. +/// + +#include "AP_Common.h" + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h new file mode 100644 index 0000000000..8ad2f0fbf5 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h @@ -0,0 +1,53 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// +/// @file AP_Common.h +/// @brief Common definitions and utility routines for the ArduPilot +/// libraries. +/// + +#ifndef _AP_COMMON_H +#define _AP_COMMON_H + +#include + +#include "include/menu.h" /// simple menu subsystem + +//////////////////////////////////////////////////////////////////////////////// +/// @name Types +/// +/// Data structures and types used throughout the libraries and applications. +/// +//@{ + +struct Location { + uint8_t id; ///< command id + 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 +}; + +//@} + +//////////////////////////////////////////////////////////////////////////////// +/// @name Conversions +/// +/// Conversion macros and factors. +/// +//@{ + +/// XXX this should probably be replaced with radians()/degrees(), but their +/// inclusion in wiring.h makes doing that here difficult. +#define ToDeg(x) (x*57.2957795131) // *180/pi + +//@} + + +#endif // _AP_COMMON_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp new file mode 100644 index 0000000000..107d7fb045 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp @@ -0,0 +1,21 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// +// C++ runtime support not provided by Arduino +// +// Note: use new/delete with caution. The heap is small and +// easily fragmented. +// + +#include + +void * operator new(size_t size) +{ + return(calloc(size, 1)); +} + +void operator delete(void *p) +{ + if (p) + free(p); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde new file mode 100644 index 0000000000..fea9efa188 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde @@ -0,0 +1,45 @@ + +#include +#include + +#include + +FastSerialPort0(Serial); + +int +menu_test(uint8_t argc, const Menu::arg *argv) +{ + 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 + } +} + +int +menu_auto(uint8_t argc, const Menu::arg *argv) +{ + Serial.println("auto text"); +} + +const struct Menu::command top_menu_commands[] PROGMEM = { + {"*", menu_auto}, + {"test", menu_test}, +}; + +MENU(top, "menu", top_menu_commands); + +void +setup(void) +{ + Serial.begin(38400); + top.run(); +} + +void +loop(void) +{ +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h new file mode 100644 index 0000000000..96b863c05b --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h @@ -0,0 +1,138 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file menu.h +/// @brief Simple commandline menu subsystem. +/// @discussion +/// The Menu class implements a simple CLI that accepts commands typed by +/// the user, and passes the arguments to those commands to a function +/// defined as handing the command. +/// +/// Commands are defined in an array of Menu::command structures passed +/// to the constructor. Each entry in the array defines one command. +/// +/// Arguments passed to the handler function are pre-converted to both +/// long and float for convenience. +/// + +#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length +#define MENU_ARGS_MAX 4 ///< maximum number of arguments +#define MENU_COMMAND_MAX 14 ///< maximum size of a command name + +/// 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) + }; + + /// 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 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 + }; + + /// 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); + +private: + /// 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); + + 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 +}; + +/// Macros used to define a menu. +/// +/// The commands argument should be an arary of Menu::command structures, one +/// per command name. The array does not need to be terminated with any special +/// record. +/// +/// Use name.run() to run the menu. +/// +/// The MENU2 macro supports the optional pre-prompt printing function. +/// +#define MENU(name, prompt, commands) \ + static const char __menu_name__ ##name[] PROGMEM = prompt; \ + static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0])) + +#define MENU2(name, prompt, commands, preprompt) \ + static const char __menu_name__ ##name[] PROGMEM = prompt; \ + static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0]), preprompt) + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt new file mode 100644 index 0000000000..470e71f7a8 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt @@ -0,0 +1,4 @@ +Menu KEYWORD1 +run KEYWORD2 +Location KEYWORD2 + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp new file mode 100644 index 0000000000..58e129d03e --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp @@ -0,0 +1,129 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// +// Simple commandline menu system. +// + +#include + +#include +#include +#include + +#include "include/menu.h" + +// statics +char Menu::_inbuf[MENU_COMMANDLINE_MAX]; +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) +{ +} + +// run the menu +void +Menu::run(void) +{ + uint8_t len, i, ret; + uint8_t argc; + int c; + char *s; + + // loop performing commands + for (;;) { + + // run the pre-prompt function, if one is defined + if ((NULL != _ppfunc) && !_ppfunc()) + return; + + // loop reading characters from the input + len = 0; + Serial.printf("%S] ", _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++; + } + + // 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", _commands[i].command); +} + +// run the n'th command in the menu +int8_t +Menu::_call(uint8_t n, uint8_t argc) +{ + func fn; + + fn = (func)pgm_read_word(&_commands[n].func); + return(fn(argc, &_argv[0])); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp new file mode 100644 index 0000000000..a92cfa7acc --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp @@ -0,0 +1,115 @@ +/* + AP_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor is conected to I2C port + Sensor is initialized in Continuos mode (10Hz) + + Variables: + heading : Magnetic heading + heading_X : Magnetic heading X component + heading_Y : Magnetic heading Y component + mag_X : Raw X axis magnetometer data + mag_Y : Raw Y axis magnetometer data + mag_Z : Raw Z axis magnetometer data + + Methods: + init() : initialization of I2C and sensor + update() : update Sensor data + + To do : Calibration of the sensor, code optimization + Mount position : UPDATED + Big capacitor pointing backward, connector forward + +*/ + + +extern "C" { + // AVR LibC Includes + #include + #include "WConstants.h" +} + +#include +#include "AP_Compass.h" + +#define COMPASS_ADDRESS 0x1E + +// Constructors //////////////////////////////////////////////////////////////// +AP_Compass::AP_Compass() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void +AP_Compass::init(void) +{ + Wire.begin(); + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(0x02); + Wire.send(0x00); // Set continouos mode (default to 10Hz) + Wire.endTransmission(); // end transmission +} + +// update Sensor data +void +AP_Compass::update() +{ + int i = 0; + byte buff[6]; + + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(0x03); // sends address to read from + Wire.endTransmission(); // end transmission + + //Wire.beginTransmission(COMPASS_ADDRESS); + Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device + while(Wire.available()){ + buff[i] = Wire.receive(); // receive one byte + i++; + } + Wire.endTransmission(); // end transmission + + // All bytes received? + if (i == 6) { + // MSB byte first, then LSB, X,Y,Z + mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis + mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis + mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis + } +} + +void +AP_Compass::calculate(float roll, float pitch) +{ + float head_X; + float head_Y; + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + + cos_roll = cos(roll); // Optimization, you can get this out of the matrix DCM? + sin_roll = sin(roll); + cos_pitch = cos(pitch); + sin_pitch = sin(pitch); + + // Tilt compensated Magnetic field X component: + head_X = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch; + + // Tilt compensated Magnetic field Y component: + head_Y = mag_Y * cos_roll - mag_Z * sin_roll; + + // Magnetic heading + heading = atan2(-head_Y, head_X); + ground_course = (degrees(heading) + 180) * 100; + + // Optimization for external DCM use. calculate normalized components + heading_X = cos(heading); + heading_Y = sin(heading); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h new file mode 100644 index 0000000000..cbafb19e18 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h @@ -0,0 +1,17 @@ +#ifndef AP_Compass_h +#define AP_Compass_h + +#include + +class AP_Compass : public Compass +{ + public: + AP_Compass(); // Constructor + void init(); + void update(); + void calculate(float roll, float pitch); + + private: +}; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h new file mode 100644 index 0000000000..0852cdd52a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h @@ -0,0 +1,23 @@ +#ifndef Compass_h +#define Compass_h + +#include + +class Compass +{ + public: + virtual void init(); + virtual void update(); + virtual void calculate(float roll, float pitch); + + int16_t mag_X; + int16_t mag_Y; + int16_t mag_Z; + int32_t ground_course; + float heading; + float heading_X; + float heading_Y; + +}; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde new file mode 100644 index 0000000000..1a69883789 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -0,0 +1,40 @@ +/* + Example of APM_Compass library (HMC5843 sensor). + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com +*/ + +#include +#include // Compass Library + +AP_Compass compass; + +unsigned long timer; + +void setup() +{ + compass.init(); // Initialization + Serial.begin(38400); + Serial.println("AP Compass library test (HMC5843)"); + delay(1000); + timer = millis(); +} + +void loop() +{ + float tmp; + + if((millis()- timer) > 100){ + timer = millis(); + compass.update(); + compass.calculate(0, 0); // roll = 0, pitch = 0 for this example + Serial.print("Heading:"); + Serial.print(compass.ground_course,DEC); + Serial.print(" ("); + Serial.print(compass.mag_X); + Serial.print(","); + Serial.print(compass.mag_Y); + Serial.print(","); + Serial.print(compass.mag_Z); + Serial.println(" )"); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt new file mode 100644 index 0000000000..91affe3e73 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt @@ -0,0 +1,15 @@ +Compass KEYWORD1 +AP_Compass KEYWORD1 +APM_Compass KEYWORD1 +init KEYWORD2 +update KEYWORD2 +calculate KEYWORD2 +heading KEYWORD2 +heading_X KEYWORD2 +heading_Y KEYWORD2 +mag_X KEYWORD2 +mag_Y KEYWORD2 +mag_Z KEYWORD2 + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h new file mode 100644 index 0000000000..384d7daccc --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h @@ -0,0 +1,13 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_GPS.h +/// @brief Catch-all header that defines all supported GPS classes. + +#include "AP_GPS_NMEA.h" +#include "AP_GPS_SIRF.h" +#include "AP_GPS_406.h" +#include "AP_GPS_UBLOX.h" +#include "AP_GPS_MTK.h" +#include "AP_GPS_IMU.h" +#include "AP_GPS_None.h" +#include "AP_GPS_Auto.h" diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp new file mode 100644 index 0000000000..379d096cdd --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp @@ -0,0 +1,81 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// GPS_406.cpp - 406 GPS library for Arduino +// Code by Michael Smith, Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com +// This code works with boards based on ATMega168/328 ATMega1280 (Serial port 1) +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh. +#include "AP_GPS_406.h" +#include "WProgram.h" + +static const char init_str[] = "$PSRF100,0,57600,8,1,0*37"; + +// Constructors //////////////////////////////////////////////////////////////// +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 + + AP_GPS_SIRF::init(); // let the superclass do anything it might need here +} + +// Private Methods ////////////////////////////////////////////////////////////// + +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 + } + } +} + +// The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary +// mode at a higher rate. +// +// The change is sticky, but only for as long as the internal supercap holds +// settings (usually less than a week). +// +void +AP_GPS_406::_change_to_sirf_protocol(void) +{ + FastSerial *fs = (FastSerial *)_port; // this is a bit grody... + + fs->begin(4800); + 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); +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h new file mode 100644 index 0000000000..39b6e55164 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h @@ -0,0 +1,32 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// EM406 GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// + +#ifndef AP_GPS_406_h +#define AP_GPS_406_h + +#include "AP_GPS_SIRF.h" + +#define GPS_406_BITRATE 57600 + +class AP_GPS_406 : public AP_GPS_SIRF +{ +public: + // Methods + AP_GPS_406(Stream *port); + void init(void); + +private: + void _change_to_sirf_protocol(void); + void _configure_gps(void); +}; + +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp new file mode 100644 index 0000000000..727b1eac74 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -0,0 +1,162 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Auto-detecting pseudo-GPS driver +// + +#include "AP_GPS.h" +#include +#include +#include + +static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; + +void +AP_GPS_Auto::init(void) +{ +} + +// +// Called the first time that a client tries to kick the GPS to update. +// +// We detect the real GPS, then update the pointer we have been called through +// and return. +void +AP_GPS_Auto::update(void) +{ + GPS *gps; + int i; + + // loop trying to find a GPS + for (;;) { + // loop through possible baudrates + for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { + printf("GPS autodetect at %d:%u\n", i, baudrates[i]); + _port->begin(baudrates[i]); + if (NULL != (gps = _detect())) { + // make the detected GPS the default + *_gps = gps; + + // configure the detected GPS and run one update + gps->print_errors = true; // XXX + gps->init(); + gps->update(); + + // drop back to our caller - subsequent calls through + // the global will not come here + return; + } + } + } +} + +// +// Perform one iteration of the auto-detection process. +// +GPS * +AP_GPS_Auto::_detect(void) +{ + unsigned long then; + int fingerprint[4]; + int tries; + GPS *gps; + + // + // Loop attempting to detect a recognised GPS + // + 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 { + if (_port->available()) { + then = millis(); + _port->read(); + } + } while ((millis() - then) < 50); + + // + // Collect four characters to fingerprint a device + // + fingerprint[0] = _getc(); + fingerprint[1] = _getc(); + fingerprint[2] = _getc(); + fingerprint[3] = _getc(); + printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n", + fingerprint[0], + fingerprint[1], + fingerprint[2], + fingerprint[3]); + + // + // u-blox 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]) { + printf("detected MTK in binary mode\n"); + gps = new AP_GPS_MTK(_port); + break; + } + + // any other message is u-blox + printf("detected u-blox in binary mode\n"); + gps = new AP_GPS_UBLOX(_port); + break; + } + + // + // SIRF in binary mode + // + if ((0xa0 == fingerprint[0]) && + (0xa2 == fingerprint[1])) { + printf("detected SIRF in binary mode\n"); + gps = new AP_GPS_SIRF(_port); + 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) { + printf("sending setup strings and trying again\n"); + _port->println(MTK_SET_BINARY); + _port->println(UBLOX_SET_BINARY); + _port->println(SIRF_SET_BINARY); + continue; + } + + // + // 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? + printf("detected NMEA\n"); + gps = new AP_GPS_NMEA(_port); + break; + } + } + return(gps); +} + +int +AP_GPS_Auto::_getc(void) +{ + while (0 == _port->available()) + ; + return(_port->read()); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h new file mode 100644 index 0000000000..a259add7c0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h @@ -0,0 +1,52 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// Auto-detecting pseudo-GPS driver +// + +#ifndef AP_GPS_Auto_h +#define AP_GPS_Auto_h + +#include +#include + +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 *port, GPS **gps) : + _port(port), + _gps(gps) + {}; + + void init(void); + + /// Detect and initialise the attached GPS unit. Returns a + /// pointer to the allocated & initialised GPS driver. + /// + void update(void); + +private: + /// Serial port connected to the GPS. + FastSerial *_port; + + /// global GPS driver pointer, updated by auto-detection + /// + GPS **_gps; + + /// low-level auto-detect routine + /// + GPS *_detect(void); + + /// fetch a character from the port + /// + int _getc(void); +}; +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp new file mode 100644 index 0000000000..53195ef75f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -0,0 +1,224 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +/* + GPS_MTK.cpp - Ublox GPS library for Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + GPS configuration : Costum protocol + Baud rate : 38400 + + 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) + altitude : altitude * 100 (meters) (long value) + ground_speed : Speed (m/s) * 100 (long value) + ground_course : Course (degrees) * 100 (long value) + 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" + + +// Constructors //////////////////////////////////////////////////////////////// +AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) +{ +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void AP_GPS_IMU::init(void) +{ + // we expect the stream to already be open at the corret bitrate +} + +// 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. +void AP_GPS_IMU::update(void) +{ + byte data; + int numc = 0; + static byte message_num = 0; + + numc = _port->available(); + + if (numc > 0){ + for (int i=0;iread(); + + 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 { + _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... + } +} + +/**************************************************************** + * + ****************************************************************/ + +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. + + //Storing IMU roll + roll_sensor = *(int *)&buffer[0]; + + //Storing IMU pitch + pitch_sensor = *(int *)&buffer[2]; + + //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 pitch + pitch_sensor = *(int *)&buffer[2]; + + //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]; + + //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]; + + 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; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h new file mode 100644 index 0000000000..0e3f1d294f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h @@ -0,0 +1,44 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +#ifndef AP_GPS_IMU_h +#define AP_GPS_IMU_h + +#include +#define MAXPAYLOAD 32 + +class AP_GPS_IMU : public GPS +{ + public: + + // Methods + AP_GPS_IMU(Stream *s); + void init(); + void update(); + + // 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; + + 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 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp new file mode 100644 index 0000000000..db281390c0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -0,0 +1,146 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" +// + +#include "AP_GPS_MTK.h" +#include "WProgram.h" + +// Constructors //////////////////////////////////////////////////////////////// +AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +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); + + // set 4Hz update rate + _port->print(MTK_OUTPUT_4HZ); +} + +// Process bytes available from the stream +// +// The stream is assumed to contain only our custom message. If it +// contains other messages, and those messages contain the preamble bytes, +// it is possible for this code to become de-synchronised. Without +// buffering the entire message and re-processing it from the top, +// this is unavoidable. +// +// The lack of a standard header length field makes it impossible to skip +// unrecognised messages. +// +void AP_GPS_MTK::update(void) +{ + byte data; + int numc; + + numc = _port->available(); + for (int i = 0; i < numc; i++){ // Process bytes received + + // read the next byte + data = _port->read(); + +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_length = sizeof(diyd_mtk_msg); // prepare to receive our message + _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 == _payload_length) + _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; + } + _parse_gps(); // Parse the new GPS packet + } + } +} + +// Private Methods +void +AP_GPS_MTK::_parse_gps(void) +{ + 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; + + // XXX docs say this is UTC, but our clients expect msToW + time = _swapl(&_buffer.msg.utc_time); + _setTime(); + valid_read = true; + new_data = true; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h new file mode 100644 index 0000000000..a69dca60df --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h @@ -0,0 +1,87 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" +// +#ifndef AP_GPS_MTK_h +#define AP_GPS_MTK_h + +#include +#define MAXPAYLOAD 32 + +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n" + +#define MTK_OUTPUT_1HZ "$PMTK220,1000*1F\r\n" +#define MTK_OUTPUT_2HZ "$PMTK220,500*2B\r\n" +#define MTK_OUTPUT_4HZ "$PMTK220,250*29\r\n" +#define MTK_OTUPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_OUTPUT_10HZ "$PMTK220,100*2F\r\n" + +#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n" + +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define SBAS_OFF "$PMTK313,0*2F\r\n" + +#define WAAS_ON "$PSRF151,1*3F\r\n" +#define WAAS_OFF "$PSRF151,0*3E\r\n" + +class AP_GPS_MTK : public GPS { +public: + AP_GPS_MTK(Stream *s); + virtual void init(void); + virtual void update(void); + +private: +#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; + }; +#pragma pack(pop) + 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 + }; + + // Packet checksum accumulators + uint8_t _ck_a; + uint8_t _ck_b; + + // State machine state + uint8_t _step; + uint8_t _payload_length; + uint8_t _payload_counter; + + // Receive buffer + union { + diyd_mtk_msg msg; + uint8_t bytes[]; + } _buffer; + + // Buffer parse & GPS state update + void _parse_gps(); +}; + +#endif // AP_GPS_MTK_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp new file mode 100644 index 0000000000..4f9bf6f6c0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -0,0 +1,246 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +/* + GPS_NMEA.cpp - Generic NMEA GPS library for Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + Edits by HappyKillmore + This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + GPS configuration : NMEA protocol + Baud rate : 38400 + NMEA Sentences : + $GPGGA : Global Positioning System fix Data + $GPVTG : Ttack and Ground Speed + + Methods: + init() : GPS Initialization + update() : Call this funcion as often as you want to ensure you read the incomming gps data + + Properties: + latitude : latitude * 10000000 (long value) + longitude : longitude * 10000000 (long value) + altitude : altitude * 1000 (milimeters) (long value) + ground_speed : Speed (m / s) * 100 (long value) + ground_course : Course (degrees) * 100 (long value) + Type : 2 (This indicate that we are using the Generic NMEA library) + 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 FIX, 0: No fix (normal logic) + quality : 0 = No fix + 1 = Bad (Num sats < 5) + 2 = Poor + 3 = Medium + 4 = Good + + NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) +*/ +#include "AP_GPS_NMEA.h" +#include "WProgram.h" + +// Constructors //////////////////////////////////////////////////////////////// +AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void +AP_GPS_NMEA::init(void) +{ + //Type = 2; + // initialize serial port for binary protocol use + _port->print(NMEA_OUTPUT_SENTENCES); + _port->print(NMEA_OUTPUT_4HZ); + _port->print(SBAS_ON); + _port->print(DGPS_SBAS); + _port->print(DATUM_GOOGLE); +} + +// This code don´t wait for data, only proccess the data available on serial port +// We can call this function on the main loop (50Hz loop) +// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. +void +AP_GPS_NMEA::update(void) +{ + char c; + int numc; + int i; + + numc = _port->available(); + + if (numc > 0){ + for (i = 0; i < numc; i++){ + c = _port->read(); + if (c == '$'){ // NMEA Start + bufferidx = 0; + buffer[bufferidx++] = c; + GPS_checksum = 0; + GPS_checksum_calc = true; + continue; + } + if (c == '\r'){ // NMEA End + buffer[bufferidx++] = 0; + parse_nmea_gps(); + } else { + if (bufferidx < (GPS_BUFFERSIZE - 1)){ + if (c == '*') + GPS_checksum_calc = false; // Checksum calculation end + buffer[bufferidx++] = c; + if (GPS_checksum_calc){ + GPS_checksum ^= c; // XOR + } + } else { + bufferidx = 0; // Buffer overflow : restart + } + } + } + } +} + +/**************************************************************** + * + * * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/ +// Private Methods ////////////////////////////////////////////////////////////// +void +AP_GPS_NMEA::parse_nmea_gps(void) +{ + uint8_t NMEA_check; + long aux_deg; + long aux_min; + char *parseptr; + + if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA + if (buffer[bufferidx-4]=='*'){ // Check for the "*" character + NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters + if (GPS_checksum == NMEA_check){ // Checksum validation + //Serial.println("buffer"); + _setTime(); + valid_read = true; + new_data = true; // New GPS Data + parseptr = strchr(buffer, ',')+1; + //parseptr = strchr(parseptr, ',')+1; + time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss + parseptr = strchr(parseptr, ',')+1; + aux_deg = parsedecimal(parseptr, 2); // degrees + aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal + latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) + parseptr = strchr(parseptr, ',')+1; + if ( * parseptr == 'S') + latitude = -1 * latitude; // South latitudes are negative + parseptr = strchr(parseptr, ',')+1; + // W longitudes are Negative + aux_deg = parsedecimal(parseptr, 3); // degrees + aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal) + longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) + //longitude = -1*longitude; // This Assumes that we are in W longitudes... + parseptr = strchr(parseptr, ',')+1; + if ( * parseptr == 'W') + longitude = -1 * longitude; // West longitudes are negative + parseptr = strchr(parseptr, ',')+1; + fix = parsedecimal(parseptr, 1); + parseptr = strchr(parseptr, ',')+1; + num_sats = parsedecimal(parseptr, 2); + parseptr = strchr(parseptr, ',')+1; + HDOP = parsenumber(parseptr, 1); // HDOP * 10 + parseptr = strchr(parseptr, ',')+1; + altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters + if (fix < 1) + quality = 0; // No FIX + else if(num_sats < 5) + quality = 1; // Bad (Num sats < 5) + else if(HDOP > 30) + quality = 2; // Poor (HDOP > 30) + else if(HDOP > 25) + quality = 3; // Medium (HDOP > 25) + else + quality = 4; // Good (HDOP < 25) + } else { + _error("GPSERR: Checksum error!!\n"); + } + } + } else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG + //Serial.println(buffer); + if (buffer[bufferidx-4]=='*'){ // Check for the "*" character + NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters + if (GPS_checksum == NMEA_check){ // Checksum validation + _setTime(); + valid_read = true; + new_data = true; // New GPS Data + parseptr = strchr(buffer, ',')+1; + ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100 + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100) + //GPS_line = true; + } else { + _error("GPSERR: Checksum error!!\n"); + } + } + } else { + bufferidx = 0; + _error("GPSERR: Bad sentence!!\n"); + } +} + + + // Parse hexadecimal numbers +uint8_t +AP_GPS_NMEA::parseHex(char c) { + if (c < '0') + return (0); + if (c <= '9') + return (c - '0'); + if (c < 'A') + return (0); + if (c <= 'F') + return ((c - 'A')+10); +} + +// Decimal number parser +long +AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) { + long d = 0; + uint8_t i; + + i = num_car; + while ((str[0] != 0) && (i > 0)) { + if ((str[0] > '9') || (str[0] < '0')) + return d; + d *= 10; + d += str[0] - '0'; + str++; + i--; + } + return d; +} + +// Function to parse fixed point numbers (numdec=number of decimals) +long +AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) { + long d = 0; + uint8_t ndec = 0; + + while (str[0] != 0) { + if (str[0] == '.'){ + ndec = 1; + } else { + if ((str[0] > '9') || (str[0] < '0')) + return d; + d *= 10; + d += str[0] - '0'; + if (ndec > 0) + ndec++; + if (ndec > numdec) // we reach the number of decimals... + return d; + } + str++; + } + return d; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h new file mode 100644 index 0000000000..b7b46bee15 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h @@ -0,0 +1,60 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +#ifndef AP_GPS_NMEA_h +#define AP_GPS_NMEA_h + +#include +#define GPS_BUFFERSIZE 120 + +#define NMEA_OUTPUT_SENTENCES "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GPGGA and GPVTG + +#define NMEA_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n" +#define NMEA_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n" +#define NMEA_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n" +#define NMEA_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n" +#define NMEA_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n" + +#define NMEA_OUTPUT_1HZ "$PMTK220,1000*1F\r\n" +#define NMEA_OUTPUT_2HZ "$PMTK220,500*2B\r\n" +#define NMEA_OUTPUT_4HZ "$PMTK220,250*29\r\n" +#define NMEA_OTUPUT_5HZ "$PMTK220,200*2C\r\n" +#define NMEA_OUTPUT_10HZ "$PMTK220,100*2F\r\n" + +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define SBAS_OFF "$PMTK313,0*2F\r\n" + +#define WAAS_ON "$PSRF151,1*3F\r\n" +#define WAAS_OFF "$PSRF151,0*3E\r\n" + +#define DGPS_OFF "$PMTK301,0*2C\r\n" +#define DGPS_RTCM "$PMTK301,1*2D\r\n" +#define DGPS_SBAS "$PMTK301,2*2E\r\n" + +#define DATUM_GOOGLE "$PMTK330,0*2E\r\n" + +class AP_GPS_NMEA : public GPS +{ + public: + // Methods + AP_GPS_NMEA(Stream *s); + void init(); + void update(); + + // Properties + uint8_t quality; // GPS Signal quality + int HDOP; // HDOP + + private: + // Internal variables + uint8_t GPS_checksum; + uint8_t GPS_checksum_calc; + char buffer[GPS_BUFFERSIZE]; + int bufferidx; + + void parse_nmea_gps(void); + uint8_t parseHex(char c); + long parsedecimal(char *str,uint8_t num_car); + long parsenumber(char *str,uint8_t numdec); + +}; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h new file mode 100644 index 0000000000..23f242b59c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h @@ -0,0 +1,13 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#ifndef AP_GPS_None_h +#define AP_GPS_None_h + +#include + +class AP_GPS_None : public GPS +{ + virtual void init(void) {}; + virtual void update(void) {}; +}; +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp new file mode 100644 index 0000000000..f1fe5e9f5c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -0,0 +1,193 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// SiRF Binary GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith. +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// + +#include "AP_GPS_SIRF.h" +#include "WProgram.h" + +// Initialisation messages +// +// Turn off all messages except for 0x29. +// +// 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 +}; + +// Constructors //////////////////////////////////////////////////////////////// +AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void AP_GPS_SIRF::init(void) +{ + _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. + + // send SiRF binary setup messages + _port->write(init_messages, sizeof(init_messages)); +} + +// Process bytes available from the stream +// +// The stream is assumed to contain only messages we recognise. If it +// contains other messages, and those messages contain the preamble +// bytes, it is possible for this code to fail to synchronise to the +// stream immediately. Without buffering the entire message and +// re-processing it from the top, this is unavoidable. The parser +// attempts to avoid this when possible. +// +void AP_GPS_SIRF::update(void) +{ + byte data; + int numc; + + numc = _port->available(); + while(numc--) { + + // read the next byte + data = _port->read(); + + 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 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; + + // 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) { + _parse_gps(); // Parse the new GPS packet + } + } + } +} + +void +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; + _setTime(); + valid_read = 1; + break; + } + new_data = true; +} + +void +AP_GPS_SIRF::_accumulate(uint8_t val) +{ + _checksum = (_checksum + val) & 0x7fff; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h new file mode 100644 index 0000000000..ce3ff454be --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h @@ -0,0 +1,96 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// SiRF Binary GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith. +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +#ifndef AP_GPS_SIRF_h +#define AP_GPS_SIRF_h + +#include + +#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" + +class AP_GPS_SIRF : public GPS { +public: + AP_GPS_SIRF(Stream *s); + + virtual void init(); + virtual void update(); + +private: +#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; + }; +#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 + }; + + + // 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; + + void _parse_gps(void); + void _accumulate(uint8_t val); +}; + +#endif // AP_GPS_SIRF_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp new file mode 100644 index 0000000000..cd2541d118 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -0,0 +1,192 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// u-blox UBX GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// + +#include "AP_GPS_UBLOX.h" +#include "WProgram.h" + +// Constructors //////////////////////////////////////////////////////////////// + +AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// + +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. + + _port->flush(); +} + +// Process bytes available from the stream +// +// The stream is assumed to contain only messages we recognise. If it +// contains other messages, and those messages contain the preamble +// bytes, it is possible for this code to fail to synchronise to the +// stream immediately. Without buffering the entire message and +// re-processing it from the top, this is unavoidable. The parser +// attempts to avoid this when possible. +// +void AP_GPS_UBLOX::update(void) +{ + byte data; + int numc; + + numc = _port->available(); + for (int i = 0; i < numc; i++){ // Process bytes received + + // read the next byte + data = _port->read(); + + 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 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 { + _error("ignoring class 0x%x\n", (int)data); + _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: + _error("ignoring message 0x%x\n", (int)data); + _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) { + _error("payload %d expected %d\n", _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; + + // Checksum and message processing + // + case 7: + _step++; + if (_ck_a != data) { + _error("GPS_UBLOX: checksum error\n"); + _step = 0; + } + break; + case 8: + _step = 0; + if (_ck_b != data) { + _error("GPS_UBLOX: checksum error\n"); + break; + } + if (_gather) + _parse_gps(); // Parse the new GPS packet + } + } +} + +// Private Methods ///////////////////////////////////////////////////////////// + +void +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; + 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; + } + _setTime(); + valid_read = 1; + new_data = 1; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h new file mode 100644 index 0000000000..ce905a85e8 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -0,0 +1,124 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// u-blox UBX GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +#ifndef AP_GPS_UBLOX_h +#define AP_GPS_UBLOX_h + +#include + +#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26" + +class AP_GPS_UBLOX : public GPS +{ +public: + // Methods + AP_GPS_UBLOX(Stream *s = NULL); + void init(void); + void update(); + +private: + // u-blox UBX protocol essentials +#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; + }; +#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 + }; + + // 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; + + // 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 + void _parse_gps(); +}; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp new file mode 100644 index 0000000000..89bfefa1a0 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp @@ -0,0 +1,35 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "GPS.h" +#include "WProgram.h" +#include + +int +GPS::status(void) +{ + if (millis() - _lastTime >= 500){ + return 0; + } else if (fix == 0) { + return 1; + } else { + return 2; + } +} + +void +GPS::_setTime(void) +{ + _lastTime = millis(); +} + +void +GPS::_error(const char *fmt, ...) +{ + va_list ap; + + if (print_errors && stderr) { + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + } +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h new file mode 100644 index 0000000000..219254ba41 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h @@ -0,0 +1,150 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file GPS.h +/// @brief Interface definition for the various GPS drivers. + +#ifndef GPS_h +#define GPS_h + +#include +#include + +/// @class GPS +/// @brief Abstract base class for GPS receiver drivers. +class GPS +{ +public: + + /// 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. If NULL, assumed + /// to be set up at ::init time. Support for setting + /// the port in the ctor for backwards compatibility. + /// + GPS(Stream *s = NULL) : _port(s) {}; + + /// Startup initialisation. + /// + /// This routine performs any one-off initialisation required to set the + /// GPS up for use. + /// + /// Must be implemented by the GPS driver. + /// + /// @param s Stream connected to the GPS module. If NULL, assumed to + /// have been set up at constructor time. + /// + virtual void init(void) = 0; + + /// Update GPS state based on possible bytes received from the module. + /// + /// This routine must be called periodically to process incoming data. + /// + /// Must be implemented by the GPS driver. + /// + virtual void update(void) = 0; + + /// Query GPS status + /// + /// The 'valid message' status indicates that a recognised message was + /// received from the GPS within the last 500ms. + /// + /// @todo should probably return an enumeration here. + /// + /// @return 0 No GPS connected/detected + /// @return 1 Receiving valid GPS messages but no lock + /// @return 2 Receiving valid messages and locked + /// + int status(void); + + // Properties + long time; ///< GPS time in milliseconds from the start of the week + 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) + 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; + + // 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; ///< if true, errors will be printed to stderr + +protected: + Stream *_port; ///< stream port the GPS is attached to + unsigned long _lastTime; ///< Timer for lost connection + + /// reset the last-message-received timer used by ::status + /// + void _setTime(void); + + /// 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 + int _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 + /// + void _error(const char *msg, ...); + +}; + +inline long +GPS::_swapl(const void *bytes) +{ + 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]; + + 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; + + u.b[0] = b[1]; + u.b[1] = b[0]; + + return(u.v); +} + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde new file mode 100644 index 0000000000..8ffadd5680 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde @@ -0,0 +1,58 @@ +/* + Example of GPS 406 library. + Code by Jordi Munoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) +*/ + +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_406 gps(&Serial1); +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + tone(A6, 1000, 200); + + 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); +} +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 + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde new file mode 100644 index 0000000000..d60057e532 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -0,0 +1,59 @@ +// +// Test for AP_GPS_AUTO +// + +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +GPS *gps; +AP_GPS_Auto GPS(&Serial1, &gps); + +#define T6 1000000 +#define T7 10000000 + +void * operator new(size_t size) +{ + return(calloc(size, 1)); +} + +void setup() +{ + Serial.begin(38400); + Serial1.begin(38400); + + Serial.println("GPS AUTO library test"); + gps = &GPS; + gps->init(); + 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; + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde new file mode 100644 index 0000000000..bda3b85229 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -0,0 +1,56 @@ +/* + Example of GPS MTK library. + Code by Jordi Munoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) +*/ + +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_MTK gps(&Serial1); +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; + + 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 + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde new file mode 100644 index 0000000000..fd071541a4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -0,0 +1,57 @@ +/* + Example of GPS NMEA library. + Code by Jordi Munoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) +*/ + +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_NMEA gps(&Serial1); +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; + + Serial.println("GPS NMEA 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 read the data + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde new file mode 100644 index 0000000000..43eb239495 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -0,0 +1,56 @@ +/* + Example of GPS UBlox library. + Code by Jordi Munoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) +*/ + +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_UBLOX gps(&Serial1); +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; + + 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 + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h new file mode 100644 index 0000000000..b94ed1c696 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h @@ -0,0 +1,7 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Assorted useful math operations for ArduPilot(Mega) + +#include "vector2.h" +#include "vector3.h" +#include "matrix3.h" diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt new file mode 100644 index 0000000000..a5b7938f47 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt @@ -0,0 +1,29 @@ +Vector2 KEYWORD1 +Vector2i KEYWORD1 +Vector2ui KEYWORD1 +Vector2l KEYWORD1 +Vector2ul KEYWORD1 +Vector2f KEYWORD1 +Vector3 KEYWORD1 +Vector3i KEYWORD1 +Vector3ui KEYWORD1 +Vector3l KEYWORD1 +Vector3ul KEYWORD1 +Vector3f KEYWORD1 +Matrix3 KEYWORD1 +Matrix3i KEYWORD1 +Matrix3ui KEYWORD1 +Matrix3l KEYWORD1 +Matrix3ul KEYWORD1 +Matrix3f KEYWORD1 +length_squared KEYWORD2 +length KEYWORD2 +normalize KEYWORD2 +normalized KEYWORD2 +reflect KEYWORD2 +project KEYWORD2 +projected KEYWORD2 +angle KEYWORD2 +angle_normalized KEYWORD2 +rotate KEYWORD2 +rotated KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h new file mode 100644 index 0000000000..51fd144c75 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h @@ -0,0 +1,134 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Copyright 2010 Michael Smith, all rights reserved. + +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// Inspired by: +/**************************************** + * 3D Vector Classes + * By Bill Perone (billperone@yahoo.com) + */ + +// +// 3x3 matrix implementation. +// +// Note that the matrix is organised in row-normal form (the same as +// applies to array indexing). +// +// In addition to the template, this header defines the following types: +// +// Matrix3i 3x3 matrix of signed integers +// Matrix3ui 3x3 matrix of unsigned integers +// Matrix3l 3x3 matrix of signed longs +// Matrix3ul 3x3 matrix of unsigned longs +// Matrix3f 3x3 matrix of signed floats +// + +#ifndef MATRIX3_H +#define MATRIX3_H + +#include "vector3.h" + +// 3x3 matrix with elements of type T +template +class Matrix3 { +public: + + // Vectors comprising the rows of the matrix + Vector3 a, b, c; + + // trivial ctor + Matrix3() {} + + // setting ctor + Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0): a(a0), b(b0), c(c0) {} + + // setting ctor + Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {} + + // function call operator + void operator () (const Vector3 a0, const Vector3 b0, const Vector3 c0) + { a = a0; b = b0; c = c0; } + + // test for equality + bool operator == (const Matrix3 &m) + { return (a==m.a && b==m.b && c==m.c); } + + // test for inequality + bool operator != (const Matrix3 &m) + { return (a!=m.a || b!=m.b || c!=m.c); } + + // negation + Matrix3 operator - (void) const + { return Matrix3(-a,-b,-c); } + + // addition + Matrix3 operator + (const Matrix3 &m) const + { return Matrix3(a+m.a, b+m.b, c+m.c); } + Matrix3 &operator += (const Matrix3 &m) + { return *this = *this + m; } + + // subtraction + Matrix3 operator - (const Matrix3 &m) const + { return Matrix3(a-m.a, b-m.b, c-m.c); } + Matrix3 &operator -= (const Matrix3 &m) + { return *this = *this - m; } + + // uniform scaling + Matrix3 operator * (const T num) const + { return Matrix3(a*num, b*num, c*num); } + Matrix3 &operator *= (const T num) + { return *this = *this * num; } + Matrix3 operator / (const T num) const + { return Matrix3(a/num, b/num, c/num); } + Matrix3 &operator /= (const T num) + { return *this = *this / num; } + + // multiplication by a vector + Vector3 operator *(const Vector3 &v) const + { + return Vector3(a.x * v.x + a.y * v.y + a.z * v.z, + b.x * v.x + b.y * v.y + b.z * v.z, + c.x * v.x + c.y * v.y + c.z * v.z); + } + + // multiplication by another Matrix3 + Matrix3 operator *(const Matrix3 &m) const + { + Matrix3 temp (Vector3(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x, + a.x * m.a.y + a.y * m.b.y + a.z * m.c.y, + a.x * m.a.z + a.y * m.b.z + a.z * m.c.z), + Vector3(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x, + b.x * m.a.y + b.y * m.b.y + b.z * m.c.y, + b.x * m.a.z + b.y * m.b.z + b.z * m.c.z), + Vector3(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x, + c.x * m.a.y + c.y * m.b.y + c.z * m.c.y, + c.x * m.a.z + c.y * m.b.z + c.z * m.c.z)); + return temp; + } + Matrix3 &operator *=(const Matrix3 &m) + { return *this = *this * m; } + + // transpose the matrix + Matrix3 transposed(void) + { + return Matrix3(Vector3(a.x, b.x, c.x), + Vector3(a.y, b.y, c.y), + Vector3(a.z, b.z, c.z)); + } + Matrix3 transpose(void) + { return *this = transposed(); } + +}; + +typedef Matrix3 Matrix3i; +typedef Matrix3 Matrix3ui; +typedef Matrix3 Matrix3l; +typedef Matrix3 Matrix3ul; +typedef Matrix3 Matrix3f; + +#endif // MATRIX3_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h new file mode 100644 index 0000000000..bbe380aeb6 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h @@ -0,0 +1,158 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Copyright 2010 Michael Smith, all rights reserved. + +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// Derived closely from: +/**************************************** + * 2D Vector Classes + * By Bill Perone (billperone@yahoo.com) + * Original: 9-16-2002 + * Revised: 19-11-2003 + * 18-12-2003 + * 06-06-2004 + * + * © 2003, This code is provided "as is" and you can use it freely as long as + * credit is given to Bill Perone in the application it is used in + ****************************************/ + +#ifndef VECTOR2_H +#define VECTOR2_H + +#include + +template +struct Vector2 +{ + T x, y; + + + // trivial ctor + Vector2() {} + + // setting ctor + Vector2(const T x0, const T y0): x(x0), y(y0) {} + + // function call operator + void operator ()(const T x0, const T y0) + { x= x0; y= y0; } + + // test for equality + bool operator==(const Vector2 &v) + { return (x==v.x && y==v.y); } + + // test for inequality + bool operator!=(const Vector2 &v) + { return (x!=v.x || y!=v.y); } + + // negation + Vector2 operator -(void) const + { return Vector2(-x, -y); } + + // addition + Vector2 operator +(const Vector2 &v) const + { return Vector2(x+v.x, y+v.y); } + + // subtraction + Vector2 operator -(const Vector2 &v) const + { return Vector2(x-v.x, y-v.y); } + + // uniform scaling + Vector2 operator *(const T num) const + { + Vector2 temp(*this); + return temp*=num; + } + + // uniform scaling + Vector2 operator /(const T num) const + { + Vector2 temp(*this); + return temp/=num; + } + + // addition + Vector2 &operator +=(const Vector2 &v) + { + x+=v.x; y+=v.y; + return *this; + } + + // subtraction + Vector2 &operator -=(const Vector2 &v) + { + x-=v.x; y-=v.y; + return *this; + } + + // uniform scaling + Vector2 &operator *=(const T num) + { + x*=num; y*=num; + return *this; + } + + // uniform scaling + Vector2 &operator /=(const T num) + { + x/=num; y/=num; + return *this; + } + + // dot product + T operator *(const Vector2 &v) const + { return x*v.x + y*v.y; } + + // gets the length of this vector squared + T length_squared() const + { return (T)(*this * *this); } + + // gets the length of this vector + T length() const + { return (T)sqrt(*this * *this); } + + // normalizes this vector + void normalize() + { *this/=length(); } + + // returns the normalized vector + Vector2 normalized() const + { return *this/length(); } + + // reflects this vector about n + void reflect(const Vector2 &n) + { + Vector2 orig(*this); + project(n); + *this= *this*2 - orig; + } + + // projects this vector onto v + void project(const Vector2 &v) + { *this= v * (*this * v)/(v*v); } + + // returns this vector projected onto v + Vector2 projected(const Vector2 &v) + { return v * (*this * v)/(v*v); } + + // computes the angle between 2 arbitrary vectors + static inline T angle(const Vector2 &v1, const Vector2 &v2) + { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } + + // computes the angle between 2 normalized arbitrary vectors + static inline T angle_normalized(const Vector2 &v1, const Vector2 &v2) + { return (T)acosf(v1*v2); } + +}; + +typedef Vector2 Vector2i; +typedef Vector2 Vector2ui; +typedef Vector2 Vector2l; +typedef Vector2 Vector2ul; +typedef Vector2 Vector2f; + +#endif // VECTOR2_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h new file mode 100644 index 0000000000..7c23a5fc63 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h @@ -0,0 +1,183 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// Copyright 2010 Michael Smith, all rights reserved. + +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// Derived closely from: +/**************************************** + * 3D Vector Classes + * By Bill Perone (billperone@yahoo.com) + * Original: 9-16-2002 + * Revised: 19-11-2003 + * 11-12-2003 + * 18-12-2003 + * 06-06-2004 + * + * © 2003, This code is provided "as is" and you can use it freely as long as + * credit is given to Bill Perone in the application it is used in + * + * Notes: + * if a*b = 0 then a & b are orthogonal + * a%b = -b%a + * a*(b%c) = (a%b)*c + * a%b = a(cast to matrix)*b + * (a%b).length() = area of parallelogram formed by a & b + * (a%b).length() = a.length()*b.length() * sin(angle between a & b) + * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0 + * a * (b%c) = volume of parallelpiped formed by a, b, c + * vector triple product: a%(b%c) = b*(a*c) - c*(a*b) + * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a) + * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c) + * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0 + * vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0 + * or if the matrix (A) * b = 0 + * + ****************************************/ + +#ifndef VECTOR3_H +#define VECTOR3_H + +#include + +template +class Vector3 +{ +public: + T x, y, z; + + // trivial ctor + Vector3() {} + + // setting ctor + Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {} + + // function call operator + void operator ()(const T x0, const T y0, const T z0) + { x= x0; y= y0; z= z0; } + + // test for equality + bool operator==(const Vector3 &v) + { return (x==v.x && y==v.y && z==v.z); } + + // test for inequality + bool operator!=(const Vector3 &v) + { return (x!=v.x || y!=v.y || z!=v.z); } + + // negation + Vector3 operator -(void) const + { return Vector3(-x,-y,-z); } + + // addition + Vector3 operator +(const Vector3 &v) const + { return Vector3(x+v.x, y+v.y, z+v.z); } + + // subtraction + Vector3 operator -(const Vector3 &v) const + { return Vector3(x-v.x, y-v.y, z-v.z); } + + // uniform scaling + Vector3 operator *(const T num) const + { + Vector3 temp(*this); + return temp*=num; + } + + // uniform scaling + Vector3 operator /(const T num) const + { + Vector3 temp(*this); + return temp/=num; + } + + // addition + Vector3 &operator +=(const Vector3 &v) + { + x+=v.x; y+=v.y; z+=v.z; + return *this; + } + + // subtraction + Vector3 &operator -=(const Vector3 &v) + { + x-=v.x; y-=v.y; z-=v.z; + return *this; + } + + // uniform scaling + Vector3 &operator *=(const T num) + { + x*=num; y*=num; z*=num; + return *this; + } + + // uniform scaling + Vector3 &operator /=(const T num) + { + x/=num; y/=num; z/=num; + return *this; + } + + // dot product + T operator *(const Vector3 &v) const + { return x*v.x + y*v.y + z*v.z; } + + // cross product + Vector3 operator %(const Vector3 &v) const + { + Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); + return temp; + } + + // gets the length of this vector squared + T length_squared() const + { return (T)(*this * *this); } + + // gets the length of this vector + float length() const + { return (T)sqrt(*this * *this); } + + // normalizes this vector + void normalize() + { *this/=length(); } + + // returns the normalized version of this vector + Vector3 normalized() const + { return *this/length(); } + + // reflects this vector about n + void reflect(const Vector3 &n) + { + Vector3 orig(*this); + project(n); + *this= *this*2 - orig; + } + + // projects this vector onto v + void project(const Vector3 &v) + { *this= v * (*this * v)/(v*v); } + + // returns this vector projected onto v + Vector3 projected(const Vector3 &v) + { return v * (*this * v)/(v*v); } + + // computes the angle between 2 arbitrary vectors + static inline T angle(const Vector3 &v1, const Vector3 &v2) + { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } + + // computes the angle between 2 arbitrary normalized vectors + static inline T angle_normalized(const Vector3 &v1, const Vector3 &v2) + { return (T)acosf(v1*v2); } + +}; + +typedef Vector3 Vector3i; +typedef Vector3 Vector3ui; +typedef Vector3 Vector3l; +typedef Vector3 Vector3ul; +typedef Vector3 Vector3f; + +#endif // VECTOR3_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp new file mode 100644 index 0000000000..9802257a89 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp @@ -0,0 +1,249 @@ +#include "Navigation.h" + +Navigation::Navigation(GPS *withGPS, Waypoints *withWP) : + _gps(withGPS), + _wp(withWP), + _hold_course(-1) +{ +} + +void +Navigation::update_gps() +{ + location.alt = _gps->altitude; + location.lng = _gps->longitude; + location.lat = _gps->latitude; + + // target_bearing is where we should be heading + bearing = get_bearing(&location, &next_wp); + + // waypoint distance from plane + distance = get_distance(&location, &next_wp); + + calc_bearing_error(); + calc_altitude_error(); + altitude_above_home = location.alt - home.alt; + + // check if we have missed the WP + _loiter_delta = (bearing - _old_bearing) / 100; + + // reset the old value + _old_bearing = bearing; + + // wrap values + if (_loiter_delta > 170) _loiter_delta -= 360; + if (_loiter_delta < -170) _loiter_delta += 360; + loiter_sum += abs(_loiter_delta); + + if (distance <= 0){ + distance = -1; + Serial.print("MSG wp error "); + Serial.println(distance, DEC); + } +} + +void +Navigation::load_first_wp(void) +{ + set_next_wp(_wp->get_waypoint_with_index(1)); +} + +void +Navigation::load_home(void) +{ + home = _wp->get_waypoint_with_index(0); +} + +void +Navigation::return_to_home_with_alt(uint32_t alt) +{ + Waypoints::WP loc = _wp->get_waypoint_with_index(0); + loc.alt += alt; + set_next_wp(loc); +} + +void +Navigation::reload_wp(void) +{ + set_next_wp(_wp->get_current_waypoint()); +} + +void +Navigation::load_wp_index(uint8_t i) +{ + _wp->set_index(i); + set_next_wp(_wp->get_current_waypoint()); +} + +void +Navigation::hold_location() +{ + // set_next_wp() XXX needs to be implemented +} + +void +Navigation::set_home(Waypoints::WP loc) +{ + _wp->set_waypoint_with_index(loc, 0); + home = loc; + //location = home; +} + +void +Navigation::set_next_wp(Waypoints::WP loc) +{ + prev_wp = next_wp; + next_wp = loc; + + if(_scaleLongDown == 0) + calc_long_scaling(loc.lat); + + total_distance = get_distance(&location, &next_wp); + distance = total_distance; + + bearing = get_bearing(&location, &next_wp); + _old_bearing = bearing; + + // clear loitering code + _loiter_delta = 0; + loiter_sum = 0; + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); +} + +void +Navigation::calc_long_scaling(int32_t lat) +{ + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (abs(lat) / T7) * 0.0174532925; + _scaleLongDown = cos(rads); + _scaleLongUp = 1.0f / cos(rads); +} + +void +Navigation::set_hold_course(int16_t b) +{ + if(b) + _hold_course = bearing; + else + _hold_course = -1; +} + +int16_t +Navigation::get_hold_course(void) +{ + return _hold_course; +} + +void +Navigation::calc_distance_error() +{ + //distance_estimate += (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01)); + //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance); + //distance = max(distance_estimate,10); +} + +void +Navigation::calc_bearing_error(void) +{ + if(_hold_course == -1){ + bearing_error = wrap_180(bearing - _gps->ground_course); + }else{ + bearing_error = _hold_course; + } +} + +int32_t +Navigation::wrap_180(int32_t error) +{ + if (error > 18000) error -= 36000; + if (error < -18000) error += 36000; + return error; +} + +int32_t +Navigation::wrap_360(int32_t angle) +{ + if (angle > 36000) angle -= 36000; + if (angle < 0) angle += 36000; + return angle; +} + +void +Navigation::set_bearing_error(int32_t error) +{ + bearing_error = wrap_180(error); +} + + +/***************************************** + * Altitude error with Airspeed correction + *****************************************/ +void +Navigation::calc_altitude_error(void) +{ + // limit climb rates + _target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20)); + if(prev_wp.alt > next_wp.alt){ + _target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt); + }else{ + _target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt); + } + altitude_error = _target_altitude - location.alt; +} + +void +Navigation::set_loiter_vector(int16_t v) +{ + // _vector = constrain(v, -18000, 18000); XXX needs to be implemented +} + +void +Navigation::update_crosstrack(void) +{ + // Crosstrack Error + // ---------------- + if (abs(bearing - _crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following + _crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance; // Meters we are off track line + bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE); + } +} + +void +Navigation::reset_crosstrack(void) +{ + _crosstrack_bearing = get_bearing(&location, &next_wp); // Used for track following +} + +long +Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2) +{ + if(loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + if(_scaleLongDown == 0) + calc_long_scaling(loc2->lat); + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown; + return sqrt(sq(dlat) + sq(dlong)) * .01113195; +} + +long +Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2) +{ + if(loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + if(_scaleLongDown == 0) + calc_long_scaling(loc2->lat); + long off_x = loc2->lng - loc1->lng; + long off_y = (loc2->lat - loc1->lat) * _scaleLongUp; + long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) + bearing += 36000; + return bearing; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h new file mode 100644 index 0000000000..4fce19da63 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h @@ -0,0 +1,76 @@ + +#ifndef AP_NAVIGATION_h +#define AP_NAVIGATION_h + +#define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter) +#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100 +#include // ArduPilot GPS Library +#include "Waypoints.h" // ArduPilot Waypoints Library +#include "WProgram.h" + +#define T7 10000000 + +class Navigation { +public: + Navigation(GPS *withGPS, Waypoints *withWP); + + void update_gps(void); // called 50 Hz + void set_home(Waypoints::WP loc); + void set_next_wp(Waypoints::WP loc); + void load_first_wp(void); + void load_wp_with_index(uint8_t i); + void load_home(void); + void return_to_home_with_alt(uint32_t alt); + + void reload_wp(void); + void load_wp_index(uint8_t i); + void hold_location(); + void set_wp(Waypoints::WP loc); + + void set_hold_course(int16_t b); // -1 deisables, 0-36000 enables + int16_t get_hold_course(void); + + int32_t get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2); + int32_t get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2); + void set_bearing_error(int32_t error); + + void set_loiter_vector(int16_t v); + void update_crosstrack(void); + + int32_t wrap_180(int32_t error); // utility + int32_t wrap_360(int32_t angle); // utility + + int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate + int32_t distance; // meters : distance plane to next waypoint + int32_t altitude_above_home; // meters * 100 relative to home position + int32_t total_distance; // meters : distance between waypoints + int32_t bearing_error; // deg * 100 : 18000 to -18000 + int32_t altitude_error; // deg * 100 : 18000 to -18000 + + int16_t loiter_sum; + Waypoints::WP home, location, prev_wp, next_wp; + +private: + void calc_int32_t_scaling(int32_t lat); + void calc_bearing_error(void); + void calc_altitude_error(void); + void calc_distance_error(void); + void calc_long_scaling(int32_t lat); + void reset_crosstrack(void); + + int16_t _old_bearing; // used to track delta on the bearing + GPS *_gps; + Waypoints *_wp; + int32_t _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target + float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline + int16_t _hold_course; // deg * 100 dir of plane + int32_t _target_altitude; // used for + int32_t _offset_altitude; // used for + float _altitude_estimate; + float _scaleLongUp; // used to reverse int32_ttitude scaling + float _scaleLongDown; // used to reverse int32_ttitude scaling + int16_t _loiter_delta; +}; + + +#endif // AP_NAVIGATION_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde new file mode 100644 index 0000000000..19b0b54483 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde @@ -0,0 +1,152 @@ +/* + +This test assumes you are at the LOWl demo Airport + +*/ + +#include "Waypoints.h" +#include "Navigation.h" +#include "AP_GPS_IMU.h" +#include "AP_RC.h" + + +AP_GPS_IMU gps; +Navigation nav((GPS *) &gps); +AP_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +#define CH3_MIN 1100 +#define CH3_MAX 1900 + +#define REVERSE_RUDDER 1 +#define REVERSE_ROLL 1 +#define REVERSE_PITCH 1 + +int8_t did_init_home; +int16_t servo_out[4]; +int16_t radio_trim[4] = {1500,1500,1100,1500}; +int16_t radio_in[4]; + +void setup() +{ + Serial.begin(38400); + Serial.println("Navigation test"); + Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012}; + nav.set_next_wp(location_B); + rc.init(radio_trim); +} + +void loop() +{ + delay(20); + gps.update(); + rc.read_pwm(); + for(int y=0; y<4; y++) { + //rc.set_ch_pwm(y, rc.input[y]); // send to Servos + radio_in[y] = rc.input[y]; + } + + servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500; + servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500; + servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500; + servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100; + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); + + output_Xplane(); + if(gps.new_data /* && gps.fix */ ){ + if(did_init_home == 0){ + did_init_home = 1; + Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude}; + nav.set_home(wp); + Serial.println("MSG init home"); + }else{ + nav.update_gps(); + } + //print_loc(); + } +} + +void print_loc() +{ + Serial.print("MSG GPS: "); + Serial.print(nav.location.lat, DEC); + Serial.print(", "); + Serial.print(nav.location.lng, DEC); + Serial.print(", "); + Serial.print(nav.location.alt, DEC); + Serial.print("\tDistance = "); + Serial.print(nav.distance, DEC); + Serial.print(" Bearing = "); + Serial.print(nav.bearing/100, DEC); + Serial.print(" Bearing err = "); + Serial.print(nav.bearing_error/100, DEC); + Serial.print(" alt err = "); + Serial.print(nav.altitude_error/100, DEC); + Serial.print(" Alt above home = "); + Serial.println(nav.altitude_above_home/100, DEC); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.input[CH_ROLL],DEC); + Serial.print("\tch2: "); + Serial.print(rc.input[CH_PITCH],DEC); + Serial.print("\tch3 :"); + Serial.print(rc.input[CH_THROTTLE],DEC); + Serial.print("\tch4 :"); + Serial.println(rc.input[CH_RUDDER],DEC); +} + + + + +byte buf_len = 0; +byte out_buffer[32]; + +void output_Xplane(void) +{ + // output real-time sensor data + Serial.print("AAA"); // Message preamble + output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1 + output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3 + output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5 + output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7 + output_int((int)nav.distance); // 4 bytes 8,9 + output_int((int)nav.bearing_error); // 5 bytes 10,11 + output_int((int)nav.altitude_error); // 6 bytes 12,13 + output_int(0); // 7 bytes 14,15 + output_byte(1); // 8 bytes 16 + output_byte(1); // 9 bytes 17 + + // print out the buffer and checksum + // --------------------------------- + print_buffer(); +} + +void output_int(int value) +{ + //buf_len += 2; + out_buffer[buf_len++] = value & 0xff; + out_buffer[buf_len++] = (value >> 8) & 0xff; +} +void output_byte(byte value) +{ + out_buffer[buf_len++] = value; +} + +void print_buffer() +{ + byte ck_a = 0; + byte ck_b = 0; + for (byte i = 0;i < buf_len; i++){ + Serial.print (out_buffer[i]); + } + Serial.print('\r'); + Serial.print('\n'); + buf_len = 0; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde new file mode 100644 index 0000000000..5c0f21739c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde @@ -0,0 +1,28 @@ +#include +#include +#include + + +AP_GPS_IMU gps; +Navigation nav((GPS *) & gps); + +void setup() +{ + Serial.begin(38400); + Serial.println("Navigation test"); + + Waypoints::WP location_A = {0, 0, 38.579633 * T7, -122.566265 * T7, 10000}; + Waypoints::WP location_B = {0, 0, 38.578743 * T7, -122.572782 * T7, 5000}; + + long distance = nav.get_distance(&location_A, &location_B); + long bearing = nav.get_bearing(&location_A, &location_B); + + Serial.print("\tDistance = "); + Serial.print(distance, DEC); + Serial.print(" Bearing = "); + Serial.println(bearing, DEC); +} + +void loop() +{ +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp new file mode 100644 index 0000000000..90655df9f8 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp @@ -0,0 +1,472 @@ +#include "DCM.h" + +// XXX HACKS +APM_ADC adc; + +// XXX END HACKS + + +#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer +#define ADC_CONSTRAINT 900 + +#define Kp_ROLLPITCH 0.0014 //0.015 // Pitch&Roll Proportional Gain +#define Ki_ROLLPITCH 0.0000003 // 0.00001 Pitch&Roll Integrator Gain +#define Kp_YAW 1.2 // 1.2 Yaw Porportional Gain +#define Ki_YAW 0.00005 // 0.00005 Yaw Integrator Gain + +// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ +const uint8_t AP_DCM::_sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware +const int AP_DCM::_sensor_signs[] = {1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1} !!!! These are probably not right + +// Temp compensation curve constants +// These must be produced by measuring data and curve fitting +// [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] +const float AP_DCM::_gyro_temp_curve[3][3] = { + {1665,0,0}, + {1665,0,0}, + {1665,0,0} +}; // values may migrate to a Config file + + + +// Constructors //////////////////////////////////////////////////////////////// +AP_DCM::AP_DCM(APM_Compass *withCompass) : + _compass(withCompass), + _dcm_matrix(1, 0, 0, + 0, 1, 0, + 0, 0, 1), + _G_Dt(0.02), + _course_over_ground_x(0), + _course_over_ground_y(1) +{ +} + +void +AP_DCM::update_DCM(void) +{ + read_adc_raw(); // Get current values for IMU sensors + matrix_update(); // Integrate the DCM matrix + normalize(); // Normalize the DCM matrix + drift_correction(); // Perform drift correction + euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation +} + + +// Read the 6 ADC channels needed for the IMU +// ------------------------------------------ +void +AP_DCM::read_adc_raw(void) +{ + int tc_temp = adc.Ch(_gyro_temp_ch); + for (int i = 0; i < 6; i++) { + _adc_in[i] = adc.Ch(_sensors[i]); + if (i < 3) { // XXX magic numbers! + _adc_in[i] -= _gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias + } else { + _adc_in[i] -= 2025; // Subtract typical accel bias + } + } +} + +// Returns the temperature compensated raw gyro value +//--------------------------------------------------- +float +AP_DCM::_gyro_temp_comp(int i, int temp) const +{ + // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 + //------------------------------------------------------------------------ + return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp; +} + +// Returns an analog value with the offset removed +// ----------------- +float +AP_DCM::read_adc(int select) +{ + float temp; + if (_sensor_signs[select] < 0) + temp = (_adc_offset[select] - _adc_in[select]); + else + temp = (_adc_in[select] - _adc_offset[select]); + + if (abs(temp) > ADC_CONSTRAINT) + adc_constraints++; // We keep track of the number of times we constrain the ADC output for performance reporting + +/* +// For checking the pitch/roll drift correction gain time constants +switch (select) { + case 3: + return 0; + break; + case 4: + return 0; + break; + case 5: + return 400; + break; +} +*/ + + +//End of drift correction gain test code + + return constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values +} + +/**************************************************/ +void +AP_DCM::normalize(void) +{ + float error = 0; + DCM_Vector temporary[3]; + + uint8_t problem = 0; + + error = -_dcm_matrix(0).dot_product(_dcm_matrix(1)) * 0.5; // eq.19 + + temporary[0] = _dcm_matrix(1) * error + _dcm_matrix(0); // eq.19 + temporary[1] = _dcm_matrix(0) * error + _dcm_matrix(1); // eq.19 + + temporary[2] = temporary[0] ^ temporary[1]; // c= a x b // eq.20 + + _dcm_matrix(0) = _renorm(temporary[0], problem); + _dcm_matrix(1) = _renorm(temporary[1], problem); + _dcm_matrix(2) = _renorm(temporary[2], problem); + + if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! + _dcm_matrix(0, 0)= 1.0f; + _dcm_matrix(0, 1)= 0.0f; + _dcm_matrix(0, 2)= 0.0f; + _dcm_matrix(1, 0)= 0.0f; + _dcm_matrix(1, 1)= 1.0f; + _dcm_matrix(1, 2)= 0.0f; + _dcm_matrix(2, 0)= 0.0f; + _dcm_matrix(2, 1)= 0.0f; + _dcm_matrix(2, 2)= 1.0f; + } +} + +DCM_Vector +AP_DCM::_renorm(DCM_Vector const &a, uint8_t &problem) +{ + float renorm; + + renorm = a.dot_product(a); + + if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion + renorm = 0.5 * (3 - renorm); // eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm = 1.0 / sqrt(renorm); + renorm_sqrt_count++; + } else { + problem = 1; + renorm_blowup_count++; + } + + return(a * renorm); +} + +/**************************************************/ +void +AP_DCM::drift_correction(void) +{ + //Compensation the Roll, Pitch and Yaw drift. + float mag_heading_x; + float mag_heading_y; + float error_course = 0; + static float scaled_omega_P[3]; + static float scaled_omega_I[3]; + float accel_magnitude; + float accel_weight; + float integrator_magnitude; + + //*****Roll and Pitch*************** + + // Calculate the magnitude of the accelerometer vector + accel_magnitude = _accel_vector.magnitude() / GRAVITY; // Scale to gravity. + + // Dynamic weighting of accelerometer info (reliability filter) + // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) + accel_weight = constrain(1 - 2 * abs(1 - accel_magnitude), 0, 1); // + + // We monitor the amount that the accelerometer based drift correction is deweighted for performanc reporting + imu_health = imu_health + 0.02 * (accel_weight-.5); + imu_health = constrain(imu_health, 0, 1); + + // adjust the ground of reference + _error_roll_pitch = _accel_vector ^ _dcm_matrix(2); + + // error_roll_pitch are in Accel ADC units + // Limit max error_roll_pitch to limit max omega_P and omega_I + _error_roll_pitch(0) = constrain(_error_roll_pitch(0), -50, 50); + _error_roll_pitch(1) = constrain(_error_roll_pitch(1), -50, 50); + _error_roll_pitch(2) = constrain(_error_roll_pitch(2), -50, 50); + + _omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight); + _omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight); + + //*****YAW*************** + + if (_compass) { + // We make the gyro YAW drift correction based on compass magnetic heading + error_course= (_dcm_matrix(0, 0) * _compass->Heading_Y) - (_dcm_matrix(1, 0) * _compass->Heading_X); // Calculating YAW error + } else { + // Use GPS Ground course to correct yaw gyro drift + if (ground_speed >= SPEEDFILT) { + // Optimization: We have precalculated course_over_ground_x and course_over_ground_y (Course over Ground X and Y) from GPS info + error_course = (_dcm_matrix(0, 0) * _course_over_ground_y) - (_dcm_matrix(1, 0) * _course_over_ground_x); // Calculating YAW error + } + } + _error_yaw = _dcm_matrix(2) * error_course; // Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + + _omega_P += _error_yaw * Kp_YAW; // Adding Proportional. + _omega_I += _error_yaw * Ki_YAW; // adding integrator to the omega_I + + // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros + integrator_magnitude = sqrt(_omega_I.dot_product(_omega_I)); + if (integrator_magnitude > radians(300)) { + _omega_I *= (0.5f * radians(300) / integrator_magnitude); + } + +} + +/**************************************************/ +void +AP_DCM::_accel_adjust(void) +{ + _accel_vector(1) += accel_scale((ground_speed / 100) * _omega(2)); // Centrifugal force on Acc_y = GPS_speed * GyroZ + _accel_vector(2) -= accel_scale((ground_speed / 100) * _omega(1)); // Centrifugal force on Acc_z = GPS_speed * GyroY +} + + +/**************************************************/ +void +AP_DCM::matrix_update(void) +{ + DCM_Matrix update_matrix; + + _gyro_vector(0) = gyro_scaled_X(read_adc(0)); // gyro x roll + _gyro_vector(1) = gyro_scaled_Y(read_adc(1)); // gyro y pitch + _gyro_vector(2) = gyro_scaled_Z(read_adc(2)); // gyro Z yaw + + //Record when you saturate any of the gyros. + if((abs(_gyro_vector(0)) >= radians(300)) || + (abs(_gyro_vector(1)) >= radians(300)) || + (abs(_gyro_vector(2)) >= radians(300))) + gyro_sat_count++; + +/* +Serial.print (__adc_in[0]); +Serial.print (" "); +Serial.print (_adc_offset[0]); +Serial.print (" "); +Serial.print (_gyro_vector(0)); +Serial.print (" "); +Serial.print (__adc_in[1]); +Serial.print (" "); +Serial.print (_adc_offset[1]); +Serial.print (" "); +Serial.print (_gyro_vector(1)); +Serial.print (" "); +Serial.print (__adc_in[2]); +Serial.print (" "); +Serial.print (_adc_offset[2]); +Serial.print (" "); +Serial.println (_gyro_vector(2)); +*/ + +// _accel_vector(0) = read_adc(3); // acc x +// _accel_vector(1) = read_adc(4); // acc y +// _accel_vector(2) = read_adc(5); // acc z + // Low pass filter on accelerometer data (to filter vibrations) + _accel_vector(0) = _accel_vector(0) * 0.6 + (float)read_adc(3) * 0.4; // acc x + _accel_vector(1) = _accel_vector(1) * 0.6 + (float)read_adc(4) * 0.4; // acc y + _accel_vector(2) = _accel_vector(2) * 0.6 + (float)read_adc(5) * 0.4; // acc z + + _omega = _gyro_vector + _omega_I; // adding proportional term + _omega_vector = _omega + _omega_P; // adding Integrator term + + _accel_adjust(); // Remove centrifugal acceleration. + + #if OUTPUTMODE == 1 + update_matrix(0, 0) = 0; + update_matrix(0, 1) = -_G_Dt * _omega_vector(2); // -z + update_matrix(0, 2) = _G_Dt * _omega_vector(1); // y + update_matrix(1, 0) = _G_Dt * _omega_vector(2); // z + update_matrix(1, 1) = 0; + update_matrix(1, 2) = -_G_Dt * _omega_vector(0); // -x + update_matrix(2, 0) = -_G_Dt * _omega_vector(1); // -y + update_matrix(2, 1) = _G_Dt * _omega_vector(0); // x + update_matrix(2, 2) = 0; + #else // Uncorrected data (no drift correction) + update_matrix(0, 0) = 0; + update_matrix(0, 1) = -_G_Dt * _gyro_vector(2); // -z + update_matrix(0, 2) = _G_Dt * _gyro_vector(1); // y + update_matrix(1, 0) = _G_Dt * _gyro_vector(2); // z + update_matrix(1, 1) = 0; + update_matrix(1, 2) = -_G_Dt * _gyro_vector(0); + update_matrix(2, 0) = -_G_Dt * _gyro_vector(1); + update_matrix(2, 1) = _G_Dt * _gyro_vector(0); + update_matrix(2, 2) = 0; + #endif + + // update + _dcm_matrix += _dcm_matrix * update_matrix; + +/* +Serial.print (_G_Dt * 1000); +Serial.print (" "); +Serial.print (dcm_matrix(0, 0)); +Serial.print (" "); +Serial.print (dcm_matrix(0, 1)); +Serial.print (" "); +Serial.print (dcm_matrix(0, 2)); +Serial.print (" "); +Serial.print (dcm_matrix(1, 0)); +Serial.print (" "); +Serial.print (dcm_matrix(1, 1)); +Serial.print (" "); +Serial.print (dcm_matrix(1, 2)); +Serial.print (" "); +Serial.print (dcm_matrix(2, 0)); +Serial.print (" "); +Serial.print (dcm_matrix(2, 1)); +Serial.print (" "); +Serial.println (dcm_matrix(2, 2)); +*/ +} + +/**************************************************/ +void +AP_DCM::euler_angles(void) +{ + #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) + roll = atan2(_accel_vector(1), _accel_vector(2)); // atan2(acc_y, acc_z) + roll_sensor = degrees(roll) * 100; + pitch = -asin((_accel_vector(0)) / (double)GRAVITY); // asin(acc_x) + pitch_sensor = degrees(pitch) * 100; + yaw = 0; + #else + pitch = -asin(_dcm_matrix(2, 0)); + pitch_sensor = degrees(pitch) * 100; + roll = atan2(_dcm_matrix(2, 1), _dcm_matrix(2, 2)); + roll_sensor = degrees(roll) * 100; + yaw = atan2(_dcm_matrix(1, 0), _dcm_matrix(0, 0)); + yaw_sensor = degrees(yaw) * 100; + #endif + + /* + Serial.print ("Roll "); + Serial.print (roll_sensor / 100); + Serial.print (", Pitch "); + Serial.print (pitch_sensor / 100); + Serial.print (", Yaw "); + Serial.println (yaw_sensor / 100); + */ +} + +/**************************************************/ +//Computes the dot product of two vectors +float +DCM_Vector::dot_product(DCM_Vector const &vector2) const +{ + float op = 0; + + for(int c = 0; c < 3; c++) + op += _v[c] * vector2(c); + + return op; +} + +// cross-product +DCM_Vector +DCM_Vector::operator^(DCM_Vector const &a) const +{ + DCM_Vector result; + + result(0) = (_v[1] * a(2)) - (_v[2] * a(1)); + result(1) = (_v[2] * a(0)) - (_v[0] * a(2)); + result(2) = (_v[0] * a(1)) - (_v[1] * a(0)); + + return(result); +} + +// scale +DCM_Vector +DCM_Vector::operator*(float scale) const +{ + DCM_Vector result; + + result(0) = _v[0] * scale; + result(1) = _v[1] * scale; + result(2) = _v[2] * scale; + + return(result); +} + +// scale +void +DCM_Vector::operator*=(float scale) +{ + _v[0] *= scale; + _v[1] *= scale; + _v[2] *= scale; +} + +// add +DCM_Vector +DCM_Vector::operator+(DCM_Vector const &a) const +{ + DCM_Vector result; + + result(0) = _v[0] + a(0); + result(1) = _v[1] + a(1); + result(2) = _v[2] + a(2); + + return(result); +} + +// add +void +DCM_Vector::operator+=(DCM_Vector const &a) +{ + _v[0] += a(0); + _v[1] += a(1); + _v[2] += a(2); +} + +// magnitude +float +DCM_Vector::magnitude(void) const +{ + return(sqrt((_v[0] * _v[0]) + + (_v[1] * _v[1]) + + (_v[2] * _v[2]))); +} + +// 3x3 matrix multiply +DCM_Matrix +DCM_Matrix::operator*(DCM_Matrix const &a) const +{ + DCM_Matrix result; + + for (int x = 0; x < 3; x++) { + for (int y = 0; y < 3; y++) { + result(x, y) = + _m[x](0) * a(0, y) + + _m[x](1) * a(1, y) + + _m[x](2) * a(2, y); + } + } + return(result); +} + +// 3x3 matrix add +void +DCM_Matrix::operator+=(DCM_Matrix const &a) +{ + for (int x = 0; x < 3; x++) + for (int y = 0; y < 3; y++) + _m[x](y) += a(x,y); +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h new file mode 100644 index 0000000000..76dbb71381 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h @@ -0,0 +1,147 @@ +#ifndef AP_DCM_h +#define AP_DCM_h + +#include +//#include "WProgram.h" + +//////////////////////////////////////////////////////////////////////////////// +// XXX HACKS +class APM_Compass { +public: + int Heading_X; + int Heading_Y; +}; + +typedef uint8_t byte; + +class APM_ADC { +public: + int Ch(int c) {return ~c;}; +}; + +extern int ground_speed; +extern int pitch; +extern int yaw; +extern int roll; +extern int roll_sensor; +extern int pitch_sensor; +extern int yaw_sensor; +#define SPEEDFILT 100 + +// XXX warning, many of these are nonsense just to make the compiler think +#define abs(_x) (((_x) < 0) ? -(_x) : (_x)) +#define constrain(_x, _min, _max) (((_x) < (_min)) ? (_min) : (((_x) > (_max)) ? (_max) : (_x))) +#define sqrt(_x) ((_x) / 2) // !!! +#define radians(_x) ((_x) / (180 * 3.14)) // !!! shoot me... +#define degrees(_x) ((_x) * (180 / 3.14)) +#define accel_scale(_x) ((_x) * 3) // !!! +#define gyro_scaled_X(_x) ((_x) / 3) +#define gyro_scaled_Y(_x) ((_x) / 3) +#define gyro_scaled_Z(_x) ((_x) / 3) +#define asin(_x) ((_x) * 5) +#define atan2(_x, _y) (((_x) + (_y)) / 5) + +// XXX END HACKS +//////////////////////////////////////////////////////////////////////////////// + +class DCM_Vector { +public: + DCM_Vector(float v0 = 0, float v1 = 0, float v2 = 0); + + // access vector elements with obj(element) + float& operator() (int x) {return _v[x];}; + float operator() (int x) const {return _v[x];}; + + DCM_Vector operator+ (DCM_Vector const &a) const; // add + void operator+= (DCM_Vector const &a); // add + DCM_Vector operator^ (DCM_Vector const &a) const; // cross-product + DCM_Vector operator* (float scale) const; // scale + void operator*= (float scale); // scale + + float dot_product(DCM_Vector const &v2) const; + float magnitude(void) const; + +private: + float _v[3]; +}; + +class DCM_Matrix { +public: + DCM_Matrix(float m00 = 0, float m01 = 0, float m02 = 0, + float m10 = 0, float m11 = 0, float m12 = 0, + float m20 = 0, float m21 = 0, float m22 = 0); + + // access matrix elements with obj(x,y) + float& operator() (int x, int y) {return _m[x](y);}; + float operator() (int x, int y) const {return _m[x](y);}; + + // access matrix columns with obj(x) + DCM_Vector& operator() (int x) {return _m[x];}; + DCM_Vector operator() (int x) const {return _m[x];}; + + // matrix multiply + DCM_Matrix operator* (DCM_Matrix const &a) const; + + // matrix add + void operator+= (DCM_Matrix const &a); + +private: + DCM_Vector _m[3]; +}; + + +class AP_DCM +{ +public: + // Methods + AP_DCM(APM_Compass *withCompass); + void update_DCM(void); //G_Dt + + // XXX these are all private (called by update_DCM only?) + void read_adc_raw(void); + void euler_angles(void); + void matrix_update(void); + void drift_correction(void); + void normalize(void); + float read_adc(int select); + + float imu_health; //Metric based on accel gain deweighting + byte gyro_sat_count; + byte adc_constraints; + byte renorm_sqrt_count; + byte renorm_blowup_count; + +private: + // Methods + void _accel_adjust(void); + float _gyro_temp_comp(int i, int temp) const; + DCM_Vector _renorm(DCM_Vector const &a, uint8_t &problem); + + // members + APM_Compass *_compass; + + DCM_Matrix _dcm_matrix; + + float _adc_in[6]; // array that store the 6 ADC channels used by IMU + float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers + float _G_Dt; // Integration time for the gyros (DCM algorithm) + DCM_Vector _accel_vector; // Store the acceleration in a vector + DCM_Vector _gyro_vector; //Store the gyros turn rate in a vector + DCM_Vector _omega_vector; //Corrected Gyro_Vector data + DCM_Vector _omega_P; //Omega Proportional correction + DCM_Vector _omega_I; //Omega Integrator + DCM_Vector _omega; + DCM_Vector _error_roll_pitch; + DCM_Vector _error_yaw; + float _errorCourse; + float _course_over_ground_x; //Course overground X axis + float _course_over_ground_y; //Course overground Y axis + + // constants + static const uint8_t _sensors[6]; + static const int _sensor_signs[9]; + static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature + static const float _gyro_temp_curve[3][3]; +}; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde new file mode 100644 index 0000000000..c7225d44ae --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde @@ -0,0 +1,38 @@ +/* + Example of APM_Compass library (HMC5843 sensor). + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com +*/ + +//#include +#include // Compass Library + +unsigned long timer; + +void setup() +{ + DCM.init(); // Initialization + Serial.begin(38400); + Serial.println("Compass library test (HMC5843)"); + delay(1000); + timer = millis(); +} + +void loop() +{ + float tmp; + + if((millis()- timer) > 100){ + timer = millis(); + APM_Compass.Read(); + APM_Compass.Calculate(0, 0); // roll = 0, pitch = 0 for this example + Serial.print("Heading:"); + Serial.print(ToDeg(APM_Compass.Heading)); + Serial.print(" ("); + Serial.print(APM_Compass.Mag_X); + Serial.print(","); + Serial.print(APM_Compass.Mag_Y); + Serial.print(","); + Serial.print(APM_Compass.Mag_Z); + Serial.println(" )"); + } +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp new file mode 100644 index 0000000000..d7c4bba837 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp @@ -0,0 +1,342 @@ +/* + DataFlash.cpp - DataFlash log library for AT45DB161 + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + This code works with boards based on ATMega168/328 and ATMega1280 using SPI port + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Dataflash library for AT45DB161D flash memory + Memory organization : 4096 pages of 512 bytes + + Maximun write bandwidth : 512 bytes in 14ms + This code is written so the master never has to wait to write the data on the eeprom + + Methods: + Init() : Library initialization (SPI initialization) + StartWrite(page) : Start a write session. page=start page. + WriteByte(data) : Write a byte + WriteInt(data) : Write an integer (2 bytes) + WriteLong(data) : Write a long (4 bytes) + StartRead(page) : Start a read on (page) + GetWritePage() : Returns the last page written to + GetPage() : Returns the last page read + ReadByte() + ReadInt() + ReadLong() + + Properties: + +*/ + +extern "C" { + // AVR LibC Includes + #include + #include + #include "WConstants.h" +} + +#include "DataFlash.h" + +#define OVERWRITE_DATA 0 // 0: When reach the end page stop, 1: Start overwritten from page 1 + +// *** INTERNAL FUNCTIONS *** +unsigned char dataflash_SPI_transfer(unsigned char output) +{ + SPDR = output; // Start the transmission + while (!(SPSR & (1<> 6)); + dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); + dataflash_SPI_transfer(0x00); // don´t care bytes + + dataflash_CS_inactive(); //initiate the transfer + dataflash_CS_active(); + + while(!ReadStatus()); //monitor the status register, wait until busy-flag is high +} + +void DataFlash_Class::BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait) +{ + dataflash_CS_inactive(); // Reset dataflash command decoder + dataflash_CS_active(); + + if (BufferNum==1) + dataflash_SPI_transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE); + else + dataflash_SPI_transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE); + dataflash_SPI_transfer((unsigned char)(PageAdr >> 6)); + dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); + dataflash_SPI_transfer(0x00); // don´t care bytes + + dataflash_CS_inactive(); //initiate the transfer + dataflash_CS_active(); + + // Check if we need to wait to write the buffer to memory or we can continue... + if (wait) + while(!ReadStatus()); //monitor the status register, wait until busy-flag is high +} + +void DataFlash_Class::BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data) +{ + dataflash_CS_inactive(); // Reset dataflash command decoder + dataflash_CS_active(); + + if (BufferNum==1) + dataflash_SPI_transfer(DF_BUFFER_1_WRITE); + else + dataflash_SPI_transfer(DF_BUFFER_2_WRITE); + dataflash_SPI_transfer(0x00); //don't cares + dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address + dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address + dataflash_SPI_transfer(Data); //write data byte +} + +unsigned char DataFlash_Class::BufferRead (unsigned char BufferNum, unsigned int IntPageAdr) +{ + byte tmp; + + dataflash_CS_inactive(); // Reset dataflash command decoder + dataflash_CS_active(); + + if (BufferNum==1) + dataflash_SPI_transfer(DF_BUFFER_1_READ); + else + dataflash_SPI_transfer(DF_BUFFER_2_READ); + dataflash_SPI_transfer(0x00); //don't cares + dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address + dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address + dataflash_SPI_transfer(0x00); //don't cares + tmp = dataflash_SPI_transfer(0x00); //read data byte + + return (tmp); +} +// *** END OF INTERNAL FUNCTIONS *** + +void DataFlash_Class::PageErase (unsigned int PageAdr) +{ + dataflash_CS_inactive(); //make sure to toggle CS signal in order + dataflash_CS_active(); //to reset Dataflash command decoder + dataflash_SPI_transfer(DF_PAGE_ERASE); // Command + dataflash_SPI_transfer((unsigned char)(PageAdr >> 6)); //upper part of page address + dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); //lower part of page address and MSB of int.page adr. + dataflash_SPI_transfer(0x00); // "dont cares" + dataflash_CS_inactive(); //initiate flash page erase + dataflash_CS_active(); + while(!ReadStatus()); +} + +// *** DATAFLASH PUBLIC FUNCTIONS *** +void DataFlash_Class::StartWrite(int PageAdr) +{ + df_BufferNum=1; + df_BufferIdx=0; + df_PageAdr=PageAdr; + df_Stop_Write=0; + WaitReady(); +} + +void DataFlash_Class::WriteByte(byte data) +{ + if (!df_Stop_Write) + { + BufferWrite(df_BufferNum,df_BufferIdx,data); + df_BufferIdx++; + if (df_BufferIdx >= 512) // End of buffer? + { + df_BufferIdx=0; + BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT + df_PageAdr++; + if (OVERWRITE_DATA==1) + { + if (df_PageAdr>=4096) // If we reach the end of the memory, start from the begining + df_PageAdr = 1; + } + else + { + if (df_PageAdr>=4096) // If we reach the end of the memory, stop here + df_Stop_Write=1; + } + + if (df_BufferNum==1) // Change buffer to continue writing... + df_BufferNum=2; + else + df_BufferNum=1; + } + } +} + +void DataFlash_Class::WriteInt(int data) +{ + WriteByte(data>>8); // High byte + WriteByte(data&0xFF); // Low byte +} + +void DataFlash_Class::WriteLong(long data) +{ + WriteByte(data>>24); // First byte + WriteByte(data>>16); + WriteByte(data>>8); + WriteByte(data&0xFF); // Last byte +} + +// Get the last page written to +int DataFlash_Class::GetWritePage() +{ + return(df_PageAdr); +} + +// Get the last page read +int DataFlash_Class::GetPage() +{ + return(df_Read_PageAdr-1); +} + +void DataFlash_Class::StartRead(int PageAdr) +{ + df_Read_BufferNum=1; + df_Read_BufferIdx=0; + df_Read_PageAdr=PageAdr; + WaitReady(); + PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer + df_Read_PageAdr++; +} + +byte DataFlash_Class::ReadByte() +{ + byte result; + + WaitReady(); + result = BufferRead(df_Read_BufferNum,df_Read_BufferIdx); + df_Read_BufferIdx++; + if (df_Read_BufferIdx >= 512) // End of buffer? + { + df_Read_BufferIdx=0; + PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer + df_Read_PageAdr++; + if (df_Read_PageAdr>=4096) // If we reach the end of the memory, start from the begining + { + df_Read_PageAdr = 0; + df_Read_END = true; + } + } + return result; +} + +int DataFlash_Class::ReadInt() +{ + int result; + + result = ReadByte(); // High byte + result = (result<<8) | ReadByte(); // Low byte + return result; +} + +long DataFlash_Class::ReadLong() +{ + long result; + + result = ReadByte(); // First byte + result = (result<<8) | ReadByte(); + result = (result<<8) | ReadByte(); + result = (result<<8) | ReadByte(); // Last byte + return result; +} + +// make one instance for the user to use +DataFlash_Class DataFlash; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h new file mode 100644 index 0000000000..037421c989 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h @@ -0,0 +1,86 @@ +/* ************************************************************ */ +/* Test for DataFlash Log library */ +/* ************************************************************ */ +#ifndef DataFlash_h +#define DataFlash_h + +// arduino mega SPI pins +#if defined(__AVR_ATmega1280__) + #define DF_DATAOUT 51 // MOSI + #define DF_DATAIN 50 // MISO + #define DF_SPICLOCK 52 // SCK + #define DF_SLAVESELECT 53 // SS (PB0) + #define DF_RESET 31 // RESET (PC6) +#else // normal arduino SPI pins... + #define DF_DATAOUT 11 //MOSI + #define DF_DATAIN 12 //MISO + #define DF_SPICLOCK 13 //SCK + #define DF_SLAVESELECT 10 //SS +#endif + +// AT45DB161D Commands (from Datasheet) +#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53 +#define DF_TRANSFER_PAGE_TO_BUFFER_2 0x55 +#define DF_STATUS_REGISTER_READ 0xD7 +#define DF_READ_MANUFACTURER_AND_DEVICE_ID 0x9F +#define DF_PAGE_READ 0xD2 +#define DF_BUFFER_1_READ 0xD4 +#define DF_BUFFER_2_READ 0xD6 +#define DF_BUFFER_1_WRITE 0x84 +#define DF_BUFFER_2_WRITE 0x87 +#define DF_BUFFER_1_TO_PAGE_WITH_ERASE 0x83 +#define DF_BUFFER_2_TO_PAGE_WITH_ERASE 0x86 +#define DF_PAGE_ERASE 0x81 +#define DF_BLOCK_ERASE 0x50 +#define DF_SECTOR_ERASE 0x7C +#define DF_CHIP_ERASE_0 0xC7 +#define DF_CHIP_ERASE_1 0x94 +#define DF_CHIP_ERASE_2 0x80 +#define DF_CHIP_ERASE_3 0x9A + +class DataFlash_Class +{ + private: + // DataFlash Log variables... + unsigned char df_BufferNum; + unsigned char df_Read_BufferNum; + unsigned int df_BufferIdx; + unsigned int df_Read_BufferIdx; + unsigned int df_PageAdr; + unsigned int df_Read_PageAdr; + unsigned char df_Read_END; + unsigned char df_Stop_Write; + //Methods + unsigned char BufferRead (unsigned char BufferNum, unsigned int IntPageAdr); + void BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data); + void BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait); + void PageToBuffer(unsigned char BufferNum, unsigned int PageAdr); + void WaitReady(); + unsigned char ReadStatus(); + + public: + unsigned char df_manufacturer; + unsigned char df_device_0; + unsigned char df_device_1; + + DataFlash_Class(); // Constructor + void Init(); + void ReadManufacturerID(); + int GetPage(); + int GetWritePage(); + void PageErase (unsigned int PageAdr); + // Write methods + void StartWrite(int PageAdr); + void WriteByte(unsigned char data); + void WriteInt(int data); + void WriteLong(long data); + // Read methods + void StartRead(int PageAdr); + unsigned char ReadByte(); + int ReadInt(); + long ReadLong(); +}; + +extern DataFlash_Class DataFlash; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde new file mode 100644 index 0000000000..8ffc712d72 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde @@ -0,0 +1,99 @@ +/* + Example of DataFlash library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com +*/ + +#include + +#define HEAD_BYTE1 0xA3 +#define HEAD_BYTE2 0x95 + +void setup() +{ + Serial.begin(57600); + DataFlash.Init(); // DataFlash initialization + + Serial.println("Dataflash Log Test 1.0"); + + // Test + delay(20); + DataFlash.ReadManufacturerID(); + delay(10); + Serial.print("Manufacturer:"); + Serial.print(int(DataFlash.df_manufacturer)); + Serial.print(","); + Serial.print(int(DataFlash.df_device_0)); + Serial.print(","); + Serial.print(int(DataFlash.df_device_1)); + Serial.println(); + + // We start to write some info (sequentialy) starting from page 1 + // This is similar to what we will do... + DataFlash.StartWrite(1); + Serial.println("Writing to flash... wait..."); + for (int i=0;i<1000;i++) // Write 1000 packets... + { + // We write packets of binary data... (without worry about nothing more) + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteInt(2000+i); + DataFlash.WriteInt(2001+i); + DataFlash.WriteInt(2002+i); + DataFlash.WriteInt(2003+i); + DataFlash.WriteLong((long)i*5000); + DataFlash.WriteLong((long)i*16268); + DataFlash.WriteByte(0xA2); // 2 bytes of checksum (example) + DataFlash.WriteByte(0x4E); + delay(10); + } + delay(100); +} + +void loop() +{ + int i; + byte tmp_byte1; + byte tmp_byte2; + int tmp_int; + long tmp_long; + + Serial.println("Start reading page 1..."); + DataFlash.StartRead(1); // We start reading from page 1 + for (i=0;i<200;i++) // Read 200 packets... + { + tmp_byte1=DataFlash.ReadByte(); + tmp_byte2=DataFlash.ReadByte(); + Serial.print("PACKET:"); + if ((tmp_byte1==HEAD_BYTE1)&&(tmp_byte1==HEAD_BYTE1)) + { + // Read 4 ints... + tmp_int=DataFlash.ReadInt(); + Serial.print(tmp_int); + Serial.print(","); + tmp_int=DataFlash.ReadInt(); + Serial.print(tmp_int); + Serial.print(","); + tmp_int=DataFlash.ReadInt(); + Serial.print(tmp_int); + Serial.print(","); + tmp_int=DataFlash.ReadInt(); + Serial.print(tmp_int); + Serial.print(","); + + // Read 2 longs... + tmp_long=DataFlash.ReadLong(); + Serial.print(tmp_long); + Serial.print(","); + tmp_long=DataFlash.ReadLong(); + Serial.print(tmp_long); + Serial.print(";"); + + // Read the checksum... + tmp_byte1=DataFlash.ReadByte(); + tmp_byte2=DataFlash.ReadByte(); + } + Serial.println(); + } + + delay(10000); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt new file mode 100644 index 0000000000..eea5025224 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt @@ -0,0 +1,14 @@ +DataFlash KEYWORD1 +Init KEYWORD2 +ReadManufacturerID KEYWORD2 +GetPage KEYWORD2 +PageErase KEYWORD2 +StartWrite KEYWORD2 +StartRead KEYWORD2 +ReadByte KEYWORD2 +ReadInt KEYWORD2 +ReadLong KEYWORD2 +WriteByte KEYWORD2 +WriteInt KEYWORD2 +WriteLong KEYWORD2 + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp new file mode 100644 index 0000000000..88f131bbe4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp @@ -0,0 +1,348 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- +// +// Interrupt-driven serial transmit/receive library. +// +// Copyright (c) 2010 Michael Smith. All rights reserved. +// +// Receive and baudrate calculations derived from the Arduino +// HardwareSerial driver: +// +// Copyright (c) 2006 Nicholas Zambetti. All right reserved. +// +// Transmit algorithm inspired by work: +// +// Code Jose Julio and Jordi Munoz. DIYDrones.com +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +// + + +//#include "../AP_Common/AP_Common.h" +#include "FastSerial.h" +#include "WProgram.h" + +#if defined(__AVR_ATmega1280__) +# define FS_MAX_PORTS 4 +#else +# define FS_MAX_PORTS 1 +#endif + +FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; +FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; + +// Default buffer sizes +#define RX_BUFFER_SIZE 128 +#define TX_BUFFER_SIZE 64 +#define BUFFER_MAX 512 + +// Interrupt handlers ////////////////////////////////////////////////////////// + +#if 0 +#define HANDLERS(_PORT, _RXVECTOR, _TXVECTOR, _UDR) \ +SIGNAL(_RXVECTOR) \ +{ \ + unsigned char c = _UDR; \ + ports[_PORT]->receive(c); \ +} \ + \ +SIGNAL(_TXVECTOR) \ +{ \ + ports[_PORT]->transmit(); \ +} \ +struct hack + +#if defined(__AVR_ATmega8__) +HANDLERS(0, SIG_UART_RECV, SIG_UART_DATA, UDR); +#else +HANDLERS(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0); +#if defined(__AVR_ATmega1280__) +HANDLERS(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1); +HANDLERS(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2); +//HANDLERS(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3); +#endif +#endif +#endif + +// Constructor ///////////////////////////////////////////////////////////////// + +FastSerial::FastSerial(const uint8_t portNumber, + volatile uint8_t *ubrrh, + volatile uint8_t *ubrrl, + volatile uint8_t *ucsra, + volatile uint8_t *ucsrb, + volatile uint8_t *udr, + const uint8_t u2x, + const uint8_t portEnableBits, + const uint8_t portTxBits) +{ + _ubrrh = ubrrh; + _ubrrl = ubrrl; + _ucsra = ucsra; + _ucsrb = ucsrb; + _udr = udr; + _u2x = u2x; + _portEnableBits = portEnableBits; + _portTxBits = portTxBits; + + // init buffers + _rxBuffer = &__FastSerial__rxBuffer[portNumber]; + _txBuffer->head = _txBuffer->tail = 0; + _txBuffer = &__FastSerial__txBuffer[portNumber]; + _rxBuffer->head = _rxBuffer->tail = 0; + + // init stdio + fdev_setup_stream(&_fd, &FastSerial::_putchar, &FastSerial::_getchar, _FDEV_SETUP_RW); + fdev_set_udata(&_fd, this); + if (0 == portNumber) { + stdout = &_fd; // serial port 0 is always the default console + stdin = &_fd; + stderr = &_fd; + } +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void FastSerial::begin(long baud) +{ + unsigned int rxb, txb; + + // If we are re-configuring an already-open port, preserve the + // existing buffer sizes. + if (_open) { + rxb = _rxBuffer->mask + 1; + txb = _txBuffer->mask + 1; + } else { + rxb = RX_BUFFER_SIZE; + txb = TX_BUFFER_SIZE; + } + + begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE); +} + +void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) +{ + uint16_t ubrr; + bool use_u2x = false; + int ureg, u2; + long breg, b2, dreg, d2; + + // if we are currently open, close and restart + if (_open) + end(); + + // allocate buffers + if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) || + !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) { + end(); + return; // couldn't allocate buffers - fatal + } + _open = true; + + // U2X mode is needed for bitrates higher than (F_CPU / 16) + if (baud > F_CPU / 16) { + use_u2x = true; + ubrr = F_CPU / (8 * baud) - 1; + } else { + // Determine whether u2x mode would give a closer + // approximation of the desired speed. + + // ubrr for non-2x mode, corresponding baudrate and delta + ureg = F_CPU / 16 / baud - 1; + breg = F_CPU / 16 / (ureg + 1); + dreg = abs(baud - breg); + + // ubrr for 2x mode, corresponding bitrate and delta + u2 = F_CPU / 8 / baud - 1; + b2 = F_CPU / 8 / (u2 + 1); + d2 = abs(baud - b2); + + // Pick the setting that gives the smallest rate + // error, preferring non-u2x mode if the delta is + // identical. + if (dreg <= d2) { + ubrr = ureg; + } else { + ubrr = u2; + use_u2x = true; + } + } + + *_ucsra = use_u2x ? _BV(_u2x) : 0; + *_ubrrh = ubrr >> 8; + *_ubrrl = ubrr; + *_ucsrb |= _portEnableBits; +} + +void FastSerial::end() +{ + *_ucsrb &= ~(_portEnableBits | _portTxBits); + + _freeBuffer(_rxBuffer); + _freeBuffer(_txBuffer); + _open = false; +} + +int +FastSerial::available(void) +{ + if (!_open) + return(-1); + return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask); +} + +int +FastSerial::read(void) +{ + uint8_t c; + + // if the head and tail are equal, the buffer is empty + if (!_open || (_rxBuffer->head == _rxBuffer->tail)) + return(-1); + + // pull character from tail + c = _rxBuffer->bytes[_rxBuffer->tail]; + _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask; + + return(c); +} + +void +FastSerial::flush(void) +{ + // don't reverse this or there may be problems if the RX interrupt + // occurs after reading the value of _rxBuffer->head but before writing + // the value to _rxBuffer->tail; the previous value of head + // may be written to tail, making it appear as if the buffer + // don't reverse this or there may be problems if the RX interrupt + // occurs after reading the value of head but before writing + // the value to tail; the previous value of rx_buffer_head + // may be written to tail, making it appear as if the buffer + // were full, not empty. + _rxBuffer->head = _rxBuffer->tail; + + // don't reverse this or there may be problems if the TX interrupt + // occurs after reading the value of _txBuffer->tail but before writing + // the value to _txBuffer->head. + _txBuffer->tail = _rxBuffer->head; +} + +void +FastSerial::write(uint8_t c) +{ + int16_t i; + + if (!_open) // drop bytes if not open + return; + + // wait for room in the tx buffer + i = (_txBuffer->head + 1) & _txBuffer->mask; + while (i == _txBuffer->tail) + ; + + // add byte to the buffer + _txBuffer->bytes[_txBuffer->head] = c; + _txBuffer->head = i; + + // enable the data-ready interrupt, as it may be off if the buffer is empty + *_ucsrb |= _portTxBits; +} + +// STDIO emulation ///////////////////////////////////////////////////////////// + +int +FastSerial::_putchar(char c, FILE *stream) +{ + FastSerial *fs; + + fs = (FastSerial *)fdev_get_udata(stream); + if ('\n' == c) + fs->write('\r'); // ASCII translation on the cheap + fs->write(c); + return(0); +} + +int +FastSerial::_getchar(FILE *stream) +{ + FastSerial *fs; + + fs = (FastSerial *)fdev_get_udata(stream); + + // We return -1 if there is nothing to read, which the library interprets + // as an error, which our clients will need to deal with. + return(fs->read()); +} + +int +FastSerial::printf(const char *fmt, ...) +{ + va_list ap; + int i; + + va_start(ap, fmt); + i = vfprintf(&_fd, fmt, ap); + va_end(ap); + + return(i); +} + +int +FastSerial::printf_P(const char *fmt, ...) +{ + va_list ap; + int i; + + va_start(ap, fmt); + i = vfprintf_P(&_fd, fmt, ap); + va_end(ap); + + return(i); +} + +// Buffer management /////////////////////////////////////////////////////////// + +bool +FastSerial::_allocBuffer(Buffer *buffer, unsigned int size) +{ + uint8_t shift; + + // init buffer state + buffer->head = buffer->tail = 0; + + // Compute the power of 2 greater or equal to the requested buffer size + // and then a mask to simplify wrapping operations. Using __builtin_clz + // would seem to make sense, but it uses a 256(!) byte table. + // Note that we ignore requests for more than BUFFER_MAX space. + for (shift = 1; (1U << shift) < min(BUFFER_MAX, size); shift++) + ; + buffer->mask = (1 << shift) - 1; + + // allocate memory for the buffer - if this fails, we fail + buffer->bytes = (uint8_t *)malloc(buffer->mask + 1); + + return(buffer->bytes != NULL); +} + +void +FastSerial::_freeBuffer(Buffer *buffer) +{ + buffer->head = buffer->tail = 0; + buffer->mask = 0; + if (NULL != buffer->bytes) { + free(buffer->bytes); + buffer->bytes = NULL; + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h new file mode 100644 index 0000000000..1b83ebcca7 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h @@ -0,0 +1,261 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- +// +// Interrupt-driven serial transmit/receive library. +// +// Copyright (c) 2010 Michael Smith. All rights reserved. +// +// Receive and baudrate calculations derived from the Arduino +// HardwareSerial driver: +// +// Copyright (c) 2006 Nicholas Zambetti. All right reserved. +// +// Transmit algorithm inspired by work: +// +// Code Jose Julio and Jordi Munoz. DIYDrones.com +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later +// version. +// +// This library is distributed in the hope that it will be +// useful, but WITHOUT ANY WARRANTY; without even the implied +// warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR +// PURPOSE. See the GNU Lesser General Public License for more +// details. +// +// You should have received a copy of the GNU Lesser General +// Public License along with this library; if not, write to the +// Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, +// Boston, MA 02110-1301 USA +// + +// +// Note that this library does not pre-declare drivers for serial +// ports; the user must explicitly create drivers for the ports they +// wish to use. This is less friendly than the stock Arduino driver, +// but it saves ~200 bytes for every unused port. +// + +#ifndef FastSerial_h +#define FastSerial_h + +// disable the stock Arduino serial driver +#ifdef HardwareSerial_h +# error Must include FastSerial.h before the Arduino serial driver is defined. +#endif +#define HardwareSerial_h + +#include +#include +#include +#include +#include + +// +// Because Arduino libraries aren't really libraries, but we want to +// only define interrupt handlers for serial ports that are actually +// used, we have to force our users to define them using a macro. +// +// Due to the way interrupt vectors are specified, we have to have +// a separate macro for every port. Ugh. +// +// The macros are: +// +// FastSerialPort0() creates referencing serial port 0 +// FastSerialPort1() creates referencing serial port 1 +// FastSerialPort2() creates referencing serial port 2 +// FastSerialPort3() creates referencing serial port 3 +// + +// +// Forward declarations for clients that want to assume that the +// default Serial* objects exist. +// +// Note that the application is responsible for ensuring that these +// actually get defined, otherwise Arduino will suck in the +// HardwareSerial library and linking will fail. +// +extern class FastSerial Serial; +extern class FastSerial Serial1; +extern class FastSerial Serial2; +//extern class FastSerial Serial3; + + +class FastSerial : public Stream { +public: + FastSerial(const uint8_t portNumber, + volatile uint8_t *ubrrh, + volatile uint8_t *ubrrl, + volatile uint8_t *ucsra, + volatile uint8_t *ucsrb, + volatile uint8_t *udr, + const uint8_t u2x, + const uint8_t portEnableBits, + const uint8_t portTxBits); + + // Serial API + void begin(long baud); + void begin(long baud, unsigned int rxSpace, unsigned int txSpace); + void end(void); + int available(void); + int read(void); + void flush(void); + void write(uint8_t c); + using Stream::write; + + // stdio extensions + int printf(const char *fmt, ...); + int printf_P(const char *fmt, ...); + FILE *getfd(void) { return &_fd; }; + + // public so the interrupt handlers can see it + struct Buffer { + volatile uint16_t head, tail; + uint16_t mask; + uint8_t *bytes; + }; + +private: + // register accessors + volatile uint8_t *_ubrrh; + volatile uint8_t *_ubrrl; + volatile uint8_t *_ucsra; + volatile uint8_t *_ucsrb; + volatile uint8_t *_udr; + + // register magic numbers + uint8_t _portEnableBits; // rx, tx and rx interrupt enables + uint8_t _portTxBits; // tx data and completion interrupt enables + uint8_t _u2x; + + // ring buffers + Buffer *_rxBuffer; + Buffer *_txBuffer; + bool _open; + + bool _allocBuffer(Buffer *buffer, unsigned int size); + void _freeBuffer(Buffer *buffer); + + // stdio emulation + FILE _fd; + static int _putchar(char c, FILE *stream); + static int _getchar(FILE *stream); +}; + +// Used by the per-port interrupt vectors +extern FastSerial::Buffer __FastSerial__rxBuffer[]; +extern FastSerial::Buffer __FastSerial__txBuffer[]; + +// Generic Rx/Tx vectors for a serial port - needs to know magic numbers +#define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \ +ISR(_RXVECTOR, ISR_BLOCK) \ +{ \ + uint8_t c; \ + int16_t i; \ + \ + /* read the byte as quickly as possible */ \ + c = _UDR; \ + /* work out where the head will go next */ \ + i = (__FastSerial__rxBuffer[_PORT].head + 1) & __FastSerial__rxBuffer[_PORT].mask; \ + /* decide whether we have space for another byte */ \ + if (i != __FastSerial__rxBuffer[_PORT].tail) { \ + /* we do, move the head */ \ + __FastSerial__rxBuffer[_PORT].bytes[__FastSerial__rxBuffer[_PORT].head] = c; \ + __FastSerial__rxBuffer[_PORT].head = i; \ + } \ +} \ +ISR(_TXVECTOR, ISR_BLOCK) \ +{ \ + /* if there is another character to send */ \ + if (__FastSerial__txBuffer[_PORT].tail != __FastSerial__txBuffer[_PORT].head) { \ + _UDR = __FastSerial__txBuffer[_PORT].bytes[__FastSerial__txBuffer[_PORT].tail]; \ + /* increment the tail */ \ + __FastSerial__txBuffer[_PORT].tail = \ + (__FastSerial__txBuffer[_PORT].tail + 1) & __FastSerial__txBuffer[_PORT].mask; \ + } else { \ + /* there are no more bytes to send, disable the interrupt */ \ + if (__FastSerial__txBuffer[_PORT].head == __FastSerial__txBuffer[_PORT].tail) \ + _UCSRB &= ~_TXBITS; \ + } \ +} \ +struct hack + +// Macros defining serial ports +#if defined(__AVR_ATmega1280__) +#define FastSerialPort0(_portName) \ + FastSerial _portName(0, \ + &UBRR0H, \ + &UBRR0L, \ + &UCSR0A, \ + &UCSR0B, \ + &UDR0, \ + U2X0, \ + (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ + (_BV(UDRIE0))); \ + FastSerialHandler(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0, UCSR0B, _BV(UDRIE0)) +#define FastSerialPort1(_portName) \ + FastSerial _portName(1, \ + &UBRR1H, \ + &UBRR1L, \ + &UCSR1A, \ + &UCSR1B, \ + &UDR1, \ + U2X1, \ + (_BV(RXEN1) | _BV(TXEN1) | _BV(RXCIE1)), \ + (_BV(UDRIE1))); \ + FastSerialHandler(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1, UCSR1B, _BV(UDRIE1)) +#define FastSerialPort2(_portName) \ + FastSerial _portName(2, \ + &UBRR2H, \ + &UBRR2L, \ + &UCSR2A, \ + &UCSR2B, \ + &UDR2, \ + U2X2, \ + (_BV(RXEN2) | _BV(TXEN2) | _BV(RXCIE2)), \ + (_BV(UDRIE2))); \ + FastSerialHandler(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2, UCSR2B, _BV(UDRIE2)) +/* + #define FastSerialPort3(_portName) \ + FastSerial _portName(3, \ + &UBRR3H, \ + &UBRR3L, \ + &UCSR3A, \ + &UCSR3B, \ + &UDR3, \ + U2X3, \ + (_BV(RXEN3) | _BV(TXEN3) | _BV(RXCIE3)), \ + (_BV(UDRIE3))); \ + FastSerialHandler(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3, UCSR3B, _BV(UDRIE3)) +*/ + #else +#if defined(__AVR_ATmega8__) +#define FastSerialPort0(_portName) \ + FastSerial _portName(0, \ + &UBRR0H, \ + &UBRR0L, \ + &UCSR0A, \ + &UCSR0B, \ + &UDR0, \ + U2X0, \ + (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ + (_BV(UDRIE0))); \ + FastSerialHandler(0, SIG_UART_RECV, SIG_UART_DATA, UDR0, UCSR0B, _BV(UDRIE0)) +#else +// note no SIG_USART_* defines for the 168, 328, etc. +#define FastSerialPort0(_portName) \ + FastSerial _portName(0, \ + &UBRR0H, \ + &UBRR0L, \ + &UCSR0A, \ + &UCSR0B, \ + &UDR0, \ + U2X0, \ + (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ + (_BV(UDRIE0))); \ + FastSerialHandler(0, USART_RX_vect, USART_UDRE_vect, UDR0, UCSR0B, _BV(UDRIE0)) +#endif +#endif +#endif // FastSerial_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde new file mode 100644 index 0000000000..e752e5c099 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde @@ -0,0 +1,55 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- + +// +// Example code for the FastSerial driver. +// +// This code is placed into the public domain. +// + +// +// Include the FastSerial library header. +// +// Note that this causes the standard Arduino Serial* driver to be +// disabled. +// +#include + +// +// Create a FastSerial driver that looks just like the stock Arduino +// driver. +// +FastSerialPort0(Serial); + +// +// To create a driver for a different serial port, on a board that +// supports more than one, use the appropriate macro: +// +//FastSerialPort2(Serial2); + + +void setup(void) +{ + // + // Set the speed for our replacement serial port. + // + Serial.begin(38400); + + // + // And send a message. + // + Serial.println("begin"); +} + +void +loop(void) +{ + int c; + + // + // Perform a simple loopback operation. + // + c = Serial.read(); + if (-1 != c) + Serial.write(c); +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt new file mode 100644 index 0000000000..5be403f4fa --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt @@ -0,0 +1,7 @@ +FastSerial KEYWORD1 +begin KEYWORD2 +end KEYWORD2 +available KEYWORD2 +read KEYWORD2 +flush KEYWORD2 +write KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..944be92a098448bf80c5e9adeeaae7f7a2998512 GIT binary patch literal 6148 zcmeHKOH0E*5T0$TramASp~uB@k+v49;30$x9)%Wqu%d}6G!UE8q*SQU;2-iY_$&N9 z&g^c%(ws%?4D5ch^Vr7+`vL&r&Vwd^4FDDvLP1J_kh#!RGQosG^)ZDAMv(Z6WLnVC zUo_F&Eu8Z+c<=;2=I=+bkG>Co8ih&PXncuMxw5mnXIcB!;cyU6f_UM^^GT50_Qvs| z7X~BWo82ir_0oCbdNbv@@zODRl-`fy2=$?f9)v+UzPD@3W$U6nny3f@uKSWb{UMm~#qM+a8)d?tT|kOX!5OAtzfj=@|bdQgN;MbxRn zd@+PhN4qp}j=@}`P6wez#(vDmtIvy2qoZA_a1f40u9*R5V4Z=oZrXJJU*VQ1{N%5v zaElpW2L2fXqSEbkU0js8TQ`xkl=R S>?$3R9|DRHu9$&eVBi~mkxG#O literal 0 HcmV?d00001 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp new file mode 100644 index 0000000000..a087ce9516 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp @@ -0,0 +1,279 @@ +/* + GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino +*/ + +#include "GPS_IMU.h" +#include +#include "WProgram.h" + + +// Constructors //////////////////////////////////////////////////////////////// +GPS_IMU_Class::GPS_IMU_Class() +{ +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void GPS_IMU_Class::Init(void) +{ + ck_a = 0; + ck_b = 0; + IMU_step = 0; + NewData = 0; + Fix = 0; + PrintErrors = 0; + + IMU_timer = millis(); //Restarting timer... + // Initialize serial port + #if defined(__AVR_ATmega1280__) + Serial1.begin(38400); // Serial port 1 on ATMega1280 + #else + Serial.begin(38400); + #endif +} + +// optimization : This code don´t wait for data, 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. +void GPS_IMU_Class::Read(void) +{ + static unsigned long GPS_timer = 0; + byte data; + int numc = 0; + static byte message_num = 0; + + #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... + numc = Serial.available(); + #else + numc = Serial.available(); + #endif + + if (numc > 0){ + for (int i=0;i 28){ + IMU_step = 0; //Bad data, so restart to IMU_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); + IMU_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) { + IMU_step++; + } + break; + + case 7: + IMU_ck_a = data; // First checksum byte + IMU_step++; + break; + + case 8: + IMU_ck_b = data; // Second checksum byte + + // We end the IMU/GPS read... + // Verify the received checksum with the generated checksum.. + if((ck_a == IMU_ck_a) && (ck_b == IMU_ck_b)) { + if (message_num == 0x02) { + IMU_join_data(); + IMU_timer = millis(); + } else if (message_num == 0x03) { + GPS_join_data(); + GPS_timer = millis(); + } else if (message_num == 0x04) { + IMU2_join_data(); + IMU_timer = millis(); + } else if (message_num == 0x0a) { + //PERF_join_data(); + } else { + Serial.print("Invalid message number = "); + Serial.println(message_num,DEC); + } + } else { + Serial.println("XXX Checksum error"); //bad checksum + //imu_checksum_error_count++; + } + // Variable initialization + IMU_step = 0; + payload_counter = 0; + ck_a = 0; + ck_b = 0; + IMU_timer = millis(); //Restarting timer... + break; + } + }// End for... + } + // If we don't receive GPS packets in 2 seconds => Bad FIX state + if ((millis() - GPS_timer) > 3000){ + Fix = 0; + } + if (PrintErrors){ + Serial.println("ERR:GPS_TIMEOUT!!"); + } +} + +/**************************************************************** + * + ****************************************************************/ + +void GPS_IMU_Class::IMU_join_data(void) +{ + int j; + + + //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 + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + roll_sensor = intUnion.word; + + //Storing IMU pitch + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + pitch_sensor = intUnion.word; + + //Storing IMU heading (yaw) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Ground_Course = intUnion.word; + imu_ok = true; +} + +void GPS_IMU_Class::IMU2_join_data() +{ + int j=0; + + //Storing IMU roll + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + roll_sensor = intUnion.word; + + //Storing IMU pitch + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + pitch_sensor = intUnion.word; + + //Storing IMU heading (yaw) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Ground_Course = (unsigned int)intUnion.word; + + //Storing airspeed + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + airspeed = intUnion.word; + + imu_ok = true; + +} + +void GPS_IMU_Class::GPS_join_data(void) +{ + //gps_messages_received++; + int j = 0; + + Longitude = join_4_bytes(&buffer[j]); // Lat and Lon * 10**7 + j += 4; + + Lattitude = join_4_bytes(&buffer[j]); + j += 4; + + //Storing GPS Height above the sea level + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Altitude = (long)intUnion.word * 10; // Altitude in meters * 100 + + //Storing Speed (3-D) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Speed_3d = Ground_Speed = (float)intUnion.word; // Speed in M/S * 100 + + //We skip the gps ground course because we use yaw value from the IMU for ground course + j += 2; + Time = join_4_bytes(&buffer[j]); // Time of Week in milliseconds + j +=4; + imu_health = buffer[j++]; + + NewData = 1; + Fix = 1; + +} + + +/**************************************************************** + * + ****************************************************************/ + // Join 4 bytes into a long +long GPS_IMU_Class::join_4_bytes(unsigned char Buffer[]) +{ + longUnion.byte[0] = *Buffer; + longUnion.byte[1] = *(Buffer+1); + longUnion.byte[2] = *(Buffer+2); + longUnion.byte[3] = *(Buffer+3); + return(longUnion.dword); +} + + +/**************************************************************** + * + ****************************************************************/ +// checksum algorithm +void GPS_IMU_Class::checksum(byte IMU_data) +{ + ck_a+=IMU_data; + ck_b+=ck_a; +} + +GPS_IMU_Class GPS; diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h new file mode 100644 index 0000000000..fcf247b911 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h @@ -0,0 +1,69 @@ +#ifndef GPS_IMU_h +#define GPS_IMU_h + +#include + +#define IMU_MAXPAYLOAD 32 + +class GPS_IMU_Class +{ + private: + // Internal variables + union int_union { + int16_t word; + uint8_t byte[2]; + } intUnion; + + union long_union { + int32_t dword; + uint8_t byte[4]; + } longUnion; + + uint8_t ck_a; // Packet checksum + uint8_t ck_b; + uint8_t IMU_ck_a; + uint8_t IMU_ck_b; + uint8_t IMU_step; + uint8_t IMU_class; + uint8_t message_num; + uint8_t payload_length; + uint8_t payload_counter; + uint8_t buffer[IMU_MAXPAYLOAD]; + + long IMU_timer; + long IMU_ecefVZ; + void IMU_join_data(); + void IMU2_join_data(); + void GPS_join_data(); + void checksum(unsigned char IMU_data); + long join_4_bytes(unsigned char Buffer[]); + + public: + // Methods + GPS_IMU_Class(); + void Init(); + void Read(); + + // 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; + + long Time; //GPS Millisecond Time of Week + long Lattitude; // Geographic coordinates + long Longitude; + long Altitude; + long Ground_Speed; + long Ground_Course; + long Speed_3d; + + uint8_t NumSats; // Number of visible satelites + uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) + uint8_t NewData; // 1:New GPS Data + uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) +}; + +extern GPS_IMU_Class GPS; +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..ccb9677482f0a50b6df789e9064a74cc06e30209 GIT binary patch literal 6148 zcmeHKJxfDD5S-N%0aK*3yuaW-I6@>?2tu${G5NBEsmQTU=CnPcf`AgvH5fNkzG{Ah;+W=4)=J(6ApMARi95dcZL-nuqN+2 ze~-gv|GItI?Y8Hs%1Qw#AO)m=6p#YHP{4aHZF-fcC*b9fm_;fJD z2tb@L9maLc62#^SVlNyLnW0%yiAl8@F)ZoKx2o%fLt@flHGEi|Y&D@+oX+>RD2Me# zMJXT!P8GP!?cDqSE&Yf2|CFSi6p#Y{N&%ZJ*UJU3RK0a{Iq$WN{!I6p54s!IL1Bn? kOpJESjkn|LD9XC#Yd-IVLt@aG4?0ml1Fnlq3jDPKpHSTuVgLXD literal 0 HcmV?d00001 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde new file mode 100644 index 0000000000..5ccd3f9dd1 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde @@ -0,0 +1,44 @@ +/* + Example of GPS IMU library. + Code by Jordi Munoz, Jose Julio and, Jason Short . DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) + and with standard ATMega168 and ATMega328 on Serial Port 0 +*/ + +#include // GPS Library + +void setup() +{ + Serial.begin(38400); + Serial.println("GPS IMU library test"); + GPS.Init(); // GPS Initialization + delay(1000); +} +void loop() +{ + GPS.Read(); + if (1){ // New GPS data? + + Serial.print("GPS:"); + Serial.print(" Lat:"); + Serial.print(GPS.Lattitude); + Serial.print(" Lon:"); + Serial.print(GPS.Longitude); + Serial.print(" Alt:"); + Serial.print((float)GPS.Altitude/100.0); + Serial.print(" GSP:"); + Serial.print((float)GPS.Ground_Speed/100.0); + Serial.print(" COG:"); + Serial.print(GPS.Ground_Course/1000000); + Serial.print(" SAT:"); + Serial.print((int)GPS.NumSats); + Serial.print(" FIX:"); + Serial.print((int)GPS.Fix); + Serial.print(" TIM:"); + Serial.print(GPS.Time); + Serial.println(); + GPS.NewData = 0; // We have read the data + } + delay(20); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt new file mode 100644 index 0000000000..9ce6d3ae99 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt @@ -0,0 +1,16 @@ +GPS KEYWORD1 +GPS_IMU KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Time KEYWORD2 +Lattitude KEYWORD2 +Longitude KEYWORD2 +Altitude KEYWORD2 +Ground_Speed KETWORD2 +Ground_Course KEYWORD2 +Speed_3d KEYWORD2 +NumSats KEYWORD2 +Fix KEYWORD2 +NewData KEYWORD2 + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp new file mode 100644 index 0000000000..b5b45b30e6 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp @@ -0,0 +1,225 @@ +/* + GPS_MTK.cpp - Ublox GPS library for Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + GPS configuration : Costum protocol + Baud rate : 38400 + + Methods: + Init() : GPS Initialization + Read() : Call this funcion as often as you want to ensure you read the incomming gps data + + Properties: + Lattitude : Lattitude * 10,000,000 (long value) + Longitude : Longitude * 10,000,000 (long value) + Altitude : Altitude * 100 (meters) (long value) + Ground_speed : Speed (m/s) * 100 (long value) + Ground_course : Course (degrees) * 100 (long value) + NewData : 1 when a new data is received. + You need to write a 0 to NewData when you read the data + Fix : 0: GPS NO FIX or 2D FIX, 1: 3D FIX. + +*/ + +#include "GPS_MTK.h" +#include +#include "WProgram.h" + + +// Constructors //////////////////////////////////////////////////////////////// +GPS_MTK_Class::GPS_MTK_Class() +{ +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void GPS_MTK_Class::Init(void) +{ + delay(200); + ck_a=0; + ck_b=0; + UBX_step=0; + NewData=0; + Fix=0; + PrintErrors=0; + GPS_timer=millis(); //Restarting timer... + // Initialize serial port + #if defined(__AVR_ATmega1280__) + Serial1.begin(38400); // Serial port 1 on ATMega1280 + #else + Serial.begin(38400); + #endif + Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); + //Serial.println("sent config string"); +} + +// optimization : This code don´t wait for data, 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_ubx_gps() to parse and update the GPS info. +void GPS_MTK_Class::Read(void) +{ + static unsigned long GPS_timer=0; + byte data; + int numc; + + #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... + numc = Serial1.available(); + #else + numc = Serial.available(); + #endif + if (numc > 0) + for (int i=0;i Bad FIX state + if ((millis() - GPS_timer)>2000) + { + Fix = 0; + if (PrintErrors) + Serial.println("ERR:GPS_TIMEOUT!!"); + } +} + +/**************************************************************** + * + ****************************************************************/ +// Private Methods ////////////////////////////////////////////////////////////// +void GPS_MTK_Class::parse_ubx_gps(void) +{ + int j; +//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. +//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. + if(UBX_class==0x01) + { + switch(UBX_id)//Checking the UBX ID + { + + + case 0x05: //ID Custom + j=0; + Lattitude= join_4_bytes(&UBX_buffer[j]) * 10; // lon*10,000,000 + j+=4; + Longitude = join_4_bytes(&UBX_buffer[j]) * 10; // lat*10000000 + j+=4; + Altitude = join_4_bytes(&UBX_buffer[j]); // MSL + j+=4; + Ground_Speed = join_4_bytes(&UBX_buffer[j]); + j+=4; + Ground_Course = join_4_bytes(&UBX_buffer[j]) / 10000; // Heading 2D + j+=4; + NumSats=UBX_buffer[j]; + j++; + Fix=UBX_buffer[j]; + if (Fix==3) + Fix = 1; + else + Fix = 0; + j++; + Time = join_4_bytes(&UBX_buffer[j]); + NewData=1; + break; + + } + } +} + + +/**************************************************************** + * + ****************************************************************/ + // Join 4 bytes into a long +long GPS_MTK_Class::join_4_bytes(unsigned char Buffer[]) +{ + union long_union { + int32_t dword; + uint8_t byte[4]; +} longUnion; + + longUnion.byte[3] = *Buffer; + longUnion.byte[2] = *(Buffer+1); + longUnion.byte[1] = *(Buffer+2); + longUnion.byte[0] = *(Buffer+3); + return(longUnion.dword); +} + +/**************************************************************** + * + ****************************************************************/ +// checksum algorithm +void GPS_MTK_Class::ubx_checksum(byte ubx_data) +{ + ck_a+=ubx_data; + ck_b+=ck_a; +} + +GPS_MTK_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h new file mode 100644 index 0000000000..3023b112ab --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h @@ -0,0 +1,49 @@ +#ifndef GPS_MTK_h +#define GPS_MTK_h + +#include + +#define UBX_MAXPAYLOAD 60 + +class GPS_MTK_Class +{ + private: + // Internal variables + uint8_t ck_a; // Packet checksum + uint8_t ck_b; + uint8_t UBX_step; + uint8_t UBX_class; + uint8_t UBX_id; + uint8_t UBX_payload_length_hi; + uint8_t UBX_payload_length_lo; + uint8_t UBX_payload_counter; + uint8_t UBX_buffer[UBX_MAXPAYLOAD]; + uint8_t UBX_ck_a; + uint8_t UBX_ck_b; + long GPS_timer; + long UBX_ecefVZ; + void parse_ubx_gps(); + void ubx_checksum(unsigned char ubx_data); + long join_4_bytes(unsigned char Buffer[]); + + public: + // Methods + GPS_MTK_Class(); + void Init(); + void Read(); + // Properties + long Time; //GPS Millisecond Time of Week + long Lattitude; // Geographic coordinates + long Longitude; + long Altitude; + long Ground_Speed; + long Ground_Course; + uint8_t NumSats; // Number of visible satelites + uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) + uint8_t NewData; // 1:New GPS Data + uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) +}; + +extern GPS_MTK_Class GPS; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde new file mode 100644 index 0000000000..6fa5a1cc2f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -0,0 +1,47 @@ +/* + Example of GPS MTK library. + Code by Jordi Mu�oz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) + and with standard ATMega168 and ATMega328 on Serial Port 0 +*/ + +#include // UBLOX GPS Library + +#define T6 1000000 +#define T7 10000000 + +void setup() +{ + Serial.begin(38400); + Serial.println("GPS MTK library test"); + GPS.Init(); // GPS Initialization + delay(1000); +} +void loop() +{ + GPS.Read(); + if (GPS.NewData) // New GPS data? + { + Serial.print("GPS:"); + Serial.print(" Lat:"); + Serial.print((float)GPS.Lattitude/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((float)GPS.Ground_Speed/100.0, DEC); + Serial.print(" COG:"); + Serial.print(GPS.Ground_Course/100.0, DEC); + Serial.print(" SAT:"); + Serial.print((int)GPS.NumSats); + Serial.print(" FIX:"); + Serial.print((int)GPS.Fix); + Serial.print(" TIM:"); + Serial.print(GPS.Time); + Serial.println(); + GPS.NewData = 0; // We have readed the data + } + delay(20); +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt new file mode 100644 index 0000000000..719ed917a1 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt @@ -0,0 +1,16 @@ +GPS KEYWORD1 +GPS_MTK KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Time KEYWORD2 +Lattitude KEYWORD2 +Longitude KEYWORD2 +Altitude KEYWORD2 +Ground_Speed KEYWORD2 +Ground_Course KEYWORD2 +Speed_3d KEYWORD2 +NumSats KEYWORD2 +Fix KEYWORD2 +NewData KEYWORD2 + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp new file mode 100644 index 0000000000..0289b4adc6 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp @@ -0,0 +1,272 @@ +/* + GPS_NMEA.cpp - Generic NMEA GPS library for Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + GPS configuration : NMEA protocol + Baud rate : 38400 + NMEA Sentences : + $GPGGA : Global Positioning System Fix Data + $GPVTG : Ttack and Ground Speed + + Methods: + Init() : GPS Initialization + Read() : 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) + Altitude : Altitude * 1000 (milimeters) (long value) + Ground_speed : Speed (m/s) * 100 (long value) + Ground_course : Course (degrees) * 100 (long value) + Type : 2 (This indicate that we are using the Generic NMEA library) + NewData : 1 when a new data is received. + You need to write a 0 to NewData when you read the data + Fix : >=1: GPS FIX, 0: No Fix (normal logic) + Quality : 0 = No Fix + 1 = Bad (Num sats < 5) + 2 = Poor + 3 = Medium + 4 = Good + + NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) +*/ + +#include "GPS_NMEA.h" + +#include +#include "WProgram.h" + + +// Constructors //////////////////////////////////////////////////////////////// +GPS_NMEA_Class::GPS_NMEA_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void GPS_NMEA_Class::Init(void) +{ + Type = 2; + GPS_checksum_calc = false; + bufferidx = 0; + NewData=0; + Fix=0; + Quality=0; + PrintErrors=0; + // Initialize serial port + #if defined(__AVR_ATmega1280__) + Serial1.begin(38400); // Serial port 1 on ATMega1280 + #else + Serial.begin(38400); + #endif +} + +// This code don´t wait for data, only proccess the data available on serial port +// We can call this function on the main loop (50Hz loop) +// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. +void GPS_NMEA_Class::Read(void) +{ + char c; + int numc; + int i; + + + #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... + numc = Serial1.available(); + #else + numc = Serial.available(); + #endif + if (numc > 0) + for (i=0;i Convert to decimal + Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5) + parseptr = strchr(parseptr, ',')+1; + // + if (*parseptr=='S') + Lattitude = -1*Lattitude; // South Lattitudes are negative + // + parseptr = strchr(parseptr, ',')+1; + // W Longitudes are Negative + aux_deg = parsedecimal(parseptr,3); // degrees + aux_min = parsenumber(parseptr+3,4); // minutes (sexagesimal) + Longitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) + //Longitude = -1*Longitude; // This Assumes that we are in W longitudes... + parseptr = strchr(parseptr, ',')+1; + // + if (*parseptr=='W') + Longitude = -1*Longitude; // West Longitudes are negative + // + parseptr = strchr(parseptr, ',')+1; + Fix = parsedecimal(parseptr,1); + parseptr = strchr(parseptr, ',')+1; + NumSats = parsedecimal(parseptr,2); + parseptr = strchr(parseptr, ',')+1; + HDOP = parsenumber(parseptr,1); // HDOP * 10 + parseptr = strchr(parseptr, ',')+1; + Altitude = parsenumber(parseptr,1)*100; // Altitude in decimeters*100 = milimeters + if (Fix < 1) + Quality = 0; // No FIX + else if(NumSats<5) + Quality = 1; // Bad (Num sats < 5) + else if(HDOP>30) + Quality = 2; // Poor (HDOP > 30) + else if(HDOP>25) + Quality = 3; // Medium (HDOP > 25) + else + Quality = 4; // Good (HDOP < 25) + } + else + { + if (PrintErrors) + Serial.println("GPSERR: Checksum error!!"); + } + } + } + else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG + //Serial.println(buffer); + if (buffer[bufferidx-4]=='*'){ // Check for the "*" character + NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters + if (GPS_checksum == NMEA_check){ // Checksum validation + parseptr = strchr(buffer, ',')+1; + Ground_Course = parsenumber(parseptr,2); // Ground course in degrees * 100 + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + parseptr = strchr(parseptr, ',')+1; + Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100) + //GPS_line = true; + } + else + { + if (PrintErrors) + Serial.println("GPSERR: Checksum error!!"); + } + } + } + else + { + bufferidx = 0; + if (PrintErrors) + Serial.println("GPSERR: Bad sentence!!"); + } +} + + +/**************************************************************** + * + ****************************************************************/ + // Parse hexadecimal numbers +byte GPS_NMEA_Class::parseHex(char c) { + if (c < '0') + return (0); + if (c <= '9') + return (c - '0'); + if (c < 'A') + return (0); + if (c <= 'F') + return ((c - 'A')+10); +} + +// Decimal number parser +long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) { + long d = 0; + byte i; + + i = num_car; + while ((str[0] != 0)&&(i>0)) { + if ((str[0] > '9') || (str[0] < '0')) + return d; + d *= 10; + d += str[0] - '0'; + str++; + i--; + } + return d; +} + +// Function to parse fixed point numbers (numdec=number of decimals) +long GPS_NMEA_Class::parsenumber(char *str,byte numdec) { + long d = 0; + byte ndec = 0; + + while (str[0] != 0) { + if (str[0] == '.'){ + ndec = 1; + } + else { + if ((str[0] > '9') || (str[0] < '0')) + return d; + d *= 10; + d += str[0] - '0'; + if (ndec > 0) + ndec++; + if (ndec > numdec) // we reach the number of decimals... + return d; + } + str++; + } + return d; +} + +GPS_NMEA_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h new file mode 100644 index 0000000000..f144a35e27 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h @@ -0,0 +1,46 @@ +#ifndef GPS_NMEA_h +#define GPS_NMEA_h + +#include + +#define GPS_BUFFERSIZE 120 + +class GPS_NMEA_Class +{ + private: + // Internal variables + uint8_t GPS_checksum; + uint8_t GPS_checksum_calc; + char buffer[GPS_BUFFERSIZE]; + int bufferidx; + + void parse_nmea_gps(void); + uint8_t parseHex(char c); + long parsedecimal(char *str,uint8_t num_car); + long parsenumber(char *str,uint8_t numdec); + + public: + // Methods + GPS_NMEA_Class(); + void Init(); + void Read(); + // Properties + long Time; //GPS Millisecond Time of Week + long Lattitude; // Geographic coordinates + long Longitude; + long Altitude; + long Ground_Speed; + long Speed_3d; //Speed (3-D) + long Ground_Course; + uint8_t Type; // Type of GPS (library used) + uint8_t NumSats; // Number of visible satelites + uint8_t Fix; // >=1:GPS FIX 0:No FIX (normal logic) + uint8_t Quality; // GPS Signal quality + uint8_t NewData; // 1:New GPS Data + uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) + int HDOP; // HDOP +}; + +extern GPS_NMEA_Class GPS; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde new file mode 100644 index 0000000000..f7a93d0c58 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -0,0 +1,42 @@ +/* + Example of GPS NMEA library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) + and with standard ATMega168 and ATMega328 on Serial Port 0 +*/ + +#include // NMEA GPS Library + +void setup() +{ + Serial.begin(57600); + Serial.println("GPS NMEA library test"); + GPS.Init(); // GPS Initialization + delay(1000); +} +void loop() +{ + GPS.Read(); + if (GPS.NewData) // New GPS data? + { + Serial.print("GPS:"); + Serial.print(" Time:"); + Serial.print(GPS.Time); + Serial.print(" Fix:"); + Serial.print((int)GPS.Fix); + Serial.print(" Lat:"); + Serial.print(GPS.Lattitude); + Serial.print(" Lon:"); + Serial.print(GPS.Longitude); + Serial.print(" Alt:"); + Serial.print(GPS.Altitude/1000.0); + Serial.print(" Speed:"); + Serial.print(GPS.Ground_Speed/100.0); + Serial.print(" Course:"); + Serial.print(GPS.Ground_Course/100.0); + Serial.println(); + GPS.NewData = 0; // We have readed the data + } + delay(20); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt new file mode 100644 index 0000000000..f2d3a7653d --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt @@ -0,0 +1,18 @@ +GPS KEYWORD1 +GPS_NMEA KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Type KEYWORD2 +Time KEYWORD2 +Lattitude KEYWORD2 +Longitude KEYWORD2 +Altitude KEYWORD2 +Ground_Speed KETWORD2 +Ground_Course KEYWORD2 +Speed_3d KEYWORD2 +NumSats KEYWORD2 +Fix KEYWORD2 +NewData KEYWORD2 +Quality KEYWORD2 + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp new file mode 100644 index 0000000000..d1f29b336f --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp @@ -0,0 +1,274 @@ +/* + GPS_UBLOX.cpp - Ublox GPS library for Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + GPS configuration : Ublox protocol + Baud rate : 38400 + Active messages : + NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet + NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet + NAV - STATUS Receiver Navigation Status + or + NAV - SOL Navigation Solution Information + + Methods: + Init() : GPS Initialization + Read() : Call this funcion as often as you want to ensure you read the incomming gps data + + Properties: + Lattitude : Lattitude * 10,000,000 (long value) + Longitude : Longitude * 10,000,000 (long value) + Altitude : Altitude * 100 (meters) (long value) + Ground_speed : Speed (m / s) * 100 (long value) + Ground_course : Course (degrees) * 100 (long value) + NewData : 1 when a new data is received. + You need to write a 0 to NewData when you read the data + Fix : 1: GPS FIX, 0: No Fix (normal logic) + +*/ + +#include "GPS_UBLOX.h" + +#include +#include "WProgram.h" + + +// Constructors //////////////////////////////////////////////////////////////// +GPS_UBLOX_Class::GPS_UBLOX_Class() +{ +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void GPS_UBLOX_Class::Init(void) +{ + ck_a = 0; + ck_b = 0; + UBX_step = 0; + NewData = 0; + Fix = 0; + PrintErrors = 0; + GPS_timer = millis(); // Restarting timer... + // Initialize serial port + #if defined(__AVR_ATmega1280__) + Serial1.begin(38400); // Serial port 1 on ATMega1280 + #else + Serial.begin(38400); + #endif +} + +// optimization : This code don´t wait for data, 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_ubx_gps() to parse and update the GPS info. +void GPS_UBLOX_Class::Read(void) +{ + static unsigned long GPS_timer = 0; + byte data; + int numc; + + #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... + numc = Serial1.available(); + #else + numc = Serial.available(); + #endif + if (numc > 0) + for (int i = 0; i < numc; i++) // Process bytes received + { + #if defined(__AVR_ATmega1280__) + data = Serial1.read(); + #else + data = Serial.read(); + #endif + switch(UBX_step) // Normally we start from zero. This is a state machine + { + case 0: + if(data == 0xB5) // UBX sync char 1 + UBX_step++; // OH first data packet is correct, so jump to the next step + break; + case 1: + if(data == 0x62) // UBX sync char 2 + UBX_step++; // ooh! The second data packet is correct, jump to the step 2 + else + UBX_step = 0; // Nop, is not correct so restart to step zero and try again. + break; + case 2: + UBX_class = data; + ubx_checksum(UBX_class); + UBX_step++; + break; + case 3: + UBX_id = data; + ubx_checksum(UBX_id); + UBX_step++; + break; + case 4: + UBX_payload_length_hi = data; + ubx_checksum(UBX_payload_length_hi); + UBX_step++; + // We check if the payload lenght is valid... + if (UBX_payload_length_hi >= UBX_MAXPAYLOAD) + { + if (PrintErrors) + Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!"); + UBX_step = 0; // Bad data, so restart to step zero and try again. + ck_a = 0; + ck_b = 0; + } + break; + case 5: + UBX_payload_length_lo = data; + ubx_checksum(UBX_payload_length_lo); + UBX_step++; + UBX_payload_counter = 0; + break; + case 6: // Payload data read... + if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length + { + UBX_buffer[UBX_payload_counter] = data; + ubx_checksum(data); + UBX_payload_counter++; + if (UBX_payload_counter == UBX_payload_length_hi) + UBX_step++; + } + break; + case 7: + UBX_ck_a = data; // First checksum byte + UBX_step++; + break; + case 8: + UBX_ck_b = data; // Second checksum byte + + // We end the GPS read... + if((ck_a == UBX_ck_a) && (ck_b == UBX_ck_b)) // Verify the received checksum with the generated checksum.. + parse_ubx_gps(); // Parse the new GPS packet + else + { + if (PrintErrors) + Serial.println("ERR:GPS_CHK!!"); + } + // Variable initialization + UBX_step = 0; + ck_a = 0; + ck_b = 0; + GPS_timer = millis(); // Restarting timer... + break; + } + } // End for... + // If we don´t receive GPS packets in 2 seconds => Bad FIX state + if ((millis() - GPS_timer) > 2000) + { + Fix = 0; + if (PrintErrors) + Serial.println("ERR:GPS_TIMEOUT!!"); + } +} + +/**************************************************************** + * + ****************************************************************/ +// Private Methods ////////////////////////////////////////////////////////////// +void GPS_UBLOX_Class::parse_ubx_gps(void) +{ + int j; +//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. +//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. + if(UBX_class == 0x01) + { + switch(UBX_id) //Checking the UBX ID + { + case 0x02: // ID NAV - POSLLH + j = 0; + Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week + j += 4; + Longitude = join_4_bytes(&UBX_buffer[j]); // lon * 10000000 + j += 4; + Lattitude = join_4_bytes(&UBX_buffer[j]); // lat * 10000000 + j += 4; + //Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm + j += 4; + Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm + Altitude /= 10.; + + // Rescale altitude to cm + //j+=4; + /* + hacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000; + j += 4; + vacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000; + j += 4; + */ + NewData = 1; + break; + case 0x03: //ID NAV - STATUS + //if(UBX_buffer[4] >= 0x03) + if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01)) + Fix = 1; // valid position + else + Fix = 0; // invalid position + break; + + case 0x06: //ID NAV - SOL + if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01)) + Fix = 1; // valid position + else + Fix = 0; // invalid position + UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]); // Vertical Speed in cm / s + NumSats = UBX_buffer[47]; // Number of sats... + break; + + case 0x12: // ID NAV - VELNED + j = 16; + Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm / s + j += 4; + Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm / s + j += 4; + Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg * 100000 + Ground_Course /= 1000; // Rescale heading to deg * 100 + j += 4; + /* + sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy + j += 4; + headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy + j += 4; + */ + break; + } + } +} + + +/**************************************************************** + * + ****************************************************************/ + // Join 4 bytes into a long +long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[]) +{ + union long_union { + int32_t dword; + uint8_t byte[4]; +} longUnion; + + longUnion.byte[0] = *Buffer; + longUnion.byte[1] = *(Buffer + 1); + longUnion.byte[2] = *(Buffer + 2); + longUnion.byte[3] = *(Buffer + 3); + return(longUnion.dword); +} + +/**************************************************************** + * + ****************************************************************/ +// Ublox checksum algorithm +void GPS_UBLOX_Class::ubx_checksum(byte ubx_data) +{ + ck_a += ubx_data; + ck_b += ck_a; +} + +GPS_UBLOX_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h new file mode 100644 index 0000000000..8a9e09e611 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h @@ -0,0 +1,50 @@ +#ifndef GPS_UBLOX_h +#define GPS_UBLOX_h + +#include + +#define UBX_MAXPAYLOAD 60 + +class GPS_UBLOX_Class +{ + private: + // Internal variables + uint8_t ck_a; // Packet checksum + uint8_t ck_b; + uint8_t UBX_step; + uint8_t UBX_class; + uint8_t UBX_id; + uint8_t UBX_payload_length_hi; + uint8_t UBX_payload_length_lo; + uint8_t UBX_payload_counter; + uint8_t UBX_buffer[UBX_MAXPAYLOAD]; + uint8_t UBX_ck_a; + uint8_t UBX_ck_b; + long GPS_timer; + long UBX_ecefVZ; + void parse_ubx_gps(); + void ubx_checksum(unsigned char ubx_data); + long join_4_bytes(unsigned char Buffer[]); + + public: + // Methods + GPS_UBLOX_Class(); + void Init(); + void Read(); + // Properties + long Time; //GPS Millisecond Time of Week + long Lattitude; // Geographic coordinates + long Longitude; + long Altitude; + long Ground_Speed; + long Speed_3d; //Speed (3-D) + long Ground_Course; + uint8_t NumSats; // Number of visible satelites + uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) + uint8_t NewData; // 1:New GPS Data + uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) +}; + +extern GPS_UBLOX_Class GPS; + +#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde new file mode 100644 index 0000000000..99975c7334 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -0,0 +1,42 @@ +/* + Example of GPS UBLOX library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Works with Ardupilot Mega Hardware (GPS on Serial Port1) + and with standard ATMega168 and ATMega328 on Serial Port 0 +*/ + +#include // UBLOX GPS Library + +void setup() +{ + Serial.begin(57600); + Serial.println("GPS UBLOX library test"); + GPS.Init(); // GPS Initialization + delay(1000); +} +void loop() +{ + GPS.Read(); + if (GPS.NewData) // New GPS data? + { + Serial.print("GPS:"); + Serial.print(" Time:"); + Serial.print(GPS.Time); + Serial.print(" Fix:"); + Serial.print((int)GPS.Fix); + Serial.print(" Lat:"); + Serial.print(GPS.Lattitude); + Serial.print(" Lon:"); + Serial.print(GPS.Longitude); + Serial.print(" Alt:"); + Serial.print(GPS.Altitude/1000.0); + Serial.print(" Speed:"); + Serial.print(GPS.Ground_Speed/100.0); + Serial.print(" Course:"); + Serial.print(GPS.Ground_Course/100000.0); + Serial.println(); + GPS.NewData = 0; // We have readed the data + } + delay(20); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt new file mode 100644 index 0000000000..e211693803 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt @@ -0,0 +1,16 @@ +GPS KEYWORD1 +GPS_UBLOX KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Time KEYWORD2 +Lattitude KEYWORD2 +Longitude KEYWORD2 +Altitude KEYWORD2 +Ground_Speed KETWORD2 +Ground_Course KEYWORD2 +Speed_3d KEYWORD2 +NumSats KEYWORD2 +Fix KEYWORD2 +NewData KEYWORD2 + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp new file mode 100644 index 0000000000..25d49da553 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp @@ -0,0 +1,202 @@ +/* + JETI_Box.cpp, Version 1.0 beta + Oct 2010, by Uwe Gartmann + + + written for Arduino Mega / ArduPilot Mega + +*/ +extern "C" { + #include + #include "Print.h" + #include + #include + #include "wiring.h" +} + +#include +#define LCDMaxPos 32 +#define LCDBufferSize LCDMaxPos + 1 + +volatile uint8_t lcd[LCDBufferSize]; +volatile uint8_t buttons; +volatile uint8_t lastbuttons; +volatile uint8_t cursor; +volatile uint8_t sendpos; +volatile uint8_t dmyError; +volatile uint8_t dmyBit9; +volatile uint8_t dmyData; +volatile uint8_t ledA; +volatile uint8_t t0cntr; +//volatile long valX; + +ISR(USART3_RX_vect) //serial data available +{ + // catch response from Jetibox + dmyError = UCSR3A; + dmyBit9 = UCSR3B; + dmyData = UDR3; + if (!(dmyError & ((1<>4) xor 0x0F; + t0cntr = 10; // set 10ms delay + } + } + } +} + +ISR(USART3_UDRE_vect) //transmit buffer empty +/* + * sendpos = 0 -> start sending data with this interrupt enabled, start byte with 9.bit=0 + * sendpos = 1-32 -> send display data with 9.bit=1 + * sendpos = 33 -> send end byte with 9.bit=0 + * sendpos = 34 -> set sendpos=0 and disable this interrupt + */ +{ + switch (sendpos) + { + case 0: + // send start byte with 9.bit=0 + UCSR3B &= ~(1< 0) --valX; +} + +JETI_Box_class::JETI_Box_class() +{ +// Class constructor +} + +// Public Methods -------------------------------------------------------------------- +void JETI_Box_class::begin(void) +{ +#ifndef F_CPU + #define F_CPU 16000000 +#endif +#define _UBRR3 (F_CPU/8/9600-1) //when U2X0 is not set use divider 16 instead of 8 + + // Set serial interface + // Baudrate + UCSR3A = (1<>8); //high byte + UBRR3L=_UBRR3; //low byte + // Set data format: 9data#1, odd parity, 1stop bit + UCSR3C = (0< + #include "Print.h" + #include + #include + #include "wiring.h" +} + +#include +#define LCDLine1 1 +#define LCDLine2 17 +#define LCDMaxPos 32 +#define LCDBufferSize LCDMaxPos + 1 + +volatile uint8_t lcd[LCDBufferSize]; +volatile uint8_t buttons; +volatile uint8_t lastbuttons; +volatile uint8_t cursor; +volatile uint8_t sendpos; +volatile uint8_t dummy; + +ISR(USART3_RX_vect) //serial data available +{ + // catch response from Jetibox + if (!(UCSR3A & ((1<>4) xor 0x0F; + } + } + } +} + +ISR(USART3_UDRE_vect) //transmit buffer empty +/* + * sendpos = 0 -> start sending data with this interrupt enabled, start byte with 9.bit=0 + * sendpos = 1-32 -> send display data with 9.bit=1 + * sendpos = 33 -> send end byte with 9.bit=0 + * sendpos = 34 -> set sendpos=0 and disable this interrupt + */ +{ + switch (sendpos) + { + case 0: + // send start byte with 9.bit=0 + UCSR3B &= ~(1<>8); //high byte + UBRR3L=_UBRR3; //low byte + + // Set data format: 9data#1, odd parity, 1stop bit + UCSR3C = (0< +//#include "Print.h" + +#define LCDLine1 1 +#define LCDLine2 17 + +#define JB_key_up 0b0010 +#define JB_key_right 0b0001 +#define JB_key_down 0b0100 +#define JB_key_left 0b1000 + +class JETI_Box_class : public Print { +public: + uint8_t readbuttons(void); + //long checkvalue(long v); + virtual void write(uint8_t c); + using Print::write; // pull in write(str) and write(buf, size) from Print + JETI_Box_class(); + void begin(); + void setcursor(uint8_t p); + void clear(); + void clear(uint8_t l); +}; + +extern JETI_Box_class JB; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt new file mode 100644 index 0000000000..699b80af69 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt @@ -0,0 +1,8 @@ +JETI_Box KEYWORD1 +Begin KEYWORD2 +Refresh KEYWORD2 +Write KEYWORD2 +Print KEYWORD2 +Line1 KEYWORD2 +Line2 KEYWORD2 +Keys KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp new file mode 100644 index 0000000000..d9b4882b21 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp @@ -0,0 +1,327 @@ +#ifdef __AVR_ATmega1280__ +/* + APM2_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + RC Input : PPM signal on IC4 pin + RC Output : 11 Servo outputs (standard 20ms frame) + + Methods: + Init() : Initialization of interrupts an Timers + OutpuCh(ch, pwm) : Output value to servos (range : 900 - 2100us) ch = 0..10 + InputCh(ch) : Read a channel input value. ch = 0..7 + GetState() : Returns the state of the input. 1 => New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ + +#include "APM2_RC.h" + +#define REVERSE 3050 + +// Variable definition for Input Capture interrupt +volatile uint16_t ICR4_old; +volatile uint8_t PPM_Counter = 0; +volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; + +// Constructors //////////////////////////////////////////////////////////////// +APM2_RC::APM2_RC() +{ + _direction_mask = 255; // move to super class + +} + +void +APM2_RC::init() +{ + // Init PWM Timer 1 + pinMode(11, OUTPUT); // (PB5 / OC1A) + pinMode(12, OUTPUT); // OUT2 (PB6 / OC1B) + pinMode(13, OUTPUT); // OUT3 (PB7 / OC1C) + + // Timer 3 + pinMode(2, OUTPUT); // OUT7 (PE4 / OC3B) + pinMode(3, OUTPUT); // OUT6 (PE5 / OC3C) + pinMode(4, OUTPUT); // (PE3 / OC3A) + + // Timer 5 + pinMode(44, OUTPUT); // OUT1 (PL5 / OC5C) + pinMode(45, OUTPUT); // OUT0 (PL4 / OC5B) + pinMode(46, OUTPUT); // (PL3 / OC5A) + + // Init PPM input and PWM Timer 4 + pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input) + pinMode(7, OUTPUT); // OUT5 (PH4 / OC4B) + pinMode(8, OUTPUT); // OUT4 (PH5 / OC4C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1 << WGM11) | (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1)); // Please read page 131 of DataSheet, we are changing the registers settings of WGM11, COM1B1, COM1A1 to 1 thats all... + TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet + OCR1A = 3000; // PB5, none + //OCR1B = 3000; // PB6, OUT2 + //OCR1C = 3000; // PB7 OUT3 + ICR1 = 40000; // 50hz freq...Datasheet says (system_freq / prescaler) / target frequency. So (16000000hz / 8) / 50hz = 40000, + + // Init PWM Timer 3 + TCCR3A =((1 << WGM31) | (1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1)); + TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31); + OCR3A = 3000; // PE3, NONE + //OCR3B = 3000; // PE4, OUT7 + //OCR3C = 3000; // PE5, OUT6 + ICR3 = 40000; // 50hz freq + + + // Init PWM Timer 5 + TCCR5A =((1 << WGM51) | (1 << COM5A1) | (1 << COM5B1) | (1 << COM5C1)); + TCCR5B = (1 << WGM53) | (1 << WGM52) | (1 << CS51); + OCR5A = 3000; // PL3, + //OCR5B = 3000; // PL4, OUT0 + //OCR5C = 3000; // PL5, OUT1 + ICR5 = 40000; // 50hz freq + + + // Init PPM input and PWM Timer 4 + TCCR4A = ((1 << WGM40) | (1 << WGM41) | (1 << COM4C1) | (1 << COM4B1) | (1 << COM4A1)); + TCCR4B = ((1 << WGM43) | (1 << WGM42) | (1 << CS41) | (1 << ICES4)); + OCR4A = 40000; // /50hz freq. + //OCR4B = 3000; // PH4, OUT5 + //OCR4C = 3000; // PH5, OUT4 + + //TCCR4B |=(1<= radio_trim[i]) + servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0; + else + servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0; + } + servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; + servo_in[CH3] = constrain(servo_out[CH3], 0, 100); +} + +void +APM2_RC::output() +{ + uint16_t out; + for (uint8_t i = 0; i < 8; i++){ + if (i == 3) continue; + if(radio_in[i] >= radio_trim[i]) + out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; + else + out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; + set_ch_pwm(i, out); + } + + out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; + out += radio_min[CH3]; + set_ch_pwm(CH3, out); +} + +void +APM2_RC::trim() +{ + uint8_t temp = _mix_mode; + _mix_mode = 0; + read(); + _mix_mode = temp; + + // Store the trim values + // --------------------- + for (int y = 0; y < 8; y++) + radio_trim[y] = radio_in[y]; +} +void +APM2_RC::twitch_servos(uint8_t times) +{ + // todo +} +void +APM2_RC::set_ch_pwm(uint8_t ch, uint16_t pwm) +{ + //pwm = constrain(pwm, MIN_PULSEWIDTH, MAX_PULSEWIDTH); + + switch(ch){ + case 0: + //Serial.print("\tpwm out "); + //Serial.print(pwm,DEC); + + if((_direction_mask & 1) == 0 ) + pwm = REVERSE - pwm; + + //Serial.print("\tpwm out "); + //Serial.println(pwm,DEC); + + OCR5B = pwm << 1; + break; // ch0 + + case 1: + if((_direction_mask & 2) == 0 ) + pwm = REVERSE - pwm; + OCR5C = pwm << 1; + break; // ch0 + + case 2: + if((_direction_mask & 4) == 0 ) + pwm = REVERSE - pwm; + OCR1B = pwm << 1; + break; // ch0 + + case 3: + if((_direction_mask & 8) == 0 ) + pwm = REVERSE - pwm; + OCR1C = pwm << 1; + break; // ch0 + + case 4: + if((_direction_mask & 16) == 0 ) + pwm = REVERSE - pwm; + OCR4C = pwm << 1; + break; // ch0 + + case 5: + if((_direction_mask & 32) == 0 ) + pwm = REVERSE - pwm; + OCR4B = pwm << 1; + break; // ch0 + + case 6: + if((_direction_mask & 64) == 0 ) + pwm = REVERSE - pwm; + OCR3C = pwm << 1; + break; // ch0 + + case 7: + if((_direction_mask & 128) == 0 ) + pwm = REVERSE - pwm; + OCR3B = pwm << 1; + break; // ch0 + + case 8: + OCR5A = pwm << 1; + break; // ch0 + + case 9: + OCR1A = pwm << 1; + break; // ch0 + + case 10: + OCR3A = pwm << 1; + break; // ch0 + } +} + +/**************************************************** + Input Capture Interrupt ICP4 => PPM signal read + ****************************************************/ +ISR(TIMER4_CAPT_vect) +{ + uint16_t pulse; + uint16_t pulse_width; + + pulse = ICR4; + if (pulse < ICR4_old){ // Take care of the overflow of Timer4 (TOP = 40000) + pulse_width = (pulse + 40000) - ICR4_old; // Calculating pulse + }else{ + pulse_width = pulse - ICR4_old; // Calculating pulse + } + ICR4_old = pulse; + + + if (pulse_width > 8000){ // SYNC pulse + PPM_Counter = 0; + } else { + //PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) + raw[PPM_Counter++] = pulse_width >> 1; // Saving pulse. + } +} + + + +// InstantPWM implementation +// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use +void APM2_RC::force_out_0_1(void) +{ + if (TCNT5 > 5000) // We take care that there are not a pulse in the output + TCNT5 = 39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 +} +// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use +void APM2_RC::force_out_2_3(void) +{ + if (TCNT1 > 5000) + TCNT1 = 39990; +} + +// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use +void APM2_RC::force_out_6_7(void) +{ + if (TCNT3 > 5000) + TCNT3 = 39990; +} +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h new file mode 100644 index 0000000000..5947a91130 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h @@ -0,0 +1,38 @@ +#ifndef APM2_RC_h +#define APM2_RC_h + +#include +#include "WProgram.h" +#include "RC.h" + +class APM2_RC : public RC +{ + public: + APM2_RC(); + void init(); + void read(); + void output(); + void set_ch_pwm(uint8_t ch, uint16_t pwm); + void trim(); + void twitch_servos(uint8_t times); + + void force_out_0_1(void); + void force_out_2_3(void); + void force_out_6_7(void); + + int16_t radio_in[8]; + int16_t radio_min[8]; + int16_t radio_trim[8]; + int16_t radio_max[8]; + + int16_t servo_in[8]; + float servo_out[8]; + + private: + uint16_t _timer_out; +}; + +#endif + + +//volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp new file mode 100644 index 0000000000..1915285790 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp @@ -0,0 +1,329 @@ +/* + AP_RC.cpp - Radio library for Arduino + Code by Jason Short. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + +*/ + +#include "AP_RC.h" +#include +#define REVERSE 3050 + +// Variable definition for interrupt +volatile uint16_t timer1count = 0; +volatile uint16_t timer2count = 0; +volatile uint16_t timer3count = 0; +volatile uint16_t timer4count = 0; + +volatile int16_t timer1diff = 1500 * 2; +volatile int16_t timer2diff = 1500 * 2; +volatile int16_t timer3diff = 1100 * 2; +volatile int16_t timer4diff = 1500 * 2; + +//volatile uint16_t raw[8]; + +#define CH1_READ 1 +#define CH2_READ 2 +#define CH3_READ 4 +#define CH4_READ 8 + +volatile int8_t _rc_ch_read = 0; +volatile uint8_t _timer_ovf_a = 0; +volatile uint8_t _timer_ovf_b = 0; +volatile uint8_t _timer_ovf = 0; + + +AP_RC::AP_RC() +{ + _direction_mask = 255; // move to super class + pinMode(2,INPUT); // PD2 - INT0 - CH 1 in + pinMode(3,INPUT); // PD3 - INT1 - CH 2 in + pinMode(11,INPUT); // PB3 - MOSI/OC2 - CH 3 in + pinMode(13,INPUT); // PB5 - SCK - CH 4 in + + pinMode(10,OUTPUT); // PB2 - OC1B - CH 1 out + pinMode(8, OUTPUT); // PB0 - AIN1 - CH 3 out + pinMode(9, OUTPUT); // PB1 - OC1A - CH 2 out + DDRC |= B00010000; // PC4 - - CH 4 out +} + +void +AP_RC::init() +{ + // enable pin change interrupt 2 - PCINT23..16 + PCICR = _BV(PCIE2); + // enable pin change interrupt 0 - PCINT7..0 + PCICR |= _BV(PCIE0); + // enable in change interrupt on PB5 (digital pin 13) + PCMSK0 = _BV(PCINT3) | _BV(PCINT5); + // enable pin change interrupt on PD2,PD3 (digital pin 2,3) + PCMSK2 = _BV(PCINT18) | _BV(PCINT19); + + // Timer 1 + TCCR1A = ((1 << WGM11) | (1 << COM1B1) | (1 << COM1A1)); + TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); + // Loop value + ICR1 = 40000; + + // Throttle; + // Setting up the Timer 2 - 8 bit timer + TCCR2A = 0x0; // Normal Mode + TCCR2B = _BV(CS21) |_BV(CS20); //prescaler 32, at 16mhz (32/16) = 2, the counter will increment 1 every 2us + + // trim out the radio + for(int c = 0; c < 50; c++){ + delay(20); + read(); + } + + trim(); + + for(int y = 0; y < 4; y++) { + set_ch_pwm(y, radio_trim[y]); + } + + // enable throttle and Ch4 output + TIMSK1 |= _BV(ICIE1); // Timer / Counter1, Input Capture Interrupt Enable // PB0 - output throttle + TIMSK2 = _BV(TOIE1) | _BV(OCIE2A) | _BV(OCIE2B); // Timer / Counter2 Compare Match A +} + +void +AP_RC::read() +{ + if((_direction_mask & 1) == 0 ) + timer1diff = REVERSE - timer1diff; + if((_direction_mask & 2) == 0 ) + timer2diff = REVERSE - timer2diff; + if((_direction_mask & 4) == 0 ) + timer3diff = REVERSE - timer3diff; + if((_direction_mask & 8) == 0 ) + timer4diff = REVERSE - timer4diff; + + if(_mix_mode == 1){ + // elevons + int16_t ailerons = (timer1diff - radio_trim[CH1]) * .3; + int16_t elevator = (timer2diff - radio_trim[CH2]) * .7; + + radio_in[CH1] = (elevator - ailerons); // left + radio_in[CH2] = (elevator + ailerons); // right + + radio_in[CH1] += radio_trim[CH1]; + radio_in[CH2] += radio_trim[CH2]; + + //Serial.print("radio_in[CH1] "); + //Serial.print(radio_in[CH1],DEC); + //Serial.print(" \tradio_in[CH2] "); + //Serial.println(radio_in[CH2],DEC); + + }else{ + // normal + radio_in[CH1] = timer1diff; + radio_in[CH2] = timer2diff; + } + + radio_in[CH3] = (float)radio_in[CH3] * .9 + timer3diff * .1; + radio_in[CH4] = timer4diff; + + check_throttle_failsafe(radio_in[CH3]); + + // output servos + for (uint8_t i = 0; i < 4; i++){ + if (i == 3) continue; + if(radio_in[i] >= radio_trim[i]) + servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0; + else + servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0; + } + servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; + servo_in[CH3] = constrain(servo_out[CH3], 0, 100); +} + +void +AP_RC::output() +{ + uint16_t out; + for (uint8_t i = 0; i < 4; i++){ + if (i == 3) continue; + if(radio_in[i] >= radio_trim[i]) + out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; + else + out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; + set_ch_pwm(i, out); + } + + out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; + out += radio_min[CH3]; + set_ch_pwm(CH3, out); +} + +void +AP_RC::trim() +{ + uint8_t temp = _mix_mode; + _mix_mode = 0; + read(); + _mix_mode = temp; + + radio_trim[CH1] = radio_in[CH1]; + radio_trim[CH2] = radio_in[CH2]; + radio_trim[CH3] = radio_in[CH3]; + radio_trim[CH4] = radio_in[CH4]; + + //Serial.print("trim "); + //Serial.println(radio_trim[CH1], DEC); +} + +void +AP_RC::twitch_servos(uint8_t times) +{ + while (times > 0){ + set_ch_pwm(CH1, radio_trim[CH1] + 100); + set_ch_pwm(CH2, radio_trim[CH2] + 100); + delay(400); + set_ch_pwm(CH1, radio_trim[CH1] - 100); + set_ch_pwm(CH2, radio_trim[CH2] - 100); + delay(200); + set_ch_pwm(CH1, radio_trim[CH1]); + set_ch_pwm(CH2, radio_trim[CH2]); + delay(30); + times--; + } +} + +void +AP_RC::set_ch_pwm(uint8_t ch, uint16_t pwm) +{ + switch(ch){ + case CH1: + if((_direction_mask & 1) == 0 ) + pwm = REVERSE - pwm; + pwm <<= 1; + OCR1A = pwm; + break; + + case CH2: + if((_direction_mask & 2) == 0 ) + pwm = REVERSE - pwm; + pwm <<= 1; + OCR1B = pwm; + break; + + case CH3: + if((_direction_mask & 4) == 0 ) + pwm = REVERSE - pwm; + // Jason's fancy 2µs hack + _timer_out = pwm % 512; + _timer_ovf_a = pwm / 512; + _timer_out >>= 1; + OCR2A = _timer_out; + break; + + case CH4: + if((_direction_mask & 8) == 0 ) + pwm = REVERSE - pwm; + _timer_out = pwm % 512; + _timer_ovf_b = pwm / 512; + _timer_out >>= 1; + OCR2B = _timer_out; + break; + } +} + + + +// radio PWM input timers +ISR(PCINT2_vect) { + int cnt = TCNT1; + if(PIND & B00000100){ // ch 1 (pin 2) is high + if ((_rc_ch_read & CH1_READ) != CH1_READ){ + _rc_ch_read |= CH1_READ; + timer1count = cnt; + } + }else if ((_rc_ch_read & CH1_READ) == CH1_READ){ // ch 1 (pin 2) is Low, and we were reading + _rc_ch_read &= B11111110; + if (cnt < timer1count) // Timer1 reset during the read of this pulse + timer1diff = (cnt + 40000 - timer1count) >> 1; // Timer1 TOP = 40000 + else + timer1diff = (cnt - timer1count) >> 1; + } + + if(PIND & B00001000){ // ch 2 (pin 3) is high + if ((_rc_ch_read & CH2_READ) != CH2_READ){ + _rc_ch_read |= CH2_READ; + timer2count = cnt; + } + }else if ((_rc_ch_read & CH2_READ) == CH2_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11111101; + if (cnt < timer2count) // Timer1 reset during the read of this pulse + timer2diff = (cnt + 40000 - timer2count) >> 1; // Timer1 TOP = 40000 + else + timer2diff = (cnt - timer2count) >> 1; + } +} + +ISR(PCINT0_vect) +{ + int cnt = TCNT1; + if(PINB & 8){ // pin 11 + if ((_rc_ch_read & CH3_READ) != CH3_READ){ + _rc_ch_read |= CH3_READ; + timer3count = cnt; + } + }else if ((_rc_ch_read & CH3_READ) == CH3_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11111011; + if (cnt < timer3count) // Timer1 reset during the read of this pulse + timer3diff = (cnt + 40000 - timer3count) >> 1; // Timer1 TOP = 40000 + else + timer3diff = (cnt - timer3count) >> 1; + } + + if(PINB & 32){ // pin 13 + if ((_rc_ch_read & CH4_READ) != CH4_READ){ + _rc_ch_read |= CH4_READ; + timer4count = cnt; + } + }else if ((_rc_ch_read & CH4_READ) == CH4_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11110111; + if (cnt < timer4count) // Timer1 reset during the read of this pulse + timer4diff = (cnt + 40000 - timer4count) >> 1; // Timer1 TOP = 40000 + else + timer4diff = (cnt - timer4count) >> 1; + } +} + + + +// Throttle Timer Interrupt +// ------------------------ +ISR(TIMER1_CAPT_vect) // Timer/Counter1 Capture Event +{ + //This is a timer 1 interrupts, executed every 20us + PORTB |= B00000001; //Putting the pin high! + PORTC |= B00010000; //Putting the pin high! + TCNT2 = 0; //restarting the counter of timer 2 + _timer_ovf = 0; +} + +ISR(TIMER2_OVF_vect) +{ + _timer_ovf++; +} + +ISR(TIMER2_COMPA_vect) // Timer/Counter2 Compare Match A +{ + if(_timer_ovf == _timer_ovf_a){ + PORTB &= B11111110; //Putting the pin low + } +} + +ISR(TIMER2_COMPB_vect) // Timer/Counter2 Compare Match B Rudder Servo +{ + if(_timer_ovf == _timer_ovf_b){ + PORTC &= B11101111; //Putting the pin low! + } +} + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h new file mode 100644 index 0000000000..8118f980fd --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h @@ -0,0 +1,32 @@ +#ifndef AP_RC_h +#define AP_RC_h + +#include +#include "WProgram.h" +#include "RC.h" + +class AP_RC : public RC +{ + public: + AP_RC(); + void init(); + void read(); + void output(); + void set_ch_pwm(uint8_t ch, uint16_t pwm); + void trim(); + void twitch_servos(uint8_t times); + + int16_t radio_in[4]; + int16_t radio_min[4]; + int16_t radio_trim[4]; + int16_t radio_max[4]; + + int16_t servo_in[4]; + float servo_out[4]; + + private: + uint16_t _timer_out; +}; + +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp new file mode 100644 index 0000000000..0f92dfd5b7 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp @@ -0,0 +1,84 @@ +/* + AP_RC.cpp - Radio library for Arduino + Code by Jason Short. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + +*/ + + + +#include "RC.h" + +/* +RC::RC()// : + _direction_mask(255) +{ +} +*/ + +// direction mask: 0 = normal, 1 = reversed servos +void +RC::set_channel_direction(uint8_t ch, int8_t dir) +{ + uint8_t bitflip = 1 << ch; + + if(dir == 1){ + _direction_mask |= bitflip; + }else{ + _direction_mask &= ~bitflip; + } +} + +void +RC::set_failsafe(uint16_t v) +{ + _fs_value = v; +} + +void +RC::set_mix_mode(uint8_t m) +{ + _mix_mode = m; +} + + +void +RC::check_throttle_failsafe(uint16_t throttle) +{ + //check for failsafe and debounce funky reads + // ------------------------------------------ + if (throttle < _fs_value){ + // we detect a failsafe from radio + // throttle has dropped below the mark + _fs_counter++; + if (_fs_counter == 9){ + + }else if(_fs_counter == 10) { + failsafe = 1; + }else if (_fs_counter > 10){ + _fs_counter = 11; + } + + }else if(_fs_counter > 0){ + // we are no longer in failsafe condition + // but we need to recover quickly + _fs_counter--; + if (_fs_counter > 3){ + _fs_counter = 3; + } + if (_fs_counter == 1){ + + }else if(_fs_counter == 0) { + failsafe = 0; + }else if (_fs_counter <0){ + _fs_counter = -1; + } + } +} + + + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h new file mode 100644 index 0000000000..fdc4505e7d --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h @@ -0,0 +1,46 @@ +#ifndef RC_h +#define RC_h + +#include +#include "WProgram.h" + +#define CH1 0 +#define CH2 1 +#define CH3 2 +#define CH4 3 +#define CH5 4 +#define CH6 5 +#define CH7 6 +#define CH8 7 + +#define MIN_PULSEWIDTH 900 +#define MAX_PULSEWIDTH 2100 + +#define ELEVONS 1 + +class RC +{ + public: + // RC(); + virtual void init(); + virtual void trim(); + virtual void read(); + virtual void output(); + virtual void set_channel_direction(uint8_t ch, int8_t dir); + virtual void set_ch_pwm(uint8_t ch, uint16_t pwm); + virtual void twitch_servos(void); + + void set_failsafe(uint16_t fs); + void set_mix_mode(uint8_t mode); + + uint8_t failsafe; + + protected: + void check_throttle_failsafe(uint16_t throttle); + uint8_t _fs_counter; + uint8_t _mix_mode; // 0 = normal, 1 = elevons + uint8_t _direction_mask; + uint16_t _fs_value; // PWM value to trigger failsafe flag +}; + +#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde new file mode 100644 index 0000000000..196fafc00a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde @@ -0,0 +1,59 @@ +/* + Example of APM2_RC library. + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +APM2_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot Mega RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.set_mix_mode(1); // 1 = elevons, 0 = normal + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + for(int y = 0; y < 8; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + //print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.print(rc.radio_in[CH_RUDDER], DEC); + Serial.print("\tch5 "); + Serial.print(rc.radio_in[4], DEC); + Serial.print("\tch6: "); + Serial.print(rc.radio_in[5], DEC); + Serial.print("\tch7 :"); + Serial.print(rc.radio_in[6], DEC); + Serial.print("\tch8 :"); + Serial.println(rc.radio_in[7], DEC); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde new file mode 100644 index 0000000000..33c722863b --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde @@ -0,0 +1,59 @@ +/* + Example of APM2_RC library. + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +APM2_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot Mega RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + + for(int y = 0; y < 8; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + //print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.print(rc.radio_in[CH_RUDDER], DEC); + Serial.print("\tch5 "); + Serial.print(rc.radio_in[4], DEC); + Serial.print("\tch6: "); + Serial.print(rc.radio_in[5], DEC); + Serial.print("\tch7 :"); + Serial.print(rc.radio_in[6], DEC); + Serial.print("\tch8 :"); + Serial.println(rc.radio_in[7], DEC); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde new file mode 100644 index 0000000000..810b2df8ac --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde @@ -0,0 +1,48 @@ +/* + Example of AP_RC library. + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +AP_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot RC Elevon library test"); + + rc.set_mix_mode(1); // 1 = elevons, 0 = normal + rc.init(); + + delay(1000); +} +void loop() +{ + delay(60); + rc.read_pwm(); + for(int y = 0; y < 4; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + print_pwm(); +} + + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.println(rc.radio_in[CH_RUDDER], DEC); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde new file mode 100644 index 0000000000..3b2b02c90c --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde @@ -0,0 +1,50 @@ +/* + Example of AP_RC library. + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot RC Library +AP_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + for(int y = 0; y < 4; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.println(rc.radio_in[CH_RUDDER], DEC); +} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt new file mode 100644 index 0000000000..26800b80d5 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt @@ -0,0 +1,4 @@ +AP_RC KEYWORD1 +init KEYWORD2 +set_ch_pwm KEYWORD2 +read_pwm KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp new file mode 100644 index 0000000000..a04c6037b4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp @@ -0,0 +1,120 @@ +/* + AP_Radio.cpp - Radio library for Arduino + Code by Jason Short. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + +*/ + +#include "Waypoints.h" + +Waypoints::Waypoints() +{ +} + +void +Waypoints::set_waypoint_with_index(Waypoints::WP wp, uint8_t i) +{ + i = constrain(i, 0, _total); + uint32_t mem = _start_byte + (i * _wp_size); + + eeprom_busy_wait(); + eeprom_write_byte((uint8_t *) mem, wp.id); + + mem++; + eeprom_busy_wait(); + eeprom_write_byte((uint8_t *) mem, wp.p1); + + mem++; + eeprom_busy_wait(); + eeprom_write_dword((uint32_t *) mem, wp.alt); + + mem += 4; + eeprom_busy_wait(); + eeprom_write_dword((uint32_t *) mem, wp.lat); + + mem += 4; + eeprom_busy_wait(); + eeprom_write_dword((uint32_t *) mem, wp.lng); +} + +Waypoints::WP +Waypoints::get_waypoint_with_index(uint8_t i) +{ + Waypoints::WP wp; + + i = constrain(i, 0, _total); + uint32_t mem = _start_byte + (i * _wp_size); + + eeprom_busy_wait(); + wp.id = eeprom_read_byte((uint8_t *)mem); + + mem++; + eeprom_busy_wait(); + wp.p1 = eeprom_read_byte((uint8_t *)mem); + + mem++; + eeprom_busy_wait(); + wp.alt = (long)eeprom_read_dword((uint32_t *)mem); + + mem += 4; + eeprom_busy_wait(); + wp.lat = (long)eeprom_read_dword((uint32_t *)mem); + + mem += 4; + eeprom_busy_wait(); + wp.lng = (long)eeprom_read_dword((uint32_t *)mem); +} + + +Waypoints::WP +Waypoints::get_current_waypoint(void) +{ + return get_waypoint_with_index(_index); +} + +uint8_t +Waypoints::get_index(void) +{ + return _index; +} + +void +Waypoints::next_index(void) +{ + _index++; + if (_index >= _total) + _index == 0; +} + +void +Waypoints::set_index(uint8_t i) +{ + i = constrain(i, 0, _total); +} + +uint8_t +Waypoints::get_total(void) +{ + return _total; +} +void +Waypoints::set_total(uint8_t total) +{ + _total = total; +} + +void +Waypoints::set_start_byte(uint16_t start_byte) +{ + _start_byte = start_byte; +} + +void +Waypoints::set_wp_size(uint8_t wp_size) +{ + _wp_size = wp_size; +} diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h new file mode 100644 index 0000000000..ddf03378a4 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h @@ -0,0 +1,46 @@ +#ifndef Waypoints_h +#define Waypoints_h + +#include +#include "WProgram.h" +#include + +class Waypoints +{ + public: + Waypoints(); + + struct WP { + uint8_t id; // for commands + int8_t p1; // for commands + int32_t alt; // Altitude in centimeters (meters * 100) + int32_t lat; // Lattitude * 10**7 + int32_t lng; // Longitude * 10**7 + }; + + WP get_waypoint_with_index(uint8_t i); + WP get_current_waypoint(void); + + void set_waypoint_with_index(Waypoints::WP wp, uint8_t i); + + void set_start_byte(uint16_t start_byte); + void set_wp_size(uint8_t wp_size); + + void next_index(void); + uint8_t get_index(void); + void set_index(uint8_t i); + + uint8_t get_total(void); + void set_total(uint8_t total); + + + + private: + uint16_t _start_byte; + uint8_t _wp_size; + uint8_t _index; + uint8_t _total; +}; + +#endif + diff --git a/Tools/ArduPPM/WorkBasket/Jeti Duplex/readme.txt b/Tools/ArduPPM/WorkBasket/Jeti Duplex/readme.txt new file mode 100644 index 0000000000..0f37e9394e --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/Jeti Duplex/readme.txt @@ -0,0 +1,29 @@ + +This code is a work from Uwe Gartmann. + + +Could be integrated in the main Ardu code for jeti telemetry support. + + + + + +JetiBox.cpp, Version 1.0 beta + July 2010, by Uwe Gartmann + + Library acts as a Sensor when connected to a Jeti Receiver + written for Arduino Mega / ArduPilot Mega + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/readme.txt b/Tools/ArduPPM/WorkBasket/readme.txt new file mode 100644 index 0000000000..9a03b12c96 --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/readme.txt @@ -0,0 +1,9 @@ + +The ArduPPM "Workbasket" is a source of code or tools usefull for ppm encoder projects. + + +It is recommanded to delete no more needed stuff when integration in the ArduPPM code has been done to keep the repository size smaller. + + + + diff --git a/Tools/ArduPPM/WorkBasket/spektrum_to_ppm_encoder/spektrum_to_ppm_encoder.pde b/Tools/ArduPPM/WorkBasket/spektrum_to_ppm_encoder/spektrum_to_ppm_encoder.pde new file mode 100644 index 0000000000..26ae6eb16a --- /dev/null +++ b/Tools/ArduPPM/WorkBasket/spektrum_to_ppm_encoder/spektrum_to_ppm_encoder.pde @@ -0,0 +1,309 @@ + +/********************************************************************************************************* + Title : C file for the ArduPilotMega PPM encoder + Author: Doug Weibel + Date: 11/11/10 + Comments: This software is FREE. Use it at your own risk. + This software has been tested with limited transmitter types and receiver binding types */ + + #include + #include + + #define TRUE 1 + #define FALSE 0 + + // buffers + struct Buffer { + volatile bool AB; + volatile uint8_t position; + volatile uint8_t bytesA[16]; + volatile uint8_t bytesB[16]; + }; + volatile Buffer buffer; + volatile bool sync = 0; // Are we in sync + volatile unsigned long last_end_time = 4294966895; // time last packet ended (or last char received if not sync'd) + + bool eleven_bit = 0; + uint16_t ch[12]; // PWM values + volatile bool falling_edge; + volatile uint8_t ch_index; + #define CH_MAX 8 // Number of channels to form PWM output + volatile uint16_t offsets[CH_MAX]; + + #define baud_setting 16 // See page 231 of the data sheet + + // Comment - this software rearranges the channel order from the tranmsitter to match APM useage + // Ch1 = Aileron = Spectrum Ch2 + // Ch2 = Elevator = Spectrum Ch3 + // Ch3 = Throttle = Spectrum Ch1 + // Ch8 = Flight mode = Spectrum Ch7 + const uint16_t failsafe_values[8] = {1500,1500,1000,1500,1500,1500,1500,1295}; + const uint16_t startup_values[8] = {1500,1500,1000,1500,1500,1500,1500,1900}; + + extern unsigned long timer0_millis; + + +//******************************************** +void setup() +{ + DDRD &= 0B11111110; // pd0 = RX0 + DDRB |= 0B00000111; // pb0 = PPM LED, pb1 = MUX OUT, pb2 = PPM OUT + PORTB |= 0B00000100; // Set PPM OUT high + + TCCR1A = 0; + TCCR1B = 0B00001001; // This will differ for the 328? + TCCR1C = 0; + OCR1A = 0Xffff; + TCNT1 = 0; + TIMSK1 = 0B00000010; //Output Compare A Match Interrupt Enable + + // init buffers + buffer.position = 0; + buffer.AB = TRUE; + sync = FALSE; + + // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) + UCSR0A |= (1<> 8; + UBRR0L = baud_setting; + UCSR0B |= (1<=20) { + ledCount=0; + PORTB ^= 0B00000001; + } + } else if (state == 3) { // LED fast flash means receiving signal + ledCount++; + if(ledCount>=5) { + ledCount=0; + PORTB ^= 0B00000001; + } + } else { // LED off means something is wrong + PORTB &= 0B11111110; + } + + } +} + +// Serial port character received interrupt +//******************************************* +ISR(USART_RX_vect) //, ISR_BLOCK) +{ + unsigned char c = UDR0; + uint8_t i; + unsigned long m; + unsigned long m2; + + // timer0_millis could change inside interrupt and we don't want to disable interrupts + // we can do two readings and compare. + m = timer0_millis; + m2 = timer0_millis; + if (m!=m2) // timer0_millis corrupted? + m = timer0_millis; // this should be fine... + + if (sync) + { + if(buffer.AB) + buffer.bytesA[buffer.position] = c; + else + buffer.bytesB[buffer.position] = c; + + buffer.position++; + if(buffer.position == 16) + { + sync = FALSE; + buffer.AB = !buffer.AB; + last_end_time = m; + } + } + else + { + if ((m - last_end_time) > 7) + { + sync = TRUE; + if (buffer.AB) + { + buffer.bytesA[0] = c; + buffer.position = 1; + } + else + { + buffer.bytesB[0] = c; + buffer.position = 1; + } + } else { + last_end_time = m; + } + } + +/* +if (sync) { + PORTB |= 0B00000001; +} else { + PORTB &= 0B11111110; +} +*/ + +} + + + +// Timer 1 Compare A Match interrupt +//********************************** +ISR(TIMER1_COMPA_vect) { + + if (!falling_edge) { + PORTB |= 0B00000100; // Set PPM OUT high + if(ch_index < CH_MAX ) { + OCR1A = 16 * offsets[ch_index]; + falling_edge = TRUE; + ch_index++; + } else { + // set timer top to max to minimize interupt execution when not sending a frame + OCR1A = 0xffff; + } + } else { + if(ch_index <= CH_MAX ) { + PORTB &= 0B11111011; // Set PPM OUT low + OCR1A = 4800; //16 * 300, 300 usec negative going pulse width; + falling_edge = FALSE; + } else { + // set timer top to max to minimize interupt execution when not sending a frame + OCR1A = 0xffff; + } + } + +//PORTB &= 0B11111110; +} + + +// Get new RC data. Call before calling pwm_output +//************************************************* +uint8_t update(void) { + +// Returns: +// 0 means that we have not yet received packets +// 1 means we are starting up +// 2 means we are in failsafe +// 3 means radio OK + + static bool setup_done = 0; + unsigned long m; + unsigned long m2; + unsigned long time_now; + uint8_t packet1[17], packet2[17]; + uint16_t temp; + bool redo = 0; + + // timer0_millis could change and we don't want to disable interrupts + // we can do two readings and compare. + time_now = timer0_millis; + m2 = timer0_millis; + if (time_now!=m2) // timer0_millis corrupted? + time_now = timer0_millis; // this should be fine... + + m = last_end_time; + m2 = last_end_time; + if (m!=m2) // last_end_time corrupted? + m = last_end_time; // this should be fine... + + if( m > time_now ) { // We have not gotten in sync yet + for(int i=0;i<12;i++) + ch[i] = startup_values[i]; + return 0; + } else if (time_now < m + 500) { // We are receiving packets + + do { + packet1[16] = buffer.AB; + if (buffer.AB) + for (int i=0; i < 16; i++) + packet1[i] = buffer.bytesB[i]; + else + for (int i=0; i < 16; i++) + packet1[i] = buffer.bytesA[i]; + if(packet1[16] != buffer.AB) { // Did buffer.AB didn't change while we were working with it? + redo = 1; + } else { + redo = 0; + } + } while (redo); + + if(!setup_done) { + for (int i=2;i<15;i=i+2) { + if(packet1[i]&0B01000000) eleven_bit = 1; + } + setup_done = 1; + } + + for(int i=2;i<15;i=i+2) { + int ch_num; + temp = packet1[i+1] + (packet1[i]<<8); + if(eleven_bit) { + ch_num = (temp&0B0111100000000000)>>11; + ch[ch_num] = temp&0B0000011111111111; + ch[ch_num] /= 2; + ch[ch_num] += 1000; + } else { + ch_num = (temp&0B0011110000000000)>>10; + ch[ch_num] = temp&0B0000001111111111; + ch[ch_num] += 1000; + } + } + // Rearrange the first three channels from Spektrum order to APM order + temp = ch[0]; + ch[0]=ch[1]; + ch[1]=ch[2]; + ch[2]=temp; + // Rearrange the last two channels from Spektrum order to APM order + temp = ch[6]; + ch[6]=ch[7]; + ch[7]=temp; + return 3; + + } else { // This is the failsafe case + for(int i=0;i<12;i++) + ch[i] = failsafe_values[i]; + return 2; + } +} + +// Send a PWM output frame +// ************************************************ +void ppm_output(void) { + +// Channels 0 to CH_MAX-1 + + // Convert PWM values to offsets + for(int i=0;i Date: Sat, 1 Oct 2011 23:43:52 +0200 Subject: [PATCH 4/6] Fix Arduino version detection on some systems --- libraries/AP_Common/Arduino.mk | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index 8f3e97a0c8..b3316e0d23 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -305,7 +305,14 @@ LIBOBJS := $(SKETCHLIBOBJS) $(ARDUINOLIBOBJS) # Pull the Arduino version from the revisions.txt file # # XXX can we count on this? If not, what? -ARDUINO_VERS := $(shell expr `head -1 $(ARDUINO)/lib/version.txt | cut -d ' ' -f 2`) +ARDUINO_VERS := $(shell expr `head -1 $(ARDUINO)/revisions.txt | cut -d ' ' -f 2`) +# If the version is not a number, try it again, using another file +ifneq ($(ARDUINO_VERS),$(shell echo $(ARDUINO_VERS) | sed 's/[^0-9]*//g')) + ARDUINO_VERS := $(shell expr `head -1 $(ARDUINO)/lib/version.txt | cut -d ' ' -f 2`) +endif +ifneq ($(ARDUINO_VERS),$(shell echo $(ARDUINO_VERS) | sed 's/[^0-9]*//g')) + $(warning Could not determine Arduino version) +endif # Find the hardware directory to use HARDWARE_DIR := $(firstword $(wildcard $(SKETCHBOOK)/hardware/$(HARDWARE) \ From c37fabea6661e248364706feb6521116ad86b9d7 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 1 Oct 2011 23:55:39 +0200 Subject: [PATCH 5/6] Added a comment to the BOARD type of teh make configure output file --- libraries/AP_Common/Arduino.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index b3316e0d23..c8e1562443 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -398,7 +398,8 @@ upload: $(SKETCHHEX) configure: $(warning WARNING - A $(SKETCHBOOK)/config.mk file has been written) $(warning Please edit the file to match your system configuration, if you use a different board or port) - @echo BOARD=mega > $(SKETCHBOOK)/config.mk + @echo \# Select 'mega' for the original APM, or 'mega2560' for the V2 APM. > $(SKETCHBOOK)/config.mk + @echo BOARD=mega >> $(SKETCHBOOK)/config.mk @echo PORT=/dev/null >> $(SKETCHBOOK)/config.mk debug: From dcef0f34e9c33b258966f87e094e595f5e5b0cbd Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 27 Sep 2011 22:12:45 +0200 Subject: [PATCH 6/6] Correct/update channel assigment comment --- ArduPlane/ArduPlane.pde | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index afebcafd5e..00a4bda8aa 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -215,10 +215,12 @@ static const char* flight_mode_strings[] = { 2 Elevator 3 Throttle 4 Rudder (if we have ailerons) - 5 Mode - 6 TBD - 7 TBD - 8 TBD + 5 Aux5 + 6 Aux6 + 7 Aux7 + 8 Aux8/Mode + Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. + See libraries/RC_Channel/RC_Channel_aux.h for more information */ // Failsafe