Hello, I have an aeronautical project for an EFIS, I use an Arduino Mega 2560 with a 3-Space Nano Carrier Board to have Le Pitch, Roll et Yaw.
Nothing works, can you help me and see where I was wrong?
Regards,
Alfred
My Arduino code:
#include "SPI.h"
#include "Wire.h"
//#include "FT_NHD_43CTP_SHIELD.h"
#include "FT_NHD_43RTP_SHIELD.h"
#include <Encoder.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP280.h>
/*****************************************************************************
*
* Cette section contient des instructions "define" qui sont utilisées pour configurer
* unité pour votre application particulière. Sauf si vous êtes programmeur, ce
* est la seule partie du code que vous devez modifier. Veuillez consulter le
* manuel pour plus de détails au-delà des commentaires. Vous changez les chiffres et
* valeurs true ou false uniquement. Ne changez rien d'autre.
*
******************************************************************************/
#define MaxVertSpeed 4000 // will determine the size of the VSI scale on the indicator.
#define PrefSpeedUnits 3 //1 is kts, 2 is mph (defaut), 3 is km/h
#define PrefPressureUnits 3 // 1 is inHG (defaut), 2 is mmHg, 3 is hPa
#define PrefAltUnits 1 // 1 is ft (defaut), 2 is meters
#define WAStart 45 // the low side of the airspeed indicator white arc in PrefSpeedUnits
#define WAEnd 100 // the high side of the airspeed indicator white arc
#define GAStart 52 // the low side of the airspeed indicator green arc
#define GAEnd 116 // the high side of the airspeed indicator green arc
#define BLine 0 // the airspeed indicator blue line. Use 0 if you do not need a blue line.
#define RLine 142 // the airspeed indicator red line.
#define MaxSpeed 250 // this is the top number on the gauge. This should be a little higher than your red line.
#define SpeedTics 50 // this determines how many ticks will print out on the speed slider
#define SpeedSlider false //false will give gauge like readout. True gives ticker type readout
#define HeadingNum false // Black box with the heading on the DG strip
#define NumAverageTrend 10 // Higher numbers are less jittery. Lower numbers are more responsive. Applies to VSI and Rate of Turn
#define ECPD 2 // (4 defaut) number of ecoder clicks per detent. Probably 1, 2, or 4.
#define StartMenu 6 // defines where the cursor starts. 6 is the Altimeter Adjust
#define StartBrightness 255 // how bright the unit is on power-on. 255 is full bright. 0 is completely dark.
#define StartHeadingBug 360 // where the heading bug shows up.
#define StartingPressure 1013.25 // 29.92 (defaut) Put this as the pressure in your preferred unit system. 29.92, 1013.25, 760.0;
#define StaticCalibrationOffset 3 // This is a value used to calibrate your static BMP280. Start with 0, and make tiny changes. Each 1 you add adds about 25 feet (8 meters). Calibrate this one first.
#define AirspeedCalibrationOffset 0 // This is a value used to calibrate your airspeed. Start with 0. Calibrate this one after the Static. Each one is 1 mph.
#define CenterDisplayType 1 // 1 is yellow line moving with the horizon. 2 Yellow lines that stay level. 3 gives yellow plane.
#define dynamicsky true // false does not change the display color based on pitch. true does. Sky gets deeper blue as you point up.
#define dynamicground false // false does not change the display color based on pitch. true does. Groiund gets reddish as you point down.
#define PermRollOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.)
#define PermPitchOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.) Use calibration routine instead.
#define PermCompassOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.)
// Used in Serial Data Transfer. This binaryInt allows the sensor to recieve floating point numbers and automatically decode them
typedef union {
float floatingPoint;
byte binary[4];
}
binaryFloat;
binaryFloat CH;
binaryFloat CP;
binaryFloat CR;
byte CalibrationStatus = 0;
// end data types and variables used in serial transfer of data .
int BotWhA;
int TopWhA;
int BotGA;
int TopGA;
int RL;
int BL;
int NumTics;
int TicMap;
int HalfSpeedTics;
int CurrentSpeed;
float CurrentRoll; // angle in radians from -Pi to + Pi. - is to the left
float CurrentPitch; // angle in radians from -Pi to + Pi. - is down
float CurrentHeading = 0; // this will actually come from the IMU unit, but we will start with a number
float CurrentRateOfTurn = -0.0115; // this is the number of radians per second. Number is for test purposes only, and cooresponds to a 2 minute left turn
float CurrentRateOfClimb = 0;
float CurrentAltitude = 400;
float CurrentGs = 1;
float PreviousHeading = 0;
float PreviousAltitude = 0;
float CurrentPressure = StartingPressure; // needed to switch on the fly.
int HeadingBug = StartHeadingBug;
int DispDegs;
int brightness = StartBrightness; //Max = 255, min = 0
int SpeedUnits = PrefSpeedUnits;
int AltUnits = PrefAltUnits;
int PressureUnits = PrefPressureUnits;
int PressureDigits[] = {2,9,9,2};
long OldEncoderPosition = -999;
long NewEncoderPosition = -999;
int Menu = StartMenu; // 1 is brightness, 2 is speed units, 3 is bug, 4 is trim, 5 is pressure, 6 is pressure units, 7 is alt units
boolean MenuSelected = false;
boolean ButtonDown = false;
boolean ButtonReleased = false;
int EncMove = 0;
int EncMult = 1;
unsigned long currentMicros; // = micros();
unsigned long previousMicros;
unsigned long intervalMicros;
int eraseme = 0;
boolean SensorError = false;
int ErrorStreak = 0;
float RollOffset = PermRollOffset;
float PitchOffset = PermPitchOffset;
float CompassOffset = PermCompassOffset;
Encoder myEnc(19, 18); // these pins are interrupt capable. Reverse these if your encoder moves the menus the wrong way.
Adafruit_BMP280 Static; // I2C communication with the static
Adafruit_BMP280 Pitot; // I2C communication with the static
/* Global object for transport */
//FT801IMPL_SPI FTImpl(FT_CS_PIN,FT_PDN_PIN,FT_INT_PIN);
FT800IMPL_SPI FTImpl(FT_CS_PIN,FT_PDN_PIN,FT_INT_PIN);
/* Api to bootup FT800 or FT801, verify FT800 or FT801 hardware and configure display/audio pins */
/* Returns 0 in case of success and 1 in case of failure */
int16_t BootupConfigure(){
uint32_t chipid = 0;
FTImpl.Init(FT_DISPLAY_RESOLUTION);//configure the display to the WQVGA
delay(20);//for safer side
chipid = FTImpl.Read32(FT_ROM_CHIPID);
/* Identify the chip */
// if(FT801_CHIPID != chipid)
if(FT800_CHIPID != chipid)
{
Serial.print("Error in chip id read ");
Serial.println(chipid,HEX);
return 1;
}
/* Set the Display & audio pins */
FTImpl.SetDisplayEnablePin(FT_DISPENABLE_PIN);
FTImpl.SetAudioEnablePin(FT_AUDIOENABLE_PIN);
FTImpl.DisplayOn();
FTImpl.AudioOn();
return 0;
}
void FlipNum(int Xloc, int Yloc, int TopDigit, int BottomDigit, int Twentiethsflip)
{ // This function is called from within a drawing list, and paints a 10 by 15 block with a rolling number display
// location is upper right referenced. The requested area of the screen is blacked and blanked. This cannot be
// used for more than two digits at a time. Twentiethsflip is the amount that the numbers have slid - as in 5/20ths
// 0/20ths prints just the TopDigit. 20/20ths prints just the BottomDigit
// use for the TopDigit or the BottomDigit if you want them to show up as blank.
int NumDigits = 1;
if ((BottomDigit >= 10) or (TopDigit >= 10)) NumDigits = 2;
FTImpl.ColorRGB(0,0,0); // set the color to Black
FTImpl.ClearStencil(0); // set the stencil to blank
FTImpl.StencilOp(FT_INCR,FT_INCR); // set the stencil operation to mask the blank
FTImpl.Begin(FT_RECTS); // get ready to make the box
FTImpl.Vertex2ii(Xloc,Yloc,0,0);
FTImpl.Vertex2ii(Xloc - (10 * NumDigits),Yloc + 16,0,0); // then draw the box that blanks the space and DEFINES THE STENCIL ....
FTImpl.StencilOp(FT_KEEP,FT_KEEP); // and we are done building the stencil. Clear the stencil creating function
FTImpl.ColorRGB(255,255,255); // set the color to White
FTImpl.StencilFunc(FT_GREATER, 0, 255); // and olnly draw in the stencil
if (TopDigit != -1) FTImpl.Cmd_Number(Xloc, Yloc-Twentiethsflip-1, 27, FT_OPT_RIGHTX, TopDigit); // print the part of the numbers that shows up in the box
if (BottomDigit != -1) FTImpl.Cmd_Number(Xloc, Yloc+19-Twentiethsflip, 27, FT_OPT_RIGHTX, BottomDigit); // print the part of the numbers that shows up in the box
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
}
void DrawScreen()
{
//** FTImpl.SetCTouchMode(FT_CTOUCH_MODE_EXTENDED); //set mode to extended for FT801
int16_t xoffset,yoffset;
xoffset = ((FT_DISPLAYWIDTH - 480)/2);
yoffset = ((FT_DISPLAYHEIGHT - 272)/2);
//** Begin building angle gauge stencil
FTImpl.DLStart();
// this should set the screen brightness
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100));
FTImpl.ClearColorRGB(0,0,0);
FTImpl.Clear(1,0,1);
FTImpl.ClearStencil(0);
FTImpl.StencilOp(FT_INCR,FT_INCR);
// Draw the rays
FTImpl.LineWidth(6);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(145,70,0,0);
FTImpl.Vertex2ii(157,81,0,0);
FTImpl.Vertex2ii(165,61,0,0);
FTImpl.Vertex2ii(171,67,0,0);
FTImpl.Vertex2ii(178,43,0,0);
FTImpl.Vertex2ii(186,56,0,0);
FTImpl.Vertex2ii(200,41,0,0);
FTImpl.Vertex2ii(203,49,0,0);
FTImpl.Vertex2ii(220,36,0,0);
FTImpl.Vertex2ii(221,44,0,0);
FTImpl.Vertex2ii(260,36,0,0);
FTImpl.Vertex2ii(259,44,0,0);
FTImpl.Vertex2ii(280,41,0,0);
FTImpl.Vertex2ii(277,49,0,0);
FTImpl.Vertex2ii(302,43,0,0);
FTImpl.Vertex2ii(294,56,0,0);
FTImpl.Vertex2ii(315,61,0,0);
FTImpl.Vertex2ii(309,67,0,0);
FTImpl.Vertex2ii(335,70,0,0);
FTImpl.Vertex2ii(323,81,0,0);
// Draw a circle centered on 240, 150.
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.PointSize(110*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.StencilOp(FT_ZERO,FT_ZERO);
FTImpl.ColorRGB(0,0,0);//set the color of the string to BLACK color
FTImpl.PointSize(107 * 16);//inner circle
FTImpl.Vertex2ii(240,150,0,0);
//Trim the bottom of the circle off
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(120,82,0,0);
FTImpl.Vertex2ii(360,270,0,0);
FTImpl.StencilOp(FT_KEEP,FT_KEEP);
//** Stencil complete. Clear Screen to Sky Color
int darkensky = 0;
if (dynamicsky) darkensky = (int)(70*sin(CurrentPitch)); // darkensky won't change the sky's color unless dynamicsky is chosen
if (darkensky > 0) darkensky = 0;
FTImpl.ClearColorRGB(150+darkensky,150+darkensky,255);
FTImpl.Clear(1,0,0); // the zero leaves the stencil in place
//** Math work in progress. Draw in a red line. First, use only shallow angles and assume rightside up
FTImpl.LineWidth(12);
int darkenground = 0;
if (dynamicground) darkenground = (int)(60*sin(CurrentPitch)); // darkenground won't change the ground's color unless dynamicground is chosen
if (darkenground < 0) darkenground = 0;
FTImpl.ColorRGB(120+darkenground,80-(darkenground/2),20-(darkenground/6)); //);//set the color to brown, with a redish adjustment as you head down,
int LS; // left side intersect
int RS; // right side intersect
int LO = 0; // Left Offset - used when the line does not extend across the screen
int RO = 0; // Right Offset - used when the line does not extend across the screen
float CXP; // Center X Position
float CYP; // Center Y Pos
float TanCurrentRoll = tan(CurrentRoll); // Do math as few times as possible
float SinCurrentRoll = sin(CurrentRoll);
float CosCurrentRoll = cos(CurrentRoll);
CXP = 240-SinCurrentRoll*((CurrentPitch/.0175)*3.6);
CYP = 150-CosCurrentRoll*((CurrentPitch/.0175)*3.6);
if ((CXP >= 0) and (CXP <= 480)) // the center of the line is on the screen
{
LS = CYP+(CXP*TanCurrentRoll);
RS = CYP-((480-CXP)*TanCurrentRoll);
if (LS < 0) // The line begins above the display.
{LO = CXP-((CYP*CXP)/(CYP-LS)); LS = 0;} // similar triangles
else if (LS > 272) // The line begins below the display.
{LO = CXP-((CXP*(272-CYP))/(LS-CYP)); LS = 272;} // similar triangles
if (RS < 0) // The line ends above the display
{RO = (480-CXP)-(((480-CXP)*CYP)/(CYP-RS)); RS = 0;} // similar triangles
else if (RS > 272) // The line ends below the display
{RO = (480-CXP)-(((480-CXP)*(270-CYP))/(RS-CYP)); RS = 272;} // similar triangles
FTImpl.Begin(FT_EDGE_STRIP_B); // the use of 4 points ensures complete usability
FTImpl.Vertex2ii(0,LS,0,0);
FTImpl.Vertex2ii(0+LO,LS,0,0);
FTImpl.Vertex2ii(480-RO,RS,0,0);
FTImpl.Vertex2ii(480,RS,0,0);
}
// VERY IMPORTANT. The ground should not show up if the sensor is offline.
if (ErrorStreak > 16) // set to about the number of frames per second...
{
FTImpl.LineWidth(24);
FTImpl.ClearColorRGB(0,0,0); // set the color to black
FTImpl.Clear(1,0,0); // the zero leaves the stencil in place, but clears the screen to black
FTImpl.ClearColorRGB(255,0,0); // set the color to red
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(140,20,0,0);
FTImpl.Vertex2ii(340,252,0,0); // Draw the gigantic red x that says not to navigate...
FTImpl.Vertex2ii(140,252,0,0);
FTImpl.Vertex2ii(340,20,0,0);
}
//** Reprint Angle Gauge from Stencil
FTImpl.StencilFunc(FT_NOTEQUAL, 0, 255); //only operate on the stencil
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.PointSize(126*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //clear the stencil function
//** Add angle numbers to the stencil gauge
FTImpl.Cmd_Number(140 , 62, 26, FT_OPT_CENTER, 50);
FTImpl.Cmd_Text(160 , 36, 26, FT_OPT_CENTER, "2 Min (30)");
FTImpl.Cmd_Number(305 , 36, 26, FT_OPT_CENTER, 30);
FTImpl.Cmd_Number(340 , 62, 26, FT_OPT_CENTER, 50);
// FTImpl.ColorRGB(255,255,255);//set the color of the string back to WHITE color
FTImpl.LineWidth(16);
//** Given CurrentHeading (float), overlay the DG Strip
// Display is 480 pixels wide, so the center is at 240. 5 pixels is one degree
// Draw the line strip line
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(75,22,0,0);
FTImpl.Vertex2ii(405,22,0,0);
int Nearest5 = round(CurrentHeading/5) * 5;
int Nearest10 = round(CurrentHeading/10) * 10;
boolean NumTic = false;
if (Nearest5 == Nearest10) NumTic = true;
for (int degs = Nearest5 - 30; degs <= (Nearest5 + 30); degs = degs + 5)
{
DispDegs = degs;
if (DispDegs > 360) DispDegs = DispDegs - 360;
if (DispDegs < 1) DispDegs = DispDegs + 360;
if (NumTic) // This one is divisible by 10, and gets a number and a tic
{
NumTic = false;
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,25,0,0);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,22,0,0);
FTImpl.Cmd_Number( (degs-(CurrentHeading-40))*5+40, 13, 26, FT_OPT_CENTER, DispDegs);
}
else // This one is between numbers, and gets a tic only
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,25,0,0);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,15,0,0);
NumTic = true;
}
}
// Now draw the white arrow for no rate of turn
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(240,0,0,0);
FTImpl.Vertex2ii(240,41,0,0);
FTImpl.Vertex2ii(238,29,0,0);
FTImpl.Vertex2ii(242,29,0,0);
FTImpl.Vertex2ii(240,41,0,0);
FTImpl.Vertex2ii(236,27,0,0);
FTImpl.Vertex2ii(240,31,0,0);
FTImpl.Vertex2ii(244,27,0,0);
FTImpl.Vertex2ii(240,41,0,0);
// if option is selected, draw the Heading Number
if (HeadingNum)
{
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(160,160,160);//set the color to White
FTImpl.Vertex2ii(240-18, 0);
FTImpl.Vertex2ii(240+18,15);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(240-16, 0);
FTImpl.Vertex2ii(240+16,13);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Number(240, 6, 27, FT_OPT_CENTER, CurrentHeading);
}
// Draw heading Bug if it shows up, and the Bug setting Regardless
FTImpl.Cmd_Text(43, 260, 27, FT_OPT_CENTER, "* Bug:");
FTImpl.Cmd_Number(87, 260, 27, FT_OPT_CENTER, HeadingBug);
FTImpl.ColorRGB(255,165,4);//set the color to Orange
//FTImpl.Cmd_Number(159, 260, 27, FT_OPT_CENTER, difference);
int BugCalc = HeadingBug;
if ((CurrentHeading < 41) and (HeadingBug > 319)) BugCalc = BugCalc - 360;
if ((CurrentHeading > 319) and (HeadingBug < 41)) BugCalc = BugCalc + 360;
if (abs(CurrentHeading-BugCalc) < 33) // it should be on the strip, so lets draw this sucker! - was 41
{
FTImpl.Begin(FT_LINES);
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(240-((CurrentHeading-BugCalc)*5),23,0,0);
FTImpl.Vertex2ii(240-((CurrentHeading-BugCalc)*5),2,0,0);
}
//** End overlay the DG Strip
//** Overlay Brightness Slider
// remember brightness is 0-255
FTImpl.ColorRGB(128,128,128);//set the color of the string to GREY color
FTImpl.LineWidth(48);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(20,235,0,0);
FTImpl.Vertex2ii(20,235-(255*.76),0,0); //255 scaled by 76%
FTImpl.PointSize(10*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(20,235-(brightness*.76),0,0); // this is the actual dot
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.PointSize(8*16);//circle
FTImpl.Vertex2ii(20,235-(brightness*.76),0,0); // this is the actual dot
// include the Empty Battery Icon. This is greyed out in this version, but in the next version with the board, it will
// be a live icon showing charging and discharging...
FTImpl.ColorRGB(170,170,170);//set the color to GREY
FTImpl.LineWidth(32);
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(9,8,0,0);
FTImpl.Vertex2ii(32,17,0,0);
FTImpl.Vertex2ii(30,11,0,0);
FTImpl.Vertex2ii(35,14,0,0);
//FTImpl.ColorRGB(150,150,255);//set the color to SKY
FTImpl.Vertex2ii(11,10,0,0);
FTImpl.Vertex2ii(30,15,0,0);
FTImpl.ColorRGB(170,170,170);//set the color to GREY
// WILL NEED to read battery info on pin A2, charge status on pin ?, and charging status on pin ? These are provided on the future board.
FTImpl.Vertex2ii(13,12,0,0);
FTImpl.Vertex2ii(28,13,0,0); // for now, just put a greyed out spot in the icon.
//** Overlay Speed Strip - this will likely change to a software selectable optional slider and ticker
// print out units...
switch (SpeedUnits) { // overlay the Speed Units. These can change on the fly...
case 1: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "kts"); break;
case 2: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "mph"); break;
case 3: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "km/h"); break; }
if (!SpeedSlider)
{// this is the gauge type speed readout
// lay out the speed line
FTImpl.LineWidth(16);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(80,235,0,0);
FTImpl.Vertex2ii(80,40,0,0);
// lay out the color bands
// White Arc
FTImpl.LineWidth(40);
FTImpl.Vertex2ii(78,235-BotWhA,0,0);
FTImpl.Vertex2ii(78,235-TopWhA,0,0);
// Yellow Line
FTImpl.LineWidth(24);
FTImpl.ColorRGB(255,232,0);//set the color of the string to YELLOW color
FTImpl.Vertex2ii(82,235-TopGA,0,0);
FTImpl.Vertex2ii(82,235-RL,0,0);
// Green Line
FTImpl.ColorRGB(0,185,0);//set the color of the string to Green color
FTImpl.Vertex2ii(82,235-TopGA,0,0);
FTImpl.Vertex2ii(82,235-BotGA,0,0);
// Red Line
FTImpl.LineWidth(24);
FTImpl.ColorRGB(185,0,0);//set the color of the string to Red color
FTImpl.Vertex2ii(84,235-RL,0,0);
FTImpl.Vertex2ii(76,235-RL,0,0);
// Blue Line
if (BL > 5)
{ // only draw it if we need it
FTImpl.LineWidth(24);
FTImpl.ColorRGB(32,32,185);//set the color of the string to Blue color
FTImpl.Vertex2ii(84,235-BL,0,0);
FTImpl.Vertex2ii(76,235-BL,0,0);
}
// lay out the tic marks and scale text
int count = 0;
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color of the string to White color
for (int tics = 0; tics <= NumTics; tics++)
{
TicMap = map(tics*SpeedTics,0,MaxSpeed,0,195); // 195 is the number of pixels it displays
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(81, 235-TicMap);
FTImpl.Vertex2ii(74,235-TicMap);
FTImpl.Cmd_Number(71, 227-TicMap, 26, FT_OPT_RIGHTX, tics*SpeedTics);
if (tics != 0)
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(80, 235-TicMap+HalfSpeedTics);
FTImpl.Vertex2ii(70,235-TicMap+HalfSpeedTics);
}
}
// lay out the current speed tic and number
TicMap = map(CurrentSpeed,0,MaxSpeed,0,195); // 195 is the number of pixels it displays
FTImpl.Vertex2ii(89, 235-TicMap);
FTImpl.Vertex2ii(78,235-TicMap);
FTImpl.Begin(FT_RECTS);
if (CurrentSpeed > 99)
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(125,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(123,241-TicMap);
}
else if (CurrentSpeed > 9)
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(113,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(111,241-TicMap);
}
else
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(102,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(100,241-TicMap);
}
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Number(92, 225-TicMap, 27, 0, CurrentSpeed);
}
else
{
// this routine still needs to be written, but I am not in a hurry for it!
}
//** Overlay the Angle Gauge, based on CurrentRoll
// this will involve angle calculations.
// the Center Strips, which can be one of three styles.
int XOffset1 = 0; int YOffset1 = 0; int XOffset2 = 0; int YOffset2 = 0; int XOffset3 = 0; int YOffset3 = 0;
if (CenterDisplayType == 1) // this is the standard yellow lines move with the horizon
{
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(24);
FTImpl.Begin(FT_LINES);
XOffset2 = 15*sin(CurrentRoll+(-1.5708));
YOffset2 = 15*cos(CurrentRoll+(-1.5708));
XOffset3 = 45*sin(CurrentRoll+(-1.5708));
YOffset3 = 45*cos(CurrentRoll+(-1.5708));
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0); // left
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0);
FTImpl.Vertex2ii(240+XOffset3,150+YOffset3,0,0); // right
}
else if (CenterDisplayType == 2) // the yellow lines stay straight
{
FTImpl.ColorRGB(0,0,0);//set the color to black
FTImpl.LineWidth(80);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(48);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(64);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
}
else // CenterDisplayType == 3 - basically does both
{
FTImpl.ColorRGB(255,255,255);//set the color to white
FTImpl.LineWidth(16);
FTImpl.Begin(FT_LINES);
XOffset2 = 15*sin(CurrentRoll+(-1.5708));
YOffset2 = 15*cos(CurrentRoll+(-1.5708));
XOffset3 = 45*sin(CurrentRoll+(-1.5708));
YOffset3 = 45*cos(CurrentRoll+(-1.5708));
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0); // left
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0);
FTImpl.Vertex2ii(240+XOffset3,150+YOffset3,0,0); // right
FTImpl.ColorRGB(0,0,0);//set the color to black
FTImpl.LineWidth(80);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,140,0,0); // tail
FTImpl.LineWidth(48);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(64);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(16);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,140,0,0); // tail
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
}
//20 degree marks
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.LineWidth(16);
XOffset1 = 80.5*sin(CurrentRoll+(0.4794));
YOffset1 = 80.5*cos(CurrentRoll+(0.4794));
XOffset2 = 80.5*sin(CurrentRoll+(-0.4794));
YOffset2 = 80.5*cos(CurrentRoll+(-0.4794));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 20 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 20 deg up
//10 degree marks
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.LineWidth(16);
XOffset1 = 51*sin(CurrentRoll+(0.8415));
YOffset1 = 51*cos(CurrentRoll+(0.84154));
XOffset2 = 51*sin(CurrentRoll+(-0.8415));
YOffset2 = 51*cos(CurrentRoll+(-0.8415));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 10 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 10 deg up
// 5 degree marks. These work out as half the offsetts of the 10, so less math!
FTImpl.LineWidth(12);
FTImpl.Vertex2ii(240+XOffset1/2,150+YOffset1/2,0,0);
FTImpl.Vertex2ii(240+XOffset2/2,150+YOffset2/2,0,0); // 20 deg down
FTImpl.Vertex2ii(240-XOffset1/2,150-YOffset1/2,0,0);
FTImpl.Vertex2ii(240-XOffset2/2,150-YOffset2/2,0,0); // 20 deg up
// 15 degree marks
XOffset1 = 56*sin(CurrentRoll+(0.3272));
YOffset1 = 56*cos(CurrentRoll+(0.3272));
XOffset2 = 56*sin(CurrentRoll+(-0.3272));
YOffset2 = 56*cos(CurrentRoll+(-0.3272));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 10 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 10 deg up
// the 20 degree numbers. These will always show up upright.
XOffset1 = 87.7*sin(CurrentRoll+(-0.6400));
YOffset1 = 87.7*cos(CurrentRoll+(-0.6400));
XOffset2 = 87.7*sin(CurrentRoll+(-2.5));
YOffset2 = 87.7*cos(CurrentRoll+(-2.5));
FTImpl.Cmd_Number(240-XOffset1, 150-YOffset1, 26, FT_OPT_CENTER, 20);
FTImpl.Cmd_Number(240-XOffset2, 150-YOffset2, 26, FT_OPT_CENTER, 20);
// the 10 degree numbers. These will always show up upright.
XOffset1 = 61.6*sin(CurrentRoll+(-0.9835));
YOffset1 = 61.6*cos(CurrentRoll+(-0.9835));
XOffset2 = 61.6*sin(CurrentRoll+(-2.15));
YOffset2 = 61.6*cos(CurrentRoll+(-2.15));
FTImpl.Cmd_Number(240-XOffset1, 150-YOffset1, 26, FT_OPT_CENTER, 10);
FTImpl.Cmd_Number(240-XOffset2, 150-YOffset2, 26, FT_OPT_CENTER, 10);
// The Arrow...
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(24);
FTImpl.Begin(FT_LINE_STRIP);
XOffset1 = 90*sin(CurrentRoll);
YOffset1 = 90*cos(CurrentRoll);
XOffset2 = 107*sin(CurrentRoll);
YOffset2 = 107*cos(CurrentRoll);
XOffset3 = XOffset2;
YOffset3 = YOffset2;
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
XOffset1 = 100*sin(CurrentRoll+(-0.040));
YOffset1 = 100*cos(CurrentRoll+(-0.040));
XOffset2 = 100*sin(CurrentRoll+( 0.040));
YOffset2 = 100*cos(CurrentRoll+( 0.040));
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0);
// The Rate of Turn Arrow
// CurrentRateOfTurn is the rate of turn in Radians/Sec. -(neg) is to the left
// It must be scaled to indicate rate of turn in 2 minutes, so
float TwoMinAngle = -1*(CurrentRateOfTurn * 45.7);
FTImpl.ColorRGB(255,128,0);//set the color to Orange
if (TwoMinAngle > 0.9) {TwoMinAngle = 0.9;
FTImpl.ColorRGB(255,0,0);
}
else if (TwoMinAngle < -0.9)
{
TwoMinAngle = -0.9;
FTImpl.ColorRGB(255,0,0);
};
FTImpl.Begin(FT_LINE_STRIP);
XOffset1 = 130*sin(TwoMinAngle);
YOffset1 = 130*cos(TwoMinAngle);
XOffset2 = 115*sin(TwoMinAngle);
YOffset2 = 115*cos(TwoMinAngle);
XOffset3 = XOffset2;
YOffset3 = YOffset2;
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
XOffset1 = 120*sin(TwoMinAngle+(-0.0250));
YOffset1 = 120*cos(TwoMinAngle+(-0.0250));
XOffset2 = 120*sin(TwoMinAngle+( 0.0250));
YOffset2 = 120*cos(TwoMinAngle+( 0.0250));
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0);
//** Overlay the Altitude Strip
// Print out the Labels, Kohlsman Window, and Units
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Text(332, 260, 27, FT_OPT_CENTER, "Altimeter:");
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: {
if (CurrentPressure > 31.0) CurrentPressure = 31.0;
if (CurrentPressure < 28.1) CurrentPressure = 28.1;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "inHg"); // inHg
FTImpl.Cmd_Number(397, 260, 27, FT_OPT_CENTER, (int)(CurrentPressure*100)); // Garder 2 chiffres après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(397,266,0,0); FTImpl.Vertex2ii(397,266,0,0);
break;}
case 2: {
if (CurrentPressure > 787.5) CurrentPressure = 787.5;
if (CurrentPressure < 713.5) CurrentPressure = 713.5;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "mmHg"); // mmHg
FTImpl.Cmd_Number(397, 260, 27, FT_OPT_CENTER, (int)(CurrentPressure*10)); // Garder 1 chiffre après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(407,266,0,0); FTImpl.Vertex2ii(407,266,0,0);
break; }
case 3: {
if (CurrentPressure > 1050.0) CurrentPressure = 1050.0;
if (CurrentPressure < 951.5) CurrentPressure = 951.5;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "hPa"); // hPa
FTImpl.Cmd_Number(420, 260, 27, FT_OPT_CENTERY | FT_OPT_RIGHTX, (int)(CurrentPressure*10)); // Garder 1 chiffre après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(410,266,0,0);
FTImpl.Vertex2ii(410,266,0,0);
break; }}
// BESOIN DE VÉRIFIER ET DE FIXER LES CAS 2, 3 - RÉGLAGES SUR LES LIAISONS VERTICALES
//** Overlay the VSI Strip
FTImpl.BlendFunc(FT_SRC_ALPHA,FT_ONE); // set for a blend instead of a paint
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(15,15,15); //set the color to a slightly lighter hue
FTImpl.Vertex2ii(429, 60);
FTImpl.Vertex2ii(480,240);
FTImpl.BlendFunc(FT_SRC_ALPHA, FT_ONE_MINUS_SRC_ALPHA); // set back to paint
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(435,230,0,0);
FTImpl.Vertex2ii(435,70,0,0);
boolean ticker = true;
for (int tics = 0; tics < 9; tics ++)
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(433,70+(tics*20),0,0);
FTImpl.Vertex2ii(439,70+(tics*20),0,0);
if (ticker)FTImpl.Cmd_Number(442, 62+(tics*20), 26, 0, abs(MaxVertSpeed-(tics*MaxVertSpeed/4)));
ticker = !ticker;
}
// now the value of CurrentRateOfClimb in a nice little box
TicMap = map(CurrentRateOfClimb, MaxVertSpeed*-1, MaxVertSpeed, 230, 70); // maps into display units
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(435, TicMap);
FTImpl.Vertex2ii(445,TicMap);
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(128,128,128);//set the color to Grey
FTImpl.Vertex2ii(442, TicMap+9);
FTImpl.Vertex2ii(480,TicMap-9);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(444, TicMap+7);
FTImpl.Vertex2ii(480,TicMap-7);
FTImpl.ColorRGB(255,255,255);//set the color to White
if ((CurrentRateOfClimb < 0) and (CurrentRateOfClimb > -1000)) // include the sign only for less than 1000 in the negative scale
FTImpl.Cmd_Number(462, TicMap, 26, FT_OPT_CENTER | FT_OPT_SIGNED, (CurrentRateOfClimb));
else FTImpl.Cmd_Number(462, TicMap, 26, FT_OPT_CENTER | FT_OPT_SIGNED, abs(CurrentRateOfClimb));
// print out units...
switch (AltUnits) { // overlay the Speed Units. These can change on the fly...
case 1: FTImpl.Cmd_Text(440, 43, 27, FT_OPT_CENTER, "ft, ft/s"); break;
case 2: FTImpl.Cmd_Text(440, 43, 27, FT_OPT_CENTER, "m, m/s"); break; }
//** Overlay the Altimeter.
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(365,38,0,0);
FTImpl.Vertex2ii(365,238,0,0); // this is a 200 pixel long line, with a 2000 foot range, so we will get 10 feet per pixel...
//now lay out the tics
int Nearestth = 500*(round(CurrentAltitude/500)); //This finds the nearest 500 unit mark.
int altoffset = (CurrentAltitude - Nearestth)/10; // this is in pixel units
int bottomshow = -1; int topshow = 3;
if (altoffset < -12) {topshow = 2; bottomshow = -2;};
for (int i = bottomshow; i < topshow; i++) //higher number shows on top
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(362,150+altoffset-(50*i),0,0);
FTImpl.Vertex2ii(367,150+altoffset-(50*i),0,0); // 10 tics
FTImpl.Cmd_Number(368, 150+altoffset-(50*i), 26, FT_OPT_CENTERY | FT_OPT_SIGNED, ((Nearestth)+(i*500)));
}
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(362,150,0,0);
FTImpl.Vertex2ii(380,150,0,0); // black tic
FTImpl.Vertex2ii(362,151,0,0);
FTImpl.Vertex2ii(380,151,0,0); // black tic
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(376,142,0,0);
FTImpl.Vertex2ii(408,159,0,0); // first the box for the hundreds and up
FTImpl.Vertex2ii(409,107,0,0);
FTImpl.Vertex2ii(427,194,0,0); // then the box for the cool number slider for the 20's...
FTImpl.ClearStencil(0); // set the stencil to blank
FTImpl.StencilOp(FT_INCR,FT_INCR);
FTImpl.Vertex2ii(411,108,0,0);
FTImpl.Vertex2ii(427,193,0,0); // then the box THAT DEFINES THE STENCIL ....
FTImpl.StencilOp(FT_KEEP,FT_KEEP); // and we are done building the stencil
if (CurrentAltitude >= 0)
{
FTImpl.ColorRGB(200,200,140); //set the color to Creamy White
int twenties = (CurrentAltitude - (100 * (int)(CurrentAltitude/100)))/20; // everything but the last 2 digits off/then divide by 20. the fact that it is an int does the rest
int teens = (CurrentAltitude - (100 * (int)(CurrentAltitude/100))); // everything but the last two digits are gone.
while (abs(teens) >= 20) teens = teens - 20; //Should knock us down to the number of feet less then 20. THis will be the numerical offset for the scroll
FTImpl.StencilFunc(FT_GREATER, 0, 255); //only operate on the stencil we just set up
for (int i = -1; i < 7; i++) // we are going to "print" 9 numbers onto the stencil
{
FTImpl.Cmd_Number(414, 190-(i*20)+(teens), 26, FT_OPT_CENTER | FT_OPT_SIGNED, 260+((i+twenties)*20)); // print the 20's, adjusted for height and for number
}
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
FTImpl.ColorRGB(255,255,255); // white me up
// now we do the other numbers as slow flipping sliders
// start with the hundreds. This will be the one to determine how all the numbers flip IF they do flip, so twentieths only need be calculated once.
int twentieths = 0; int tenthousands = 0; int thousands = 0; int hundreds = 0; int hundredsdigit = 0; int thousandsdigit = 0; int tenthousandsdigit = 0;
int Destructor = abs(CurrentAltitude); // we are going to take this sucker apart. get rid of the minus sign if needed
if (Destructor >= 10000) {tenthousandsdigit = Destructor/10000; Destructor = Destructor - (tenthousandsdigit * 10000);} // int gets rid of any extra. then we trim
if (Destructor >= 1000) {thousandsdigit = Destructor/1000; Destructor = Destructor - (thousandsdigit * 1000);} // int gets rid of any extra. then we trim
if (Destructor >= 100) hundredsdigit = Destructor/100; // int gets rid of any extra. no need to trim, as we dont care
Destructor = abs(CurrentAltitude); // reset the sucker. get rid of the minu sign if needed
tenthousands = Destructor - tenthousandsdigit*10000; Destructor = tenthousands;
thousands = Destructor - thousandsdigit*1000; Destructor = thousands;
hundreds = Destructor - hundredsdigit*100; // and I no longer give a rip about Destructor!
int topdigit = -1; int bottomdigit = -1;
if (abs(CurrentAltitude) > 80) // we have to put numbers in the left box
{
// we will start out with the hunderedsdigit
if (hundreds > 82) // we are flipping to the higher number
{
twentieths = hundreds - 82;
topdigit = hundredsdigit + 1;
bottomdigit = hundredsdigit;
if (CurrentAltitude < 100) bottomdigit = -1;
//if ((CurrentAltitude > 1050) and (topdigit == 10)) topdigit = 0;
FlipNum(409, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(400, 141, 27, 0, hundredsdigit); }; // we get to just print it out.
}
// now we do the thousands digit
if (abs(CurrentAltitude) > 982) // we have to put numbers in the left box
{
if (thousands > 982) // we are flipping to the higher number
{
topdigit = thousandsdigit + 1;
bottomdigit = thousandsdigit;
if (CurrentAltitude < 1000) bottomdigit = -1;
if ((CurrentAltitude > 9900) and (bottomdigit == 9)) topdigit = 10;
FlipNum(400, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(391, 141, 27, 0, thousandsdigit); }; // we get to just print it out.
}
// now we do the ten thousands digit
if (abs(CurrentAltitude) > 9982) // we have to put numbers in the left box
{
if (tenthousands > 9982) // we are flipping to the higher number
{
topdigit = tenthousandsdigit + 1;
bottomdigit = tenthousandsdigit;
if (bottomdigit == 0) bottomdigit = -1;
if (topdigit == 0) topdigit = -1;
FlipNum(391, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(382, 141, 27, 0, tenthousandsdigit); }; // we get to just print it out.
}
}
else // CurrentAltitude est negative !
{
FTImpl.ColorRGB(200,200,140); //set the color to Creamy White
FTImpl.Cmd_Text(386, 144, 27, FT_OPT_CENTER , "_");
int twenties = 20 - (CurrentAltitude - (100 * (int)(CurrentAltitude/100)))/20; // everything but the last 2 digits off/then divide by 20. the fact that it is an int does the rest
int teens = -1 * (CurrentAltitude - (100 * (int)(CurrentAltitude/100))); // everything but the last two digits are gone.
while (abs(teens) >= 20) teens = teens - 20; //Should knock us down to the number of feet less then 20. THis will be the numerical offset for the scroll
FTImpl.StencilFunc(FT_GREATER, 0, 255); //only operate on the stencil we just set up
for (int i = -4; i < 3; i++) // we are going to "print" 7 numbers onto the stencil
{
FTImpl.Cmd_Number(414, 190+(i*20)-(teens), 26, FT_OPT_CENTER | FT_OPT_SIGNED, 240+((i+twenties)*20)); // print the 20's, adjusted for height and for number
}
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
FTImpl.ColorRGB(255,255,255); // white me up
// now we do the other numbers as slow flipping sliders
// start with the hundreds. This will be the one to determine how all the numbers flip IF they do flip, so twentieths only need be calculated once.
int twentieths = 0; int tenthousands = 0; int thousands = 0; int hundreds = 0; int hundredsdigit = 0; int thousandsdigit = 0; int tenthousandsdigit = 0;
int Destructor = abs(CurrentAltitude); // we are going to take this sucker apart. get rid of the minus sign if needed
if (Destructor >= 10000) {tenthousandsdigit = Destructor/10000; Destructor = Destructor - (tenthousandsdigit * 10000);} // int gets rid of any extra. then we trim
if (Destructor >= 1000) {thousandsdigit = Destructor/1000; Destructor = Destructor - (thousandsdigit * 1000);} // int gets rid of any extra. then we trim
if (Destructor >= 100) hundredsdigit = Destructor/100; // int gets rid of any extra. no need to trim, as we dont care
Destructor = abs(CurrentAltitude); // reset the sucker. get rid of the minu sign if needed
tenthousands = Destructor - tenthousandsdigit*10000; Destructor = tenthousands;
thousands = Destructor - thousandsdigit*1000; Destructor = thousands;
hundreds = Destructor - hundredsdigit*100; // and I no longer give a rip about Destructor!
int topdigit = -1; int bottomdigit = -1;
if (CurrentAltitude < -80) // we have to put numbers in the left box
{
// we will start out with the hunderedsdigit
if (hundreds > 80) // we are flipping to the higher number
{
twentieths = (-1*hundreds) + 80;
topdigit = hundredsdigit;
bottomdigit = hundredsdigit + 1;
if (CurrentAltitude > -100) topdigit = -1;
//if ((CurrentAltitude > 1050) and (topdigit == 10)) topdigit = 0;
FlipNum(409, 143, topdigit, bottomdigit, (-1*twentieths)+1);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(400, 141, 27, 0, hundredsdigit); }; // we get to just print it out.
}
// now we do the thousands digit
if (CurrentAltitude < -980) // we have to put numbers in the left box
{
if (thousands > 980) // we are flipping to the higher number
{
topdigit = thousandsdigit;
bottomdigit = thousandsdigit + 1;
if (CurrentAltitude > -1000) topdigit = -1;
FlipNum(400, 143, topdigit, bottomdigit, (-1*twentieths)+1);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(391, 141, 27, 0, thousandsdigit); }; // we get to just print it out.
}
// there is no tenthousands digit in the negative range...
}
//** Overlay the Control Knob Functions
// trim gyro buttons
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Text(201, 260, 27, FT_OPT_CENTER, "Compass Roll Pitch");
// Sensor Error Flag
if (SensorError)
FTImpl.ColorRGB(200,0,0);//set the color to RED because we have a sensor error.
else
FTImpl.ColorRGB(170,170,170);//set the color to grey becuase our last sensor read was successful.
FTImpl.Cmd_Text(455, 12, 18, FT_OPT_CENTER, "Snsr");
FTImpl.Cmd_Text(455, 25, 18, FT_OPT_CENTER, "Err");
//** DRAW THIS LAST - The Menu items on the bottom of the screen.
if (Menu == 1)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // brightness box color
FTImpl.Begin(FT_LINE_STRIP); // *
FTImpl.Vertex2ii(3,250,0,0);
FTImpl.Vertex2ii(27,250,0,0);
FTImpl.Vertex2ii(27,269,0,0);
FTImpl.Vertex2ii(3,269,0,0);
FTImpl.Vertex2ii(3,250,0,0);
if (Menu == 2)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // speedunits box color
FTImpl.Begin(FT_LINE_STRIP); // BUG
FTImpl.Vertex2ii(31,250,0,0);
FTImpl.Vertex2ii(108,250,0,0);
FTImpl.Vertex2ii(108,269,0,0);
FTImpl.Vertex2ii(31,269,0,0);
FTImpl.Vertex2ii(31,250,0,0);
if (Menu == 3)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // bug box color
FTImpl.Begin(FT_LINE_STRIP); // Compass
FTImpl.Vertex2ii(112,250,0,0);
FTImpl.Vertex2ii(196,250,0,0);
FTImpl.Vertex2ii(196,269,0,0);
FTImpl.Vertex2ii(112,269,0,0);
FTImpl.Vertex2ii(112,250,0,0);
if (Menu == 4)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // trim box color
FTImpl.Begin(FT_LINE_STRIP); // Roll - where I was working
FTImpl.Vertex2ii(200,250,0,0);
FTImpl.Vertex2ii(238,250,0,0);
FTImpl.Vertex2ii(238,269,0,0);
FTImpl.Vertex2ii(200,269,0,0);
FTImpl.Vertex2ii(200,250,0,0);
if (Menu == 5)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // altimeter setting box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(242,250,0,0);
FTImpl.Vertex2ii(291,250,0,0);
FTImpl.Vertex2ii(291,269,0,0);
FTImpl.Vertex2ii(242,269,0,0);
FTImpl.Vertex2ii(242,250,0,0);
if (Menu == 6)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // pressure units box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(423,250,0,0);
FTImpl.Vertex2ii(295,250,0,0);
FTImpl.Vertex2ii(295,269,0,0);
FTImpl.Vertex2ii(423,269,0,0);
FTImpl.Vertex2ii(423,250,0,0);
if (Menu == 7)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // altitude units box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(427,250,0,0);
FTImpl.Vertex2ii(475,250,0,0);
FTImpl.Vertex2ii(475,269,0,0);
FTImpl.Vertex2ii(427,269,0,0);
FTImpl.Vertex2ii(427,250,0,0);
// ****************************** End My Routine *******************************
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
// }
}
void Calibrate_AHRS() // A la mise sous tension, le bouton a été enfoncé.
{
boolean LiftButton = false;
boolean PressButton = false;
boolean DoZero = false;
while (!PressButton) // this is the menu we will draw for the calibration screen
{
FTImpl.DLStart(); // begin a new drawing list
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100)); // go maximum brightness
FTImpl.ClearColorRGB(255,255,255); // set the background color to white
FTImpl.Clear(1,1,1); // clear the screen
FTImpl.ColorRGB(0,0,0); //set the color to Black.
FTImpl.Cmd_Text(240, 24, 28, FT_OPT_CENTER, "Zero the AHRS screen. Do not zero");
FTImpl.Cmd_Text(240, 46, 28, FT_OPT_CENTER, "the AHRS unless it is in level flight");
FTImpl.Cmd_Text(240, 68, 28, FT_OPT_CENTER, "attitude and facing 0 degrees due North.");
FTImpl.Cmd_Text(240, 90, 28, FT_OPT_CENTER, "It may be easier to zero the AHRS when");
FTImpl.Cmd_Text(240, 112, 28, FT_OPT_CENTER, "it is not mounted in the aircraft. In any");
FTImpl.Cmd_Text(240, 134, 28, FT_OPT_CENTER, "case, twist the knob to the right several");
FTImpl.Cmd_Text(240, 156, 28, FT_OPT_CENTER, "times until the ZERO option is selected");
FTImpl.Cmd_Text(240, 178, 28, FT_OPT_CENTER, "to perform a calibration.");
FTImpl.Cmd_Text(240, 250, 28, FT_OPT_CENTER, "Exit without saving. Zero AHRS and Save.");
// and now highlight the choice we have made
FTImpl.ColorRGB(0,0,0); //set the color to black.
FTImpl.Begin(FT_LINE_STRIP); // and get ready to draw a box
if (DoZero)
{ // draw a box around "Zero
FTImpl.Vertex2ii(232,239,0,0);
FTImpl.Vertex2ii(460,239,0,0);
FTImpl.Vertex2ii(460,261,0,0);
FTImpl.Vertex2ii(232,261,0,0);
FTImpl.Vertex2ii(232,239,0,0);
}
else
{ // draw a box around "Don't Zero
FTImpl.Vertex2ii(15,239,0);
FTImpl.Vertex2ii(230,239,0);
FTImpl.Vertex2ii(230,261,0,0);
FTImpl.Vertex2ii(15,261,0,0);
FTImpl.Vertex2ii(15,239,0,0);
}
// close out the screen
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
if (digitalRead(A15) == HIGH) LiftButton = true; // we won't sense the button press until this is so
if (((digitalRead(A15) == LOW) and (LiftButton))) PressButton = true; // we have lifted the button and pressed it again
NewEncoderPosition = myEnc.read(); // read the encoder
if (OldEncoderPosition - 50 < NewEncoderPosition) DoZero = false; else DoZero = true;
}
if (DoZero) // we need to tare the sensor and save the results
{
// Tare the Sensor
Serial2.write(247);// "Start of Packet Byte"
Serial2.write(96); // "Set Compass Range"
Serial2.write(96); // "Checksum" of the one byte we just sent.
// Save the new Zero
// Serial2.write(247); // "Start of Packet Byte"
// Serial2.write(105); // "Set Reference Vector Mode"
// Serial2.write(3); // "Range set to "Single Auto Continual" (2) Other possibilites include "Single Auto" (1) and "Multiple" (3)
// Serial2.write(107); // "Checksum" - sum of the last two bytes mod 256.
}
}
void setup()
{
String AHRS_Version = "N"; // Ceci est utilisé plus tard pour voir si nous sommes connectés à la puce AHRS
// set up the pins we will need to be using...
pinMode(A15,INPUT_PULLUP); // A 15 is the select button on the encoder
pinMode(19, INPUT_PULLUP); // 19 is an interrupt pin, and is used as the encoder quadrature signal A
pinMode(18, INPUT_PULLUP); // 18 is an interrupt pin, and is used as the encoder quadrature signal B
// start tracking the encoder knob
OldEncoderPosition = myEnc.read();
boolean agree = false;
boolean Iagree = false;
// enable the display
BootupConfigure();
//SPI.begin();
// Serial2 is the Remote AHRS sensor.
Serial2.begin(115200); // begin serial communications with Yost Labs 3-Space Nano AHRS Chip
// AHRS chip can be made to change speeds, but defaults to 115200. Mode is 8N1 (8 data, no parity, 1 stop) This is the arduino default.
delay(500); // give a bit of time to ensure everything is good.
while (Serial2.available()) Serial2.read(); // empty the receive serial port
// write a "get version" request to the AHRS chip
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(230); // "Get Version"
Serial2.write(230); // "Checksum" - in this case the exact same as the message, because it only sums the message byte.
if (!digitalRead(A15)) Calibrate_AHRS(); // this is the calibration routine
// begin a while loop here that draws the startup screen over and over until you select "I agree"
while (!Iagree)
{
if ((digitalRead(A15) == LOW) and (agree)) Iagree = true;
// start drawing the startup screen
FTImpl.DLStart(); // begin a new drawing list
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100)); // go maximum brightness
FTImpl.ClearColorRGB(0,0,0); // set the background color to black
FTImpl.Clear(1,1,1); // clear the screen
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Cmd_Text(240, 24, 28, FT_OPT_CENTER, "TheOpenCockpit Airduino 4.3 version 1.0 Beta");
FTImpl.Cmd_Text(240, 46, 28, FT_OPT_CENTER, "Power-Up Report and Legal Disclaimer");
if (!Static.begin()) { // initialize the Static Port // BMP280 setup
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 78, 26, FT_OPT_CENTER, "Static Port BMP 280 Sensor ......... NOT FOUND @ 0x77");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 78, 26, FT_OPT_CENTER, "Static Port BMP 280 Sensor ......... FOUND & 0x77");
}
if (!Pitot.begin(0x76)) { // initialize the Pitot Port // BMP280 setup
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 96, 26, FT_OPT_CENTER, "Pitot Port BMP 280 Sensor ......... NOT FOUND @ 0x76");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 96, 26, FT_OPT_CENTER, "Pitot Port BMP 280 Sensor ......... FOUND & 0x76");
}
if (Serial2.available() > 11) // it is time to read the AHRS response
{
AHRS_Version = "";
while (Serial2.available()) AHRS_Version = AHRS_Version + Serial2.read();
}
if (AHRS_Version == "N") {// we have not heard from the AHRS sensor
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 118, 26, FT_OPT_CENTER, "Searching for AHRS Sensor on UART2");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 118, 26, FT_OPT_CENTER, "Yost Labs AHRS Sensor .......... FOUND on UART2");
}
// Disclaimer
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Cmd_Text(240, 145, 27, FT_OPT_CENTER, "This instrument is experimental in nature, and should");
FTImpl.Cmd_Text(240, 165, 27, FT_OPT_CENTER, "be used to enhance situational awareness only. Any");
FTImpl.Cmd_Text(240, 185, 27, FT_OPT_CENTER, "and all use is at the operator's discretion and risk.");
// choices
FTImpl.Cmd_Text(240, 217, 27, FT_OPT_CENTER, "I do not agree. I agree.");
// more info
FTImpl.Cmd_Text(132, 250, 26, FT_OPT_CENTER, "For more information, please visit");
FTImpl.ColorRGB(100,100,255); //set the color to light blue.
FTImpl.Cmd_Text(348, 250, 26, FT_OPT_CENTER, "http://www.etna-soft.com");
// and now highlight the choice we have made
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Begin(FT_LINE_STRIP); // and get ready to draw a box
if (agree)
{ // draw a box around "agree"
FTImpl.Vertex2ii(278,206,0,0);
FTImpl.Vertex2ii(358,206,0,0);
FTImpl.Vertex2ii(358,229,0,0);
FTImpl.Vertex2ii(278,229,0,0);
FTImpl.Vertex2ii(278,206,0,0);
}
else
{ // draw a box around "I do not agree"
FTImpl.Vertex2ii(120,206,0,0);
FTImpl.Vertex2ii(252,206,0,0);
FTImpl.Vertex2ii(252,229,0,0);
FTImpl.Vertex2ii(120,229,0,0);
FTImpl.Vertex2ii(120,206,0,0);
}
// close out the startup screen
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
NewEncoderPosition = myEnc.read(); // read the encoder
if (OldEncoderPosition != NewEncoderPosition) // the knob has moved
if (agree) agree = false; else agree = true; // so we need to change our choice
OldEncoderPosition = NewEncoderPosition;
// end the while loop here, so that you cannot move on without selecting "I Agree"
}
// now set up the AHRS chip
// Set Accelerometer range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(121); // "Set Accelerometer Range"
Serial2.write(16); // "Range set to +- 4G" Could be 2G (0) 4G (16) and 8G (32)
Serial2.write(137); // "Checksum" - sum of the last two bytes mod 256.
// Set Gyroscope range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(125); // "Set Gyroscope Range"
Serial2.write(1); // "Range set to +- 500dps" Could also be 250 (0) and 2000 (2)
Serial2.write(126); // "Checksum" - sum of the last two bytes mod 256.
// Set Compass range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(126); // "Set Compass Range"
Serial2.write(1); // "Range set to +- 1.33Ga" Could also be +-.88 Ga (0) and many more
Serial2.write(127); // "Checksum" - sum of the last two bytes mod 256.
// Set Reference Vector Mode
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(105); // "Set Reference Vector Mode"
Serial2.write(3); // "Range set to "Single Auto Continual" (2) Other possibilites include "Single Auto" (1) and "Multiple" (3)
Serial2.write(107); // "Checksum" - sum of the last two bytes mod 256.
// setup variables from defined values so that this doesn't have to be done over and over again
BotWhA = map(WAStart, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
TopWhA = map(WAEnd, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
BotGA = map(GAStart, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
TopGA = map(GAEnd, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
RL = map(RLine, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
BL = map(BLine, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: PressureDigits[0] = 2; PressureDigits[1] = 9; PressureDigits[2] = 9; PressureDigits[3] = 2; break; // inHg
case 2: PressureDigits[0] = 7; PressureDigits[1] = 6; PressureDigits[2] = 0; PressureDigits[3] = 0; break; // mmHg
case 3: PressureDigits[0] = 1; PressureDigits[1] = 0; PressureDigits[2] = 1; PressureDigits[3] = 8; break; } // hPa
NumTics = MaxSpeed/SpeedTics; // This is how many tics we will need
HalfSpeedTics = (map(SpeedTics,0,MaxSpeed,0,195)/2); // 195 is the number of pixels it displays
CurrentSpeed = 84;
CurrentRoll = -0.5236; // Radians. 30 degrees to the left
CurrentPitch = -0.17453/2; // Radians. 10 degrees up
} // Fin setup()
void loop()
{
// Calcul Altitude
PreviousAltitude = CurrentAltitude;
float AltCallUnits = CurrentPressure;
if (PressureUnits == 1) AltCallUnits = CurrentPressure * 25.4 * 1.3332; // inHg à mmHg à hPa
if (PressureUnits == 2) AltCallUnits = CurrentPressure * 1.3332; // mmHg à hPa
if (AltUnits == 1)CurrentAltitude = (3.28*Static.readAltitude(AltCallUnits+StaticCalibrationOffset)); // Pression en hPa et Altitude en Ft
else
CurrentAltitude = (Static.readAltitude(AltCallUnits+StaticCalibrationOffset)); // Pression hPa et Altitude en metres
// Calcul Airspeed
float CalcSpeed = 0;
switch(SpeedUnits) { // Choisir les unités pour avoir KTS, m/h ou Km/h
// case 1: CalcSpeed = (59.2484 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 1.15); break; // kts/hr
// case 2: CalcSpeed = (68.1818 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset); break; // m/h
// case 3: CalcSpeed = (109.7280 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 0.62); break; } // km/h
case 1: CalcSpeed = (0.592484 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 1.15); break; // kts/hr
case 2: CalcSpeed = (0.681818 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset); break; // m/h
case 3: CalcSpeed = (1.097280 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 0.62); break; } // km/h
CurrentSpeed = CalcSpeed;
// handle the input encoder section. This is activated by raising the button.
NewEncoderPosition = myEnc.read();
EncMove = (OldEncoderPosition - NewEncoderPosition)/ECPD;
if (ButtonDown and digitalRead(A15)) ButtonReleased = true;
ButtonDown = !digitalRead(A15);
if (ButtonReleased and !MenuSelected) { MenuSelected = true; ButtonReleased = false; }
int oldunits;
if (MenuSelected) // we are in a menu, and need to handle it
{
if (ButtonReleased) // anything that happens when we leave a menu needs to happen now
{
MenuSelected = false; // this has to happen
}
switch(Menu) {
case 1: // brightness
if (ButtonDown) EncMult = 25; else EncMult = 3; // higher speed if you hold down the button
brightness = brightness + (EncMove * EncMult);
if (brightness > 255) brightness = 255;
if (brightness < 6) brightness = 6; //don't want to be so low that you cant see to brighten it back up
break;
case 2: // bug
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
HeadingBug = HeadingBug + (EncMove * EncMult);
if (HeadingBug > 360) HeadingBug = HeadingBug - 360;
if (HeadingBug < 0) HeadingBug = HeadingBug + 360; // corresponds to degrees. Wraps around the 360 point
break;
case 3: // compass
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
CompassOffset = CompassOffset + (EncMove * EncMult * 0.5); // 0.5 is a half degree in degrees ;-)
break;
case 4: // Roll
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
RollOffset = RollOffset + (EncMove * EncMult * 0.00872); // 0.00872 is a half degree in Radians
break;
case 5: // Pitch
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
PitchOffset = PitchOffset + (EncMove * EncMult * 0.00872); // 0.00872 is a half degree in Radians
break;
case 6: // altimeter
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/100); break; // inHg
case 2: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/10); break; // mmHg
case 3: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/10); break; } // hPa
break;
case 7: // altimeter units
if (EncMove != 0) // we gonna covert us some units
if (EncMove > 0) // we are going up
{
if (PressureUnits == 1) CurrentPressure = CurrentPressure * 25.4; // Converting inHg to mmHg
if (PressureUnits == 2) CurrentPressure = CurrentPressure * 1.3332; // Converting mmHg to hPa
} // note - not conversion necessary for 3 becuase we have no other units
else // we are going down
{ // and again, we aren't going down farther if we are at 1...
if (PressureUnits == 2) CurrentPressure = CurrentPressure / 25.4; // Converting mmHg to inHg
if (PressureUnits == 3) CurrentPressure = CurrentPressure / 1.3332; // Converting hPa to mmHg
}
PressureUnits = PressureUnits + EncMove;
if (PressureUnits > 3) PressureUnits = 3;
if (PressureUnits < 1) PressureUnits = 1; // corresponds to altimeter units. no wrap
break; } // and close the switch
if (ButtonDown and ButtonReleased) {MenuSelected = false; } // you just moved out of whatever menu you are in
}
else // we are not in a menu, and the encoder has turned to switch between menus
{
Menu = Menu + EncMove;
if (Menu > 7) Menu = 7;
if (Menu < 1) Menu = 1;
}
ButtonReleased = false;
OldEncoderPosition = NewEncoderPosition; // Fin de la routine du menu
PreviousHeading = CurrentHeading;
// Gérer maintenant le calcul du temps qui nous permet de calculer le taux de virage TC et VSI
previousMicros = currentMicros;
currentMicros = micros();
intervalMicros = currentMicros - previousMicros;
// ROUTINE STILL NEEDS CALIBRATED AND SMOOTHED.
// ROUTINE DES DONNEES COURANTES QUI ONT BESOIN TOUJOURS D'ETRE CALIBRES ET LISSES.
// Je pense que j'ai aussi besoin d'une horloge plus précise.
// Il peut également être nécessaire d'étudier le réglage des modes de puce BNO055.
CurrentRateOfTurn = 500*(float)(CurrentHeading - PreviousHeading)/(intervalMicros);
// Lecture des angles Euler du paquet d'octets du AHRS
if (Serial2.available() > 11)
{ // Nous avons un paquet d'octets, proceder au decodage
//Serial2.read(); N'est plus nécessaire
CP.binary[3] = Serial2.read(); CP.binary[2] = Serial2.read(); CP.binary[1] = Serial2.read(); CP.binary[0] = Serial2.read(); // Va aller dans CurrentHeading
CH.binary[3] = Serial2.read(); CH.binary[2] = Serial2.read(); CH.binary[1] = Serial2.read(); CH.binary[0] = Serial2.read(); // Va aller dans CurrentPitch
CR.binary[3] = Serial2.read(); CR.binary[2] = Serial2.read(); CR.binary[1] = Serial2.read(); CR.binary[0] = Serial2.read(); // Va aller dans CurrentRoll
// CalibrationStatus = Serial2.read(); N'est plus disponible
CurrentHeading = CH.floatingPoint * 57.2958 + CompassOffset; // Note: HDG est en degrés, Pitch et Roll en radians.
CurrentPitch = CP.floatingPoint + PitchOffset; //- (0.008726 * 26.0); // Pitch en radians.
CurrentRoll = -1*CR.floatingPoint + RollOffset; //+ (0.008726 * 4.5); // Roll en radians.
if (CurrentHeading > 360) CurrentHeading = CurrentHeading - 360; // Correction pour toutes les valeurs hors limites.
if (CurrentHeading <= 0) CurrentHeading = CurrentHeading + 360; // Correction pour toutes les valeurs hors limites.
SensorError = false; // Nous avons pu faire une lecture
ErrorStreak = 0; // et cela met fin à toute "séquence" de mauvaises lectures ...
}
else
{
SensorError = true; // Nous avons donc échoué à lire le capteur
ErrorStreak = ErrorStreak + 1; // et prolongé toute séquence d'échecs qui pourrait se produire
}
if (ErrorStreak > 25) ErrorStreak = 25; // Cela indique à l'écran de se déconnecter en cas d'erreur de capteur à long terme
// Section de correction et de demande de données série - Communication avec la puce AHRS
while (Serial2.available()) Serial2.read(); // Vider toutes les erreurs de réception du port série. Cela doit être fait afin de restaurer les communications perdues.
Serial2.write(247); // Start of Packet Byte
Serial2.write(1); // Command Byte - "Read Filtered, Tared Orientation data in Euler Angle Format"
Serial2.write(1); // Checksum Byte - in this case the exact same as the message, because it only sums the message byte.
// Mise à jour l'écran
DrawScreen();
} // Fin loop()
Nothing works, can you help me and see where I was wrong?
Regards,
Alfred
My Arduino code:
#include "SPI.h"
#include "Wire.h"
//#include "FT_NHD_43CTP_SHIELD.h"
#include "FT_NHD_43RTP_SHIELD.h"
#include <Encoder.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP280.h>
/*****************************************************************************
*
* Cette section contient des instructions "define" qui sont utilisées pour configurer
* unité pour votre application particulière. Sauf si vous êtes programmeur, ce
* est la seule partie du code que vous devez modifier. Veuillez consulter le
* manuel pour plus de détails au-delà des commentaires. Vous changez les chiffres et
* valeurs true ou false uniquement. Ne changez rien d'autre.
*
******************************************************************************/
#define MaxVertSpeed 4000 // will determine the size of the VSI scale on the indicator.
#define PrefSpeedUnits 3 //1 is kts, 2 is mph (defaut), 3 is km/h
#define PrefPressureUnits 3 // 1 is inHG (defaut), 2 is mmHg, 3 is hPa
#define PrefAltUnits 1 // 1 is ft (defaut), 2 is meters
#define WAStart 45 // the low side of the airspeed indicator white arc in PrefSpeedUnits
#define WAEnd 100 // the high side of the airspeed indicator white arc
#define GAStart 52 // the low side of the airspeed indicator green arc
#define GAEnd 116 // the high side of the airspeed indicator green arc
#define BLine 0 // the airspeed indicator blue line. Use 0 if you do not need a blue line.
#define RLine 142 // the airspeed indicator red line.
#define MaxSpeed 250 // this is the top number on the gauge. This should be a little higher than your red line.
#define SpeedTics 50 // this determines how many ticks will print out on the speed slider
#define SpeedSlider false //false will give gauge like readout. True gives ticker type readout
#define HeadingNum false // Black box with the heading on the DG strip
#define NumAverageTrend 10 // Higher numbers are less jittery. Lower numbers are more responsive. Applies to VSI and Rate of Turn
#define ECPD 2 // (4 defaut) number of ecoder clicks per detent. Probably 1, 2, or 4.
#define StartMenu 6 // defines where the cursor starts. 6 is the Altimeter Adjust
#define StartBrightness 255 // how bright the unit is on power-on. 255 is full bright. 0 is completely dark.
#define StartHeadingBug 360 // where the heading bug shows up.
#define StartingPressure 1013.25 // 29.92 (defaut) Put this as the pressure in your preferred unit system. 29.92, 1013.25, 760.0;
#define StaticCalibrationOffset 3 // This is a value used to calibrate your static BMP280. Start with 0, and make tiny changes. Each 1 you add adds about 25 feet (8 meters). Calibrate this one first.
#define AirspeedCalibrationOffset 0 // This is a value used to calibrate your airspeed. Start with 0. Calibrate this one after the Static. Each one is 1 mph.
#define CenterDisplayType 1 // 1 is yellow line moving with the horizon. 2 Yellow lines that stay level. 3 gives yellow plane.
#define dynamicsky true // false does not change the display color based on pitch. true does. Sky gets deeper blue as you point up.
#define dynamicground false // false does not change the display color based on pitch. true does. Groiund gets reddish as you point down.
#define PermRollOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.)
#define PermPitchOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.) Use calibration routine instead.
#define PermCompassOffset 0 // this is used to calibrate for AHRS offsets and mounting. (Not used in this version.)
// Used in Serial Data Transfer. This binaryInt allows the sensor to recieve floating point numbers and automatically decode them
typedef union {
float floatingPoint;
byte binary[4];
}
binaryFloat;
binaryFloat CH;
binaryFloat CP;
binaryFloat CR;
byte CalibrationStatus = 0;
// end data types and variables used in serial transfer of data .
int BotWhA;
int TopWhA;
int BotGA;
int TopGA;
int RL;
int BL;
int NumTics;
int TicMap;
int HalfSpeedTics;
int CurrentSpeed;
float CurrentRoll; // angle in radians from -Pi to + Pi. - is to the left
float CurrentPitch; // angle in radians from -Pi to + Pi. - is down
float CurrentHeading = 0; // this will actually come from the IMU unit, but we will start with a number
float CurrentRateOfTurn = -0.0115; // this is the number of radians per second. Number is for test purposes only, and cooresponds to a 2 minute left turn
float CurrentRateOfClimb = 0;
float CurrentAltitude = 400;
float CurrentGs = 1;
float PreviousHeading = 0;
float PreviousAltitude = 0;
float CurrentPressure = StartingPressure; // needed to switch on the fly.
int HeadingBug = StartHeadingBug;
int DispDegs;
int brightness = StartBrightness; //Max = 255, min = 0
int SpeedUnits = PrefSpeedUnits;
int AltUnits = PrefAltUnits;
int PressureUnits = PrefPressureUnits;
int PressureDigits[] = {2,9,9,2};
long OldEncoderPosition = -999;
long NewEncoderPosition = -999;
int Menu = StartMenu; // 1 is brightness, 2 is speed units, 3 is bug, 4 is trim, 5 is pressure, 6 is pressure units, 7 is alt units
boolean MenuSelected = false;
boolean ButtonDown = false;
boolean ButtonReleased = false;
int EncMove = 0;
int EncMult = 1;
unsigned long currentMicros; // = micros();
unsigned long previousMicros;
unsigned long intervalMicros;
int eraseme = 0;
boolean SensorError = false;
int ErrorStreak = 0;
float RollOffset = PermRollOffset;
float PitchOffset = PermPitchOffset;
float CompassOffset = PermCompassOffset;
Encoder myEnc(19, 18); // these pins are interrupt capable. Reverse these if your encoder moves the menus the wrong way.
Adafruit_BMP280 Static; // I2C communication with the static
Adafruit_BMP280 Pitot; // I2C communication with the static
/* Global object for transport */
//FT801IMPL_SPI FTImpl(FT_CS_PIN,FT_PDN_PIN,FT_INT_PIN);
FT800IMPL_SPI FTImpl(FT_CS_PIN,FT_PDN_PIN,FT_INT_PIN);
/* Api to bootup FT800 or FT801, verify FT800 or FT801 hardware and configure display/audio pins */
/* Returns 0 in case of success and 1 in case of failure */
int16_t BootupConfigure(){
uint32_t chipid = 0;
FTImpl.Init(FT_DISPLAY_RESOLUTION);//configure the display to the WQVGA
delay(20);//for safer side
chipid = FTImpl.Read32(FT_ROM_CHIPID);
/* Identify the chip */
// if(FT801_CHIPID != chipid)
if(FT800_CHIPID != chipid)
{
Serial.print("Error in chip id read ");
Serial.println(chipid,HEX);
return 1;
}
/* Set the Display & audio pins */
FTImpl.SetDisplayEnablePin(FT_DISPENABLE_PIN);
FTImpl.SetAudioEnablePin(FT_AUDIOENABLE_PIN);
FTImpl.DisplayOn();
FTImpl.AudioOn();
return 0;
}
void FlipNum(int Xloc, int Yloc, int TopDigit, int BottomDigit, int Twentiethsflip)
{ // This function is called from within a drawing list, and paints a 10 by 15 block with a rolling number display
// location is upper right referenced. The requested area of the screen is blacked and blanked. This cannot be
// used for more than two digits at a time. Twentiethsflip is the amount that the numbers have slid - as in 5/20ths
// 0/20ths prints just the TopDigit. 20/20ths prints just the BottomDigit
// use for the TopDigit or the BottomDigit if you want them to show up as blank.
int NumDigits = 1;
if ((BottomDigit >= 10) or (TopDigit >= 10)) NumDigits = 2;
FTImpl.ColorRGB(0,0,0); // set the color to Black
FTImpl.ClearStencil(0); // set the stencil to blank
FTImpl.StencilOp(FT_INCR,FT_INCR); // set the stencil operation to mask the blank
FTImpl.Begin(FT_RECTS); // get ready to make the box
FTImpl.Vertex2ii(Xloc,Yloc,0,0);
FTImpl.Vertex2ii(Xloc - (10 * NumDigits),Yloc + 16,0,0); // then draw the box that blanks the space and DEFINES THE STENCIL ....
FTImpl.StencilOp(FT_KEEP,FT_KEEP); // and we are done building the stencil. Clear the stencil creating function
FTImpl.ColorRGB(255,255,255); // set the color to White
FTImpl.StencilFunc(FT_GREATER, 0, 255); // and olnly draw in the stencil
if (TopDigit != -1) FTImpl.Cmd_Number(Xloc, Yloc-Twentiethsflip-1, 27, FT_OPT_RIGHTX, TopDigit); // print the part of the numbers that shows up in the box
if (BottomDigit != -1) FTImpl.Cmd_Number(Xloc, Yloc+19-Twentiethsflip, 27, FT_OPT_RIGHTX, BottomDigit); // print the part of the numbers that shows up in the box
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
}
void DrawScreen()
{
//** FTImpl.SetCTouchMode(FT_CTOUCH_MODE_EXTENDED); //set mode to extended for FT801
int16_t xoffset,yoffset;
xoffset = ((FT_DISPLAYWIDTH - 480)/2);
yoffset = ((FT_DISPLAYHEIGHT - 272)/2);
//** Begin building angle gauge stencil
FTImpl.DLStart();
// this should set the screen brightness
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100));
FTImpl.ClearColorRGB(0,0,0);
FTImpl.Clear(1,0,1);
FTImpl.ClearStencil(0);
FTImpl.StencilOp(FT_INCR,FT_INCR);
// Draw the rays
FTImpl.LineWidth(6);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(145,70,0,0);
FTImpl.Vertex2ii(157,81,0,0);
FTImpl.Vertex2ii(165,61,0,0);
FTImpl.Vertex2ii(171,67,0,0);
FTImpl.Vertex2ii(178,43,0,0);
FTImpl.Vertex2ii(186,56,0,0);
FTImpl.Vertex2ii(200,41,0,0);
FTImpl.Vertex2ii(203,49,0,0);
FTImpl.Vertex2ii(220,36,0,0);
FTImpl.Vertex2ii(221,44,0,0);
FTImpl.Vertex2ii(260,36,0,0);
FTImpl.Vertex2ii(259,44,0,0);
FTImpl.Vertex2ii(280,41,0,0);
FTImpl.Vertex2ii(277,49,0,0);
FTImpl.Vertex2ii(302,43,0,0);
FTImpl.Vertex2ii(294,56,0,0);
FTImpl.Vertex2ii(315,61,0,0);
FTImpl.Vertex2ii(309,67,0,0);
FTImpl.Vertex2ii(335,70,0,0);
FTImpl.Vertex2ii(323,81,0,0);
// Draw a circle centered on 240, 150.
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.PointSize(110*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.StencilOp(FT_ZERO,FT_ZERO);
FTImpl.ColorRGB(0,0,0);//set the color of the string to BLACK color
FTImpl.PointSize(107 * 16);//inner circle
FTImpl.Vertex2ii(240,150,0,0);
//Trim the bottom of the circle off
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(120,82,0,0);
FTImpl.Vertex2ii(360,270,0,0);
FTImpl.StencilOp(FT_KEEP,FT_KEEP);
//** Stencil complete. Clear Screen to Sky Color
int darkensky = 0;
if (dynamicsky) darkensky = (int)(70*sin(CurrentPitch)); // darkensky won't change the sky's color unless dynamicsky is chosen
if (darkensky > 0) darkensky = 0;
FTImpl.ClearColorRGB(150+darkensky,150+darkensky,255);
FTImpl.Clear(1,0,0); // the zero leaves the stencil in place
//** Math work in progress. Draw in a red line. First, use only shallow angles and assume rightside up
FTImpl.LineWidth(12);
int darkenground = 0;
if (dynamicground) darkenground = (int)(60*sin(CurrentPitch)); // darkenground won't change the ground's color unless dynamicground is chosen
if (darkenground < 0) darkenground = 0;
FTImpl.ColorRGB(120+darkenground,80-(darkenground/2),20-(darkenground/6)); //);//set the color to brown, with a redish adjustment as you head down,
int LS; // left side intersect
int RS; // right side intersect
int LO = 0; // Left Offset - used when the line does not extend across the screen
int RO = 0; // Right Offset - used when the line does not extend across the screen
float CXP; // Center X Position
float CYP; // Center Y Pos
float TanCurrentRoll = tan(CurrentRoll); // Do math as few times as possible
float SinCurrentRoll = sin(CurrentRoll);
float CosCurrentRoll = cos(CurrentRoll);
CXP = 240-SinCurrentRoll*((CurrentPitch/.0175)*3.6);
CYP = 150-CosCurrentRoll*((CurrentPitch/.0175)*3.6);
if ((CXP >= 0) and (CXP <= 480)) // the center of the line is on the screen
{
LS = CYP+(CXP*TanCurrentRoll);
RS = CYP-((480-CXP)*TanCurrentRoll);
if (LS < 0) // The line begins above the display.
{LO = CXP-((CYP*CXP)/(CYP-LS)); LS = 0;} // similar triangles
else if (LS > 272) // The line begins below the display.
{LO = CXP-((CXP*(272-CYP))/(LS-CYP)); LS = 272;} // similar triangles
if (RS < 0) // The line ends above the display
{RO = (480-CXP)-(((480-CXP)*CYP)/(CYP-RS)); RS = 0;} // similar triangles
else if (RS > 272) // The line ends below the display
{RO = (480-CXP)-(((480-CXP)*(270-CYP))/(RS-CYP)); RS = 272;} // similar triangles
FTImpl.Begin(FT_EDGE_STRIP_B); // the use of 4 points ensures complete usability
FTImpl.Vertex2ii(0,LS,0,0);
FTImpl.Vertex2ii(0+LO,LS,0,0);
FTImpl.Vertex2ii(480-RO,RS,0,0);
FTImpl.Vertex2ii(480,RS,0,0);
}
// VERY IMPORTANT. The ground should not show up if the sensor is offline.
if (ErrorStreak > 16) // set to about the number of frames per second...
{
FTImpl.LineWidth(24);
FTImpl.ClearColorRGB(0,0,0); // set the color to black
FTImpl.Clear(1,0,0); // the zero leaves the stencil in place, but clears the screen to black
FTImpl.ClearColorRGB(255,0,0); // set the color to red
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(140,20,0,0);
FTImpl.Vertex2ii(340,252,0,0); // Draw the gigantic red x that says not to navigate...
FTImpl.Vertex2ii(140,252,0,0);
FTImpl.Vertex2ii(340,20,0,0);
}
//** Reprint Angle Gauge from Stencil
FTImpl.StencilFunc(FT_NOTEQUAL, 0, 255); //only operate on the stencil
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.PointSize(126*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //clear the stencil function
//** Add angle numbers to the stencil gauge
FTImpl.Cmd_Number(140 , 62, 26, FT_OPT_CENTER, 50);
FTImpl.Cmd_Text(160 , 36, 26, FT_OPT_CENTER, "2 Min (30)");
FTImpl.Cmd_Number(305 , 36, 26, FT_OPT_CENTER, 30);
FTImpl.Cmd_Number(340 , 62, 26, FT_OPT_CENTER, 50);
// FTImpl.ColorRGB(255,255,255);//set the color of the string back to WHITE color
FTImpl.LineWidth(16);
//** Given CurrentHeading (float), overlay the DG Strip
// Display is 480 pixels wide, so the center is at 240. 5 pixels is one degree
// Draw the line strip line
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(75,22,0,0);
FTImpl.Vertex2ii(405,22,0,0);
int Nearest5 = round(CurrentHeading/5) * 5;
int Nearest10 = round(CurrentHeading/10) * 10;
boolean NumTic = false;
if (Nearest5 == Nearest10) NumTic = true;
for (int degs = Nearest5 - 30; degs <= (Nearest5 + 30); degs = degs + 5)
{
DispDegs = degs;
if (DispDegs > 360) DispDegs = DispDegs - 360;
if (DispDegs < 1) DispDegs = DispDegs + 360;
if (NumTic) // This one is divisible by 10, and gets a number and a tic
{
NumTic = false;
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,25,0,0);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,22,0,0);
FTImpl.Cmd_Number( (degs-(CurrentHeading-40))*5+40, 13, 26, FT_OPT_CENTER, DispDegs);
}
else // This one is between numbers, and gets a tic only
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,25,0,0);
FTImpl.Vertex2ii((degs-(CurrentHeading-40))*5+40,15,0,0);
NumTic = true;
}
}
// Now draw the white arrow for no rate of turn
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(240,0,0,0);
FTImpl.Vertex2ii(240,41,0,0);
FTImpl.Vertex2ii(238,29,0,0);
FTImpl.Vertex2ii(242,29,0,0);
FTImpl.Vertex2ii(240,41,0,0);
FTImpl.Vertex2ii(236,27,0,0);
FTImpl.Vertex2ii(240,31,0,0);
FTImpl.Vertex2ii(244,27,0,0);
FTImpl.Vertex2ii(240,41,0,0);
// if option is selected, draw the Heading Number
if (HeadingNum)
{
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(160,160,160);//set the color to White
FTImpl.Vertex2ii(240-18, 0);
FTImpl.Vertex2ii(240+18,15);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(240-16, 0);
FTImpl.Vertex2ii(240+16,13);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Number(240, 6, 27, FT_OPT_CENTER, CurrentHeading);
}
// Draw heading Bug if it shows up, and the Bug setting Regardless
FTImpl.Cmd_Text(43, 260, 27, FT_OPT_CENTER, "* Bug:");
FTImpl.Cmd_Number(87, 260, 27, FT_OPT_CENTER, HeadingBug);
FTImpl.ColorRGB(255,165,4);//set the color to Orange
//FTImpl.Cmd_Number(159, 260, 27, FT_OPT_CENTER, difference);
int BugCalc = HeadingBug;
if ((CurrentHeading < 41) and (HeadingBug > 319)) BugCalc = BugCalc - 360;
if ((CurrentHeading > 319) and (HeadingBug < 41)) BugCalc = BugCalc + 360;
if (abs(CurrentHeading-BugCalc) < 33) // it should be on the strip, so lets draw this sucker! - was 41
{
FTImpl.Begin(FT_LINES);
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(240-((CurrentHeading-BugCalc)*5),23,0,0);
FTImpl.Vertex2ii(240-((CurrentHeading-BugCalc)*5),2,0,0);
}
//** End overlay the DG Strip
//** Overlay Brightness Slider
// remember brightness is 0-255
FTImpl.ColorRGB(128,128,128);//set the color of the string to GREY color
FTImpl.LineWidth(48);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(20,235,0,0);
FTImpl.Vertex2ii(20,235-(255*.76),0,0); //255 scaled by 76%
FTImpl.PointSize(10*16);//circle
FTImpl.Begin(FT_POINTS);
FTImpl.Vertex2ii(20,235-(brightness*.76),0,0); // this is the actual dot
FTImpl.ColorRGB(255,255,255);//set the color of the string to WHITE color
FTImpl.PointSize(8*16);//circle
FTImpl.Vertex2ii(20,235-(brightness*.76),0,0); // this is the actual dot
// include the Empty Battery Icon. This is greyed out in this version, but in the next version with the board, it will
// be a live icon showing charging and discharging...
FTImpl.ColorRGB(170,170,170);//set the color to GREY
FTImpl.LineWidth(32);
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(9,8,0,0);
FTImpl.Vertex2ii(32,17,0,0);
FTImpl.Vertex2ii(30,11,0,0);
FTImpl.Vertex2ii(35,14,0,0);
//FTImpl.ColorRGB(150,150,255);//set the color to SKY
FTImpl.Vertex2ii(11,10,0,0);
FTImpl.Vertex2ii(30,15,0,0);
FTImpl.ColorRGB(170,170,170);//set the color to GREY
// WILL NEED to read battery info on pin A2, charge status on pin ?, and charging status on pin ? These are provided on the future board.
FTImpl.Vertex2ii(13,12,0,0);
FTImpl.Vertex2ii(28,13,0,0); // for now, just put a greyed out spot in the icon.
//** Overlay Speed Strip - this will likely change to a software selectable optional slider and ticker
// print out units...
switch (SpeedUnits) { // overlay the Speed Units. These can change on the fly...
case 1: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "kts"); break;
case 2: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "mph"); break;
case 3: FTImpl.Cmd_Text(55, 17, 27, FT_OPT_CENTER, "km/h"); break; }
if (!SpeedSlider)
{// this is the gauge type speed readout
// lay out the speed line
FTImpl.LineWidth(16);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(80,235,0,0);
FTImpl.Vertex2ii(80,40,0,0);
// lay out the color bands
// White Arc
FTImpl.LineWidth(40);
FTImpl.Vertex2ii(78,235-BotWhA,0,0);
FTImpl.Vertex2ii(78,235-TopWhA,0,0);
// Yellow Line
FTImpl.LineWidth(24);
FTImpl.ColorRGB(255,232,0);//set the color of the string to YELLOW color
FTImpl.Vertex2ii(82,235-TopGA,0,0);
FTImpl.Vertex2ii(82,235-RL,0,0);
// Green Line
FTImpl.ColorRGB(0,185,0);//set the color of the string to Green color
FTImpl.Vertex2ii(82,235-TopGA,0,0);
FTImpl.Vertex2ii(82,235-BotGA,0,0);
// Red Line
FTImpl.LineWidth(24);
FTImpl.ColorRGB(185,0,0);//set the color of the string to Red color
FTImpl.Vertex2ii(84,235-RL,0,0);
FTImpl.Vertex2ii(76,235-RL,0,0);
// Blue Line
if (BL > 5)
{ // only draw it if we need it
FTImpl.LineWidth(24);
FTImpl.ColorRGB(32,32,185);//set the color of the string to Blue color
FTImpl.Vertex2ii(84,235-BL,0,0);
FTImpl.Vertex2ii(76,235-BL,0,0);
}
// lay out the tic marks and scale text
int count = 0;
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color of the string to White color
for (int tics = 0; tics <= NumTics; tics++)
{
TicMap = map(tics*SpeedTics,0,MaxSpeed,0,195); // 195 is the number of pixels it displays
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(81, 235-TicMap);
FTImpl.Vertex2ii(74,235-TicMap);
FTImpl.Cmd_Number(71, 227-TicMap, 26, FT_OPT_RIGHTX, tics*SpeedTics);
if (tics != 0)
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(80, 235-TicMap+HalfSpeedTics);
FTImpl.Vertex2ii(70,235-TicMap+HalfSpeedTics);
}
}
// lay out the current speed tic and number
TicMap = map(CurrentSpeed,0,MaxSpeed,0,195); // 195 is the number of pixels it displays
FTImpl.Vertex2ii(89, 235-TicMap);
FTImpl.Vertex2ii(78,235-TicMap);
FTImpl.Begin(FT_RECTS);
if (CurrentSpeed > 99)
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(125,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(123,241-TicMap);
}
else if (CurrentSpeed > 9)
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(113,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(111,241-TicMap);
}
else
{
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Vertex2ii(90, 226-TicMap);
FTImpl.Vertex2ii(102,243-TicMap);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(92, 228-TicMap);
FTImpl.Vertex2ii(100,241-TicMap);
}
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Number(92, 225-TicMap, 27, 0, CurrentSpeed);
}
else
{
// this routine still needs to be written, but I am not in a hurry for it!
}
//** Overlay the Angle Gauge, based on CurrentRoll
// this will involve angle calculations.
// the Center Strips, which can be one of three styles.
int XOffset1 = 0; int YOffset1 = 0; int XOffset2 = 0; int YOffset2 = 0; int XOffset3 = 0; int YOffset3 = 0;
if (CenterDisplayType == 1) // this is the standard yellow lines move with the horizon
{
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(24);
FTImpl.Begin(FT_LINES);
XOffset2 = 15*sin(CurrentRoll+(-1.5708));
YOffset2 = 15*cos(CurrentRoll+(-1.5708));
XOffset3 = 45*sin(CurrentRoll+(-1.5708));
YOffset3 = 45*cos(CurrentRoll+(-1.5708));
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0); // left
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0);
FTImpl.Vertex2ii(240+XOffset3,150+YOffset3,0,0); // right
}
else if (CenterDisplayType == 2) // the yellow lines stay straight
{
FTImpl.ColorRGB(0,0,0);//set the color to black
FTImpl.LineWidth(80);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(48);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(64);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
}
else // CenterDisplayType == 3 - basically does both
{
FTImpl.ColorRGB(255,255,255);//set the color to white
FTImpl.LineWidth(16);
FTImpl.Begin(FT_LINES);
XOffset2 = 15*sin(CurrentRoll+(-1.5708));
YOffset2 = 15*cos(CurrentRoll+(-1.5708));
XOffset3 = 45*sin(CurrentRoll+(-1.5708));
YOffset3 = 45*cos(CurrentRoll+(-1.5708));
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0); // left
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0);
FTImpl.Vertex2ii(240+XOffset3,150+YOffset3,0,0); // right
FTImpl.ColorRGB(0,0,0);//set the color to black
FTImpl.LineWidth(80);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,140,0,0); // tail
FTImpl.LineWidth(48);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(64);
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,150,0,0); // center dot
FTImpl.LineWidth(16);
FTImpl.Vertex2ii(240,150,0,0);
FTImpl.Vertex2ii(240,140,0,0); // tail
FTImpl.LineWidth(32);
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(195,150,0,0); // left
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(285,150,0,0); // left
FTImpl.Vertex2ii(225,150,0,0);
FTImpl.Vertex2ii(225,157,0,0); // right
FTImpl.Vertex2ii(255,150,0,0);
FTImpl.Vertex2ii(255,157,0,0); // right
}
//20 degree marks
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.LineWidth(16);
XOffset1 = 80.5*sin(CurrentRoll+(0.4794));
YOffset1 = 80.5*cos(CurrentRoll+(0.4794));
XOffset2 = 80.5*sin(CurrentRoll+(-0.4794));
YOffset2 = 80.5*cos(CurrentRoll+(-0.4794));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 20 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 20 deg up
//10 degree marks
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.LineWidth(16);
XOffset1 = 51*sin(CurrentRoll+(0.8415));
YOffset1 = 51*cos(CurrentRoll+(0.84154));
XOffset2 = 51*sin(CurrentRoll+(-0.8415));
YOffset2 = 51*cos(CurrentRoll+(-0.8415));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 10 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 10 deg up
// 5 degree marks. These work out as half the offsetts of the 10, so less math!
FTImpl.LineWidth(12);
FTImpl.Vertex2ii(240+XOffset1/2,150+YOffset1/2,0,0);
FTImpl.Vertex2ii(240+XOffset2/2,150+YOffset2/2,0,0); // 20 deg down
FTImpl.Vertex2ii(240-XOffset1/2,150-YOffset1/2,0,0);
FTImpl.Vertex2ii(240-XOffset2/2,150-YOffset2/2,0,0); // 20 deg up
// 15 degree marks
XOffset1 = 56*sin(CurrentRoll+(0.3272));
YOffset1 = 56*cos(CurrentRoll+(0.3272));
XOffset2 = 56*sin(CurrentRoll+(-0.3272));
YOffset2 = 56*cos(CurrentRoll+(-0.3272));
FTImpl.Vertex2ii(240+XOffset1,150+YOffset1,0,0);
FTImpl.Vertex2ii(240+XOffset2,150+YOffset2,0,0); // 10 deg down
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0); // 10 deg up
// the 20 degree numbers. These will always show up upright.
XOffset1 = 87.7*sin(CurrentRoll+(-0.6400));
YOffset1 = 87.7*cos(CurrentRoll+(-0.6400));
XOffset2 = 87.7*sin(CurrentRoll+(-2.5));
YOffset2 = 87.7*cos(CurrentRoll+(-2.5));
FTImpl.Cmd_Number(240-XOffset1, 150-YOffset1, 26, FT_OPT_CENTER, 20);
FTImpl.Cmd_Number(240-XOffset2, 150-YOffset2, 26, FT_OPT_CENTER, 20);
// the 10 degree numbers. These will always show up upright.
XOffset1 = 61.6*sin(CurrentRoll+(-0.9835));
YOffset1 = 61.6*cos(CurrentRoll+(-0.9835));
XOffset2 = 61.6*sin(CurrentRoll+(-2.15));
YOffset2 = 61.6*cos(CurrentRoll+(-2.15));
FTImpl.Cmd_Number(240-XOffset1, 150-YOffset1, 26, FT_OPT_CENTER, 10);
FTImpl.Cmd_Number(240-XOffset2, 150-YOffset2, 26, FT_OPT_CENTER, 10);
// The Arrow...
FTImpl.ColorRGB(255,255,0);//set the color to Yellow
FTImpl.LineWidth(24);
FTImpl.Begin(FT_LINE_STRIP);
XOffset1 = 90*sin(CurrentRoll);
YOffset1 = 90*cos(CurrentRoll);
XOffset2 = 107*sin(CurrentRoll);
YOffset2 = 107*cos(CurrentRoll);
XOffset3 = XOffset2;
YOffset3 = YOffset2;
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
XOffset1 = 100*sin(CurrentRoll+(-0.040));
YOffset1 = 100*cos(CurrentRoll+(-0.040));
XOffset2 = 100*sin(CurrentRoll+( 0.040));
YOffset2 = 100*cos(CurrentRoll+( 0.040));
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0);
// The Rate of Turn Arrow
// CurrentRateOfTurn is the rate of turn in Radians/Sec. -(neg) is to the left
// It must be scaled to indicate rate of turn in 2 minutes, so
float TwoMinAngle = -1*(CurrentRateOfTurn * 45.7);
FTImpl.ColorRGB(255,128,0);//set the color to Orange
if (TwoMinAngle > 0.9) {TwoMinAngle = 0.9;
FTImpl.ColorRGB(255,0,0);
}
else if (TwoMinAngle < -0.9)
{
TwoMinAngle = -0.9;
FTImpl.ColorRGB(255,0,0);
};
FTImpl.Begin(FT_LINE_STRIP);
XOffset1 = 130*sin(TwoMinAngle);
YOffset1 = 130*cos(TwoMinAngle);
XOffset2 = 115*sin(TwoMinAngle);
YOffset2 = 115*cos(TwoMinAngle);
XOffset3 = XOffset2;
YOffset3 = YOffset2;
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
XOffset1 = 120*sin(TwoMinAngle+(-0.0250));
YOffset1 = 120*cos(TwoMinAngle+(-0.0250));
XOffset2 = 120*sin(TwoMinAngle+( 0.0250));
YOffset2 = 120*cos(TwoMinAngle+( 0.0250));
FTImpl.Vertex2ii(240-XOffset1,150-YOffset1,0,0);
FTImpl.Vertex2ii(240-XOffset2,150-YOffset2,0,0);
FTImpl.Vertex2ii(240-XOffset3,150-YOffset3,0,0);
//** Overlay the Altitude Strip
// Print out the Labels, Kohlsman Window, and Units
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Text(332, 260, 27, FT_OPT_CENTER, "Altimeter:");
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: {
if (CurrentPressure > 31.0) CurrentPressure = 31.0;
if (CurrentPressure < 28.1) CurrentPressure = 28.1;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "inHg"); // inHg
FTImpl.Cmd_Number(397, 260, 27, FT_OPT_CENTER, (int)(CurrentPressure*100)); // Garder 2 chiffres après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(397,266,0,0); FTImpl.Vertex2ii(397,266,0,0);
break;}
case 2: {
if (CurrentPressure > 787.5) CurrentPressure = 787.5;
if (CurrentPressure < 713.5) CurrentPressure = 713.5;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "mmHg"); // mmHg
FTImpl.Cmd_Number(397, 260, 27, FT_OPT_CENTER, (int)(CurrentPressure*10)); // Garder 1 chiffre après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(407,266,0,0); FTImpl.Vertex2ii(407,266,0,0);
break; }
case 3: {
if (CurrentPressure > 1050.0) CurrentPressure = 1050.0;
if (CurrentPressure < 951.5) CurrentPressure = 951.5;
FTImpl.Cmd_Text(450, 260, 27, FT_OPT_CENTER, "hPa"); // hPa
FTImpl.Cmd_Number(420, 260, 27, FT_OPT_CENTERY | FT_OPT_RIGHTX, (int)(CurrentPressure*10)); // Garder 1 chiffre après le nombre
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(410,266,0,0);
FTImpl.Vertex2ii(410,266,0,0);
break; }}
// BESOIN DE VÉRIFIER ET DE FIXER LES CAS 2, 3 - RÉGLAGES SUR LES LIAISONS VERTICALES
//** Overlay the VSI Strip
FTImpl.BlendFunc(FT_SRC_ALPHA,FT_ONE); // set for a blend instead of a paint
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(15,15,15); //set the color to a slightly lighter hue
FTImpl.Vertex2ii(429, 60);
FTImpl.Vertex2ii(480,240);
FTImpl.BlendFunc(FT_SRC_ALPHA, FT_ONE_MINUS_SRC_ALPHA); // set back to paint
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(435,230,0,0);
FTImpl.Vertex2ii(435,70,0,0);
boolean ticker = true;
for (int tics = 0; tics < 9; tics ++)
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(433,70+(tics*20),0,0);
FTImpl.Vertex2ii(439,70+(tics*20),0,0);
if (ticker)FTImpl.Cmd_Number(442, 62+(tics*20), 26, 0, abs(MaxVertSpeed-(tics*MaxVertSpeed/4)));
ticker = !ticker;
}
// now the value of CurrentRateOfClimb in a nice little box
TicMap = map(CurrentRateOfClimb, MaxVertSpeed*-1, MaxVertSpeed, 230, 70); // maps into display units
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(435, TicMap);
FTImpl.Vertex2ii(445,TicMap);
FTImpl.Begin(FT_RECTS);
FTImpl.ColorRGB(128,128,128);//set the color to Grey
FTImpl.Vertex2ii(442, TicMap+9);
FTImpl.Vertex2ii(480,TicMap-9);
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Vertex2ii(444, TicMap+7);
FTImpl.Vertex2ii(480,TicMap-7);
FTImpl.ColorRGB(255,255,255);//set the color to White
if ((CurrentRateOfClimb < 0) and (CurrentRateOfClimb > -1000)) // include the sign only for less than 1000 in the negative scale
FTImpl.Cmd_Number(462, TicMap, 26, FT_OPT_CENTER | FT_OPT_SIGNED, (CurrentRateOfClimb));
else FTImpl.Cmd_Number(462, TicMap, 26, FT_OPT_CENTER | FT_OPT_SIGNED, abs(CurrentRateOfClimb));
// print out units...
switch (AltUnits) { // overlay the Speed Units. These can change on the fly...
case 1: FTImpl.Cmd_Text(440, 43, 27, FT_OPT_CENTER, "ft, ft/s"); break;
case 2: FTImpl.Cmd_Text(440, 43, 27, FT_OPT_CENTER, "m, m/s"); break; }
//** Overlay the Altimeter.
FTImpl.LineWidth(16);
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(365,38,0,0);
FTImpl.Vertex2ii(365,238,0,0); // this is a 200 pixel long line, with a 2000 foot range, so we will get 10 feet per pixel...
//now lay out the tics
int Nearestth = 500*(round(CurrentAltitude/500)); //This finds the nearest 500 unit mark.
int altoffset = (CurrentAltitude - Nearestth)/10; // this is in pixel units
int bottomshow = -1; int topshow = 3;
if (altoffset < -12) {topshow = 2; bottomshow = -2;};
for (int i = bottomshow; i < topshow; i++) //higher number shows on top
{
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(362,150+altoffset-(50*i),0,0);
FTImpl.Vertex2ii(367,150+altoffset-(50*i),0,0); // 10 tics
FTImpl.Cmd_Number(368, 150+altoffset-(50*i), 26, FT_OPT_CENTERY | FT_OPT_SIGNED, ((Nearestth)+(i*500)));
}
FTImpl.ColorRGB(0,0,0);//set the color to Black
FTImpl.Begin(FT_LINES);
FTImpl.Vertex2ii(362,150,0,0);
FTImpl.Vertex2ii(380,150,0,0); // black tic
FTImpl.Vertex2ii(362,151,0,0);
FTImpl.Vertex2ii(380,151,0,0); // black tic
FTImpl.Begin(FT_RECTS);
FTImpl.Vertex2ii(376,142,0,0);
FTImpl.Vertex2ii(408,159,0,0); // first the box for the hundreds and up
FTImpl.Vertex2ii(409,107,0,0);
FTImpl.Vertex2ii(427,194,0,0); // then the box for the cool number slider for the 20's...
FTImpl.ClearStencil(0); // set the stencil to blank
FTImpl.StencilOp(FT_INCR,FT_INCR);
FTImpl.Vertex2ii(411,108,0,0);
FTImpl.Vertex2ii(427,193,0,0); // then the box THAT DEFINES THE STENCIL ....
FTImpl.StencilOp(FT_KEEP,FT_KEEP); // and we are done building the stencil
if (CurrentAltitude >= 0)
{
FTImpl.ColorRGB(200,200,140); //set the color to Creamy White
int twenties = (CurrentAltitude - (100 * (int)(CurrentAltitude/100)))/20; // everything but the last 2 digits off/then divide by 20. the fact that it is an int does the rest
int teens = (CurrentAltitude - (100 * (int)(CurrentAltitude/100))); // everything but the last two digits are gone.
while (abs(teens) >= 20) teens = teens - 20; //Should knock us down to the number of feet less then 20. THis will be the numerical offset for the scroll
FTImpl.StencilFunc(FT_GREATER, 0, 255); //only operate on the stencil we just set up
for (int i = -1; i < 7; i++) // we are going to "print" 9 numbers onto the stencil
{
FTImpl.Cmd_Number(414, 190-(i*20)+(teens), 26, FT_OPT_CENTER | FT_OPT_SIGNED, 260+((i+twenties)*20)); // print the 20's, adjusted for height and for number
}
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
FTImpl.ColorRGB(255,255,255); // white me up
// now we do the other numbers as slow flipping sliders
// start with the hundreds. This will be the one to determine how all the numbers flip IF they do flip, so twentieths only need be calculated once.
int twentieths = 0; int tenthousands = 0; int thousands = 0; int hundreds = 0; int hundredsdigit = 0; int thousandsdigit = 0; int tenthousandsdigit = 0;
int Destructor = abs(CurrentAltitude); // we are going to take this sucker apart. get rid of the minus sign if needed
if (Destructor >= 10000) {tenthousandsdigit = Destructor/10000; Destructor = Destructor - (tenthousandsdigit * 10000);} // int gets rid of any extra. then we trim
if (Destructor >= 1000) {thousandsdigit = Destructor/1000; Destructor = Destructor - (thousandsdigit * 1000);} // int gets rid of any extra. then we trim
if (Destructor >= 100) hundredsdigit = Destructor/100; // int gets rid of any extra. no need to trim, as we dont care
Destructor = abs(CurrentAltitude); // reset the sucker. get rid of the minu sign if needed
tenthousands = Destructor - tenthousandsdigit*10000; Destructor = tenthousands;
thousands = Destructor - thousandsdigit*1000; Destructor = thousands;
hundreds = Destructor - hundredsdigit*100; // and I no longer give a rip about Destructor!
int topdigit = -1; int bottomdigit = -1;
if (abs(CurrentAltitude) > 80) // we have to put numbers in the left box
{
// we will start out with the hunderedsdigit
if (hundreds > 82) // we are flipping to the higher number
{
twentieths = hundreds - 82;
topdigit = hundredsdigit + 1;
bottomdigit = hundredsdigit;
if (CurrentAltitude < 100) bottomdigit = -1;
//if ((CurrentAltitude > 1050) and (topdigit == 10)) topdigit = 0;
FlipNum(409, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(400, 141, 27, 0, hundredsdigit); }; // we get to just print it out.
}
// now we do the thousands digit
if (abs(CurrentAltitude) > 982) // we have to put numbers in the left box
{
if (thousands > 982) // we are flipping to the higher number
{
topdigit = thousandsdigit + 1;
bottomdigit = thousandsdigit;
if (CurrentAltitude < 1000) bottomdigit = -1;
if ((CurrentAltitude > 9900) and (bottomdigit == 9)) topdigit = 10;
FlipNum(400, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(391, 141, 27, 0, thousandsdigit); }; // we get to just print it out.
}
// now we do the ten thousands digit
if (abs(CurrentAltitude) > 9982) // we have to put numbers in the left box
{
if (tenthousands > 9982) // we are flipping to the higher number
{
topdigit = tenthousandsdigit + 1;
bottomdigit = tenthousandsdigit;
if (bottomdigit == 0) bottomdigit = -1;
if (topdigit == 0) topdigit = -1;
FlipNum(391, 143, topdigit, bottomdigit, 20-twentieths);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(382, 141, 27, 0, tenthousandsdigit); }; // we get to just print it out.
}
}
else // CurrentAltitude est negative !
{
FTImpl.ColorRGB(200,200,140); //set the color to Creamy White
FTImpl.Cmd_Text(386, 144, 27, FT_OPT_CENTER , "_");
int twenties = 20 - (CurrentAltitude - (100 * (int)(CurrentAltitude/100)))/20; // everything but the last 2 digits off/then divide by 20. the fact that it is an int does the rest
int teens = -1 * (CurrentAltitude - (100 * (int)(CurrentAltitude/100))); // everything but the last two digits are gone.
while (abs(teens) >= 20) teens = teens - 20; //Should knock us down to the number of feet less then 20. THis will be the numerical offset for the scroll
FTImpl.StencilFunc(FT_GREATER, 0, 255); //only operate on the stencil we just set up
for (int i = -4; i < 3; i++) // we are going to "print" 7 numbers onto the stencil
{
FTImpl.Cmd_Number(414, 190+(i*20)-(teens), 26, FT_OPT_CENTER | FT_OPT_SIGNED, 240+((i+twenties)*20)); // print the 20's, adjusted for height and for number
}
FTImpl.StencilFunc(FT_ALWAYS, 1, 255); //go back to ignoring the stencil
FTImpl.ColorRGB(255,255,255); // white me up
// now we do the other numbers as slow flipping sliders
// start with the hundreds. This will be the one to determine how all the numbers flip IF they do flip, so twentieths only need be calculated once.
int twentieths = 0; int tenthousands = 0; int thousands = 0; int hundreds = 0; int hundredsdigit = 0; int thousandsdigit = 0; int tenthousandsdigit = 0;
int Destructor = abs(CurrentAltitude); // we are going to take this sucker apart. get rid of the minus sign if needed
if (Destructor >= 10000) {tenthousandsdigit = Destructor/10000; Destructor = Destructor - (tenthousandsdigit * 10000);} // int gets rid of any extra. then we trim
if (Destructor >= 1000) {thousandsdigit = Destructor/1000; Destructor = Destructor - (thousandsdigit * 1000);} // int gets rid of any extra. then we trim
if (Destructor >= 100) hundredsdigit = Destructor/100; // int gets rid of any extra. no need to trim, as we dont care
Destructor = abs(CurrentAltitude); // reset the sucker. get rid of the minu sign if needed
tenthousands = Destructor - tenthousandsdigit*10000; Destructor = tenthousands;
thousands = Destructor - thousandsdigit*1000; Destructor = thousands;
hundreds = Destructor - hundredsdigit*100; // and I no longer give a rip about Destructor!
int topdigit = -1; int bottomdigit = -1;
if (CurrentAltitude < -80) // we have to put numbers in the left box
{
// we will start out with the hunderedsdigit
if (hundreds > 80) // we are flipping to the higher number
{
twentieths = (-1*hundreds) + 80;
topdigit = hundredsdigit;
bottomdigit = hundredsdigit + 1;
if (CurrentAltitude > -100) topdigit = -1;
//if ((CurrentAltitude > 1050) and (topdigit == 10)) topdigit = 0;
FlipNum(409, 143, topdigit, bottomdigit, (-1*twentieths)+1);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(400, 141, 27, 0, hundredsdigit); }; // we get to just print it out.
}
// now we do the thousands digit
if (CurrentAltitude < -980) // we have to put numbers in the left box
{
if (thousands > 980) // we are flipping to the higher number
{
topdigit = thousandsdigit;
bottomdigit = thousandsdigit + 1;
if (CurrentAltitude > -1000) topdigit = -1;
FlipNum(400, 143, topdigit, bottomdigit, (-1*twentieths)+1);
} // 0-10 as we approach the number
else
{ FTImpl.Cmd_Number(391, 141, 27, 0, thousandsdigit); }; // we get to just print it out.
}
// there is no tenthousands digit in the negative range...
}
//** Overlay the Control Knob Functions
// trim gyro buttons
FTImpl.ColorRGB(255,255,255);//set the color to White
FTImpl.Cmd_Text(201, 260, 27, FT_OPT_CENTER, "Compass Roll Pitch");
// Sensor Error Flag
if (SensorError)
FTImpl.ColorRGB(200,0,0);//set the color to RED because we have a sensor error.
else
FTImpl.ColorRGB(170,170,170);//set the color to grey becuase our last sensor read was successful.
FTImpl.Cmd_Text(455, 12, 18, FT_OPT_CENTER, "Snsr");
FTImpl.Cmd_Text(455, 25, 18, FT_OPT_CENTER, "Err");
//** DRAW THIS LAST - The Menu items on the bottom of the screen.
if (Menu == 1)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // brightness box color
FTImpl.Begin(FT_LINE_STRIP); // *
FTImpl.Vertex2ii(3,250,0,0);
FTImpl.Vertex2ii(27,250,0,0);
FTImpl.Vertex2ii(27,269,0,0);
FTImpl.Vertex2ii(3,269,0,0);
FTImpl.Vertex2ii(3,250,0,0);
if (Menu == 2)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // speedunits box color
FTImpl.Begin(FT_LINE_STRIP); // BUG
FTImpl.Vertex2ii(31,250,0,0);
FTImpl.Vertex2ii(108,250,0,0);
FTImpl.Vertex2ii(108,269,0,0);
FTImpl.Vertex2ii(31,269,0,0);
FTImpl.Vertex2ii(31,250,0,0);
if (Menu == 3)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // bug box color
FTImpl.Begin(FT_LINE_STRIP); // Compass
FTImpl.Vertex2ii(112,250,0,0);
FTImpl.Vertex2ii(196,250,0,0);
FTImpl.Vertex2ii(196,269,0,0);
FTImpl.Vertex2ii(112,269,0,0);
FTImpl.Vertex2ii(112,250,0,0);
if (Menu == 4)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // trim box color
FTImpl.Begin(FT_LINE_STRIP); // Roll - where I was working
FTImpl.Vertex2ii(200,250,0,0);
FTImpl.Vertex2ii(238,250,0,0);
FTImpl.Vertex2ii(238,269,0,0);
FTImpl.Vertex2ii(200,269,0,0);
FTImpl.Vertex2ii(200,250,0,0);
if (Menu == 5)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // altimeter setting box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(242,250,0,0);
FTImpl.Vertex2ii(291,250,0,0);
FTImpl.Vertex2ii(291,269,0,0);
FTImpl.Vertex2ii(242,269,0,0);
FTImpl.Vertex2ii(242,250,0,0);
if (Menu == 6)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // pressure units box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(423,250,0,0);
FTImpl.Vertex2ii(295,250,0,0);
FTImpl.Vertex2ii(295,269,0,0);
FTImpl.Vertex2ii(423,269,0,0);
FTImpl.Vertex2ii(423,250,0,0);
if (Menu == 7)
{
FTImpl.ColorRGB(210,180,40);
if (MenuSelected)
FTImpl.ColorRGB(255,255,0);
}
else
FTImpl.ColorRGB(180,120,30); // altitude units box color
FTImpl.Begin(FT_LINE_STRIP);
FTImpl.Vertex2ii(427,250,0,0);
FTImpl.Vertex2ii(475,250,0,0);
FTImpl.Vertex2ii(475,269,0,0);
FTImpl.Vertex2ii(427,269,0,0);
FTImpl.Vertex2ii(427,250,0,0);
// ****************************** End My Routine *******************************
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
// }
}
void Calibrate_AHRS() // A la mise sous tension, le bouton a été enfoncé.
{
boolean LiftButton = false;
boolean PressButton = false;
boolean DoZero = false;
while (!PressButton) // this is the menu we will draw for the calibration screen
{
FTImpl.DLStart(); // begin a new drawing list
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100)); // go maximum brightness
FTImpl.ClearColorRGB(255,255,255); // set the background color to white
FTImpl.Clear(1,1,1); // clear the screen
FTImpl.ColorRGB(0,0,0); //set the color to Black.
FTImpl.Cmd_Text(240, 24, 28, FT_OPT_CENTER, "Zero the AHRS screen. Do not zero");
FTImpl.Cmd_Text(240, 46, 28, FT_OPT_CENTER, "the AHRS unless it is in level flight");
FTImpl.Cmd_Text(240, 68, 28, FT_OPT_CENTER, "attitude and facing 0 degrees due North.");
FTImpl.Cmd_Text(240, 90, 28, FT_OPT_CENTER, "It may be easier to zero the AHRS when");
FTImpl.Cmd_Text(240, 112, 28, FT_OPT_CENTER, "it is not mounted in the aircraft. In any");
FTImpl.Cmd_Text(240, 134, 28, FT_OPT_CENTER, "case, twist the knob to the right several");
FTImpl.Cmd_Text(240, 156, 28, FT_OPT_CENTER, "times until the ZERO option is selected");
FTImpl.Cmd_Text(240, 178, 28, FT_OPT_CENTER, "to perform a calibration.");
FTImpl.Cmd_Text(240, 250, 28, FT_OPT_CENTER, "Exit without saving. Zero AHRS and Save.");
// and now highlight the choice we have made
FTImpl.ColorRGB(0,0,0); //set the color to black.
FTImpl.Begin(FT_LINE_STRIP); // and get ready to draw a box
if (DoZero)
{ // draw a box around "Zero
FTImpl.Vertex2ii(232,239,0,0);
FTImpl.Vertex2ii(460,239,0,0);
FTImpl.Vertex2ii(460,261,0,0);
FTImpl.Vertex2ii(232,261,0,0);
FTImpl.Vertex2ii(232,239,0,0);
}
else
{ // draw a box around "Don't Zero
FTImpl.Vertex2ii(15,239,0);
FTImpl.Vertex2ii(230,239,0);
FTImpl.Vertex2ii(230,261,0,0);
FTImpl.Vertex2ii(15,261,0,0);
FTImpl.Vertex2ii(15,239,0,0);
}
// close out the screen
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
if (digitalRead(A15) == HIGH) LiftButton = true; // we won't sense the button press until this is so
if (((digitalRead(A15) == LOW) and (LiftButton))) PressButton = true; // we have lifted the button and pressed it again
NewEncoderPosition = myEnc.read(); // read the encoder
if (OldEncoderPosition - 50 < NewEncoderPosition) DoZero = false; else DoZero = true;
}
if (DoZero) // we need to tare the sensor and save the results
{
// Tare the Sensor
Serial2.write(247);// "Start of Packet Byte"
Serial2.write(96); // "Set Compass Range"
Serial2.write(96); // "Checksum" of the one byte we just sent.
// Save the new Zero
// Serial2.write(247); // "Start of Packet Byte"
// Serial2.write(105); // "Set Reference Vector Mode"
// Serial2.write(3); // "Range set to "Single Auto Continual" (2) Other possibilites include "Single Auto" (1) and "Multiple" (3)
// Serial2.write(107); // "Checksum" - sum of the last two bytes mod 256.
}
}
void setup()
{
String AHRS_Version = "N"; // Ceci est utilisé plus tard pour voir si nous sommes connectés à la puce AHRS
// set up the pins we will need to be using...
pinMode(A15,INPUT_PULLUP); // A 15 is the select button on the encoder
pinMode(19, INPUT_PULLUP); // 19 is an interrupt pin, and is used as the encoder quadrature signal A
pinMode(18, INPUT_PULLUP); // 18 is an interrupt pin, and is used as the encoder quadrature signal B
// start tracking the encoder knob
OldEncoderPosition = myEnc.read();
boolean agree = false;
boolean Iagree = false;
// enable the display
BootupConfigure();
//SPI.begin();
// Serial2 is the Remote AHRS sensor.
Serial2.begin(115200); // begin serial communications with Yost Labs 3-Space Nano AHRS Chip
// AHRS chip can be made to change speeds, but defaults to 115200. Mode is 8N1 (8 data, no parity, 1 stop) This is the arduino default.
delay(500); // give a bit of time to ensure everything is good.
while (Serial2.available()) Serial2.read(); // empty the receive serial port
// write a "get version" request to the AHRS chip
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(230); // "Get Version"
Serial2.write(230); // "Checksum" - in this case the exact same as the message, because it only sums the message byte.
if (!digitalRead(A15)) Calibrate_AHRS(); // this is the calibration routine
// begin a while loop here that draws the startup screen over and over until you select "I agree"
while (!Iagree)
{
if ((digitalRead(A15) == LOW) and (agree)) Iagree = true;
// start drawing the startup screen
FTImpl.DLStart(); // begin a new drawing list
FTImpl.Write(REG_PWM_DUTY,map(brightness, 0, 255, 0, 100)); // go maximum brightness
FTImpl.ClearColorRGB(0,0,0); // set the background color to black
FTImpl.Clear(1,1,1); // clear the screen
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Cmd_Text(240, 24, 28, FT_OPT_CENTER, "TheOpenCockpit Airduino 4.3 version 1.0 Beta");
FTImpl.Cmd_Text(240, 46, 28, FT_OPT_CENTER, "Power-Up Report and Legal Disclaimer");
if (!Static.begin()) { // initialize the Static Port // BMP280 setup
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 78, 26, FT_OPT_CENTER, "Static Port BMP 280 Sensor ......... NOT FOUND @ 0x77");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 78, 26, FT_OPT_CENTER, "Static Port BMP 280 Sensor ......... FOUND & 0x77");
}
if (!Pitot.begin(0x76)) { // initialize the Pitot Port // BMP280 setup
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 96, 26, FT_OPT_CENTER, "Pitot Port BMP 280 Sensor ......... NOT FOUND @ 0x76");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 96, 26, FT_OPT_CENTER, "Pitot Port BMP 280 Sensor ......... FOUND & 0x76");
}
if (Serial2.available() > 11) // it is time to read the AHRS response
{
AHRS_Version = "";
while (Serial2.available()) AHRS_Version = AHRS_Version + Serial2.read();
}
if (AHRS_Version == "N") {// we have not heard from the AHRS sensor
FTImpl.ColorRGB(255,0,0); //set the color to red.
FTImpl.Cmd_Text(240, 118, 26, FT_OPT_CENTER, "Searching for AHRS Sensor on UART2");
}
else {
FTImpl.ColorRGB(255,255,0); //set the color to yellow.
FTImpl.Cmd_Text(240, 118, 26, FT_OPT_CENTER, "Yost Labs AHRS Sensor .......... FOUND on UART2");
}
// Disclaimer
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Cmd_Text(240, 145, 27, FT_OPT_CENTER, "This instrument is experimental in nature, and should");
FTImpl.Cmd_Text(240, 165, 27, FT_OPT_CENTER, "be used to enhance situational awareness only. Any");
FTImpl.Cmd_Text(240, 185, 27, FT_OPT_CENTER, "and all use is at the operator's discretion and risk.");
// choices
FTImpl.Cmd_Text(240, 217, 27, FT_OPT_CENTER, "I do not agree. I agree.");
// more info
FTImpl.Cmd_Text(132, 250, 26, FT_OPT_CENTER, "For more information, please visit");
FTImpl.ColorRGB(100,100,255); //set the color to light blue.
FTImpl.Cmd_Text(348, 250, 26, FT_OPT_CENTER, "http://www.etna-soft.com");
// and now highlight the choice we have made
FTImpl.ColorRGB(255,255,255); //set the color to white.
FTImpl.Begin(FT_LINE_STRIP); // and get ready to draw a box
if (agree)
{ // draw a box around "agree"
FTImpl.Vertex2ii(278,206,0,0);
FTImpl.Vertex2ii(358,206,0,0);
FTImpl.Vertex2ii(358,229,0,0);
FTImpl.Vertex2ii(278,229,0,0);
FTImpl.Vertex2ii(278,206,0,0);
}
else
{ // draw a box around "I do not agree"
FTImpl.Vertex2ii(120,206,0,0);
FTImpl.Vertex2ii(252,206,0,0);
FTImpl.Vertex2ii(252,229,0,0);
FTImpl.Vertex2ii(120,229,0,0);
FTImpl.Vertex2ii(120,206,0,0);
}
// close out the startup screen
FTImpl.DLEnd();//end the display list
FTImpl.Finish();//render the display list and wait for the completion of the DL
NewEncoderPosition = myEnc.read(); // read the encoder
if (OldEncoderPosition != NewEncoderPosition) // the knob has moved
if (agree) agree = false; else agree = true; // so we need to change our choice
OldEncoderPosition = NewEncoderPosition;
// end the while loop here, so that you cannot move on without selecting "I Agree"
}
// now set up the AHRS chip
// Set Accelerometer range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(121); // "Set Accelerometer Range"
Serial2.write(16); // "Range set to +- 4G" Could be 2G (0) 4G (16) and 8G (32)
Serial2.write(137); // "Checksum" - sum of the last two bytes mod 256.
// Set Gyroscope range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(125); // "Set Gyroscope Range"
Serial2.write(1); // "Range set to +- 500dps" Could also be 250 (0) and 2000 (2)
Serial2.write(126); // "Checksum" - sum of the last two bytes mod 256.
// Set Compass range
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(126); // "Set Compass Range"
Serial2.write(1); // "Range set to +- 1.33Ga" Could also be +-.88 Ga (0) and many more
Serial2.write(127); // "Checksum" - sum of the last two bytes mod 256.
// Set Reference Vector Mode
Serial2.write(247); // "Start of Packet Byte"
Serial2.write(105); // "Set Reference Vector Mode"
Serial2.write(3); // "Range set to "Single Auto Continual" (2) Other possibilites include "Single Auto" (1) and "Multiple" (3)
Serial2.write(107); // "Checksum" - sum of the last two bytes mod 256.
// setup variables from defined values so that this doesn't have to be done over and over again
BotWhA = map(WAStart, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
TopWhA = map(WAEnd, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
BotGA = map(GAStart, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
TopGA = map(GAEnd, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
RL = map(RLine, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
BL = map(BLine, 0,MaxSpeed, 0, 195); // 195 is the number of pixels it displays
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: PressureDigits[0] = 2; PressureDigits[1] = 9; PressureDigits[2] = 9; PressureDigits[3] = 2; break; // inHg
case 2: PressureDigits[0] = 7; PressureDigits[1] = 6; PressureDigits[2] = 0; PressureDigits[3] = 0; break; // mmHg
case 3: PressureDigits[0] = 1; PressureDigits[1] = 0; PressureDigits[2] = 1; PressureDigits[3] = 8; break; } // hPa
NumTics = MaxSpeed/SpeedTics; // This is how many tics we will need
HalfSpeedTics = (map(SpeedTics,0,MaxSpeed,0,195)/2); // 195 is the number of pixels it displays
CurrentSpeed = 84;
CurrentRoll = -0.5236; // Radians. 30 degrees to the left
CurrentPitch = -0.17453/2; // Radians. 10 degrees up
} // Fin setup()
void loop()
{
// Calcul Altitude
PreviousAltitude = CurrentAltitude;
float AltCallUnits = CurrentPressure;
if (PressureUnits == 1) AltCallUnits = CurrentPressure * 25.4 * 1.3332; // inHg à mmHg à hPa
if (PressureUnits == 2) AltCallUnits = CurrentPressure * 1.3332; // mmHg à hPa
if (AltUnits == 1)CurrentAltitude = (3.28*Static.readAltitude(AltCallUnits+StaticCalibrationOffset)); // Pression en hPa et Altitude en Ft
else
CurrentAltitude = (Static.readAltitude(AltCallUnits+StaticCalibrationOffset)); // Pression hPa et Altitude en metres
// Calcul Airspeed
float CalcSpeed = 0;
switch(SpeedUnits) { // Choisir les unités pour avoir KTS, m/h ou Km/h
// case 1: CalcSpeed = (59.2484 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 1.15); break; // kts/hr
// case 2: CalcSpeed = (68.1818 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset); break; // m/h
// case 3: CalcSpeed = (109.7280 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 0.62); break; } // km/h
case 1: CalcSpeed = (0.592484 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 1.15); break; // kts/hr
case 2: CalcSpeed = (0.681818 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset); break; // m/h
case 3: CalcSpeed = (1.097280 * sqrt(122.45*(Pitot.readPressure() - Static.readPressure()))) + (AirspeedCalibrationOffset * 0.62); break; } // km/h
CurrentSpeed = CalcSpeed;
// handle the input encoder section. This is activated by raising the button.
NewEncoderPosition = myEnc.read();
EncMove = (OldEncoderPosition - NewEncoderPosition)/ECPD;
if (ButtonDown and digitalRead(A15)) ButtonReleased = true;
ButtonDown = !digitalRead(A15);
if (ButtonReleased and !MenuSelected) { MenuSelected = true; ButtonReleased = false; }
int oldunits;
if (MenuSelected) // we are in a menu, and need to handle it
{
if (ButtonReleased) // anything that happens when we leave a menu needs to happen now
{
MenuSelected = false; // this has to happen
}
switch(Menu) {
case 1: // brightness
if (ButtonDown) EncMult = 25; else EncMult = 3; // higher speed if you hold down the button
brightness = brightness + (EncMove * EncMult);
if (brightness > 255) brightness = 255;
if (brightness < 6) brightness = 6; //don't want to be so low that you cant see to brighten it back up
break;
case 2: // bug
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
HeadingBug = HeadingBug + (EncMove * EncMult);
if (HeadingBug > 360) HeadingBug = HeadingBug - 360;
if (HeadingBug < 0) HeadingBug = HeadingBug + 360; // corresponds to degrees. Wraps around the 360 point
break;
case 3: // compass
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
CompassOffset = CompassOffset + (EncMove * EncMult * 0.5); // 0.5 is a half degree in degrees ;-)
break;
case 4: // Roll
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
RollOffset = RollOffset + (EncMove * EncMult * 0.00872); // 0.00872 is a half degree in Radians
break;
case 5: // Pitch
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
PitchOffset = PitchOffset + (EncMove * EncMult * 0.00872); // 0.00872 is a half degree in Radians
break;
case 6: // altimeter
if (ButtonDown) EncMult = 10; else EncMult = 1; // higher speed if you hold down the button
switch (PressureUnits) { // overlay the Pressure Units. These can change on the fly...
case 1: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/100); break; // inHg
case 2: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/10); break; // mmHg
case 3: CurrentPressure = CurrentPressure + ((float)(EncMove * EncMult)/10); break; } // hPa
break;
case 7: // altimeter units
if (EncMove != 0) // we gonna covert us some units
if (EncMove > 0) // we are going up
{
if (PressureUnits == 1) CurrentPressure = CurrentPressure * 25.4; // Converting inHg to mmHg
if (PressureUnits == 2) CurrentPressure = CurrentPressure * 1.3332; // Converting mmHg to hPa
} // note - not conversion necessary for 3 becuase we have no other units
else // we are going down
{ // and again, we aren't going down farther if we are at 1...
if (PressureUnits == 2) CurrentPressure = CurrentPressure / 25.4; // Converting mmHg to inHg
if (PressureUnits == 3) CurrentPressure = CurrentPressure / 1.3332; // Converting hPa to mmHg
}
PressureUnits = PressureUnits + EncMove;
if (PressureUnits > 3) PressureUnits = 3;
if (PressureUnits < 1) PressureUnits = 1; // corresponds to altimeter units. no wrap
break; } // and close the switch
if (ButtonDown and ButtonReleased) {MenuSelected = false; } // you just moved out of whatever menu you are in
}
else // we are not in a menu, and the encoder has turned to switch between menus
{
Menu = Menu + EncMove;
if (Menu > 7) Menu = 7;
if (Menu < 1) Menu = 1;
}
ButtonReleased = false;
OldEncoderPosition = NewEncoderPosition; // Fin de la routine du menu
PreviousHeading = CurrentHeading;
// Gérer maintenant le calcul du temps qui nous permet de calculer le taux de virage TC et VSI
previousMicros = currentMicros;
currentMicros = micros();
intervalMicros = currentMicros - previousMicros;
// ROUTINE STILL NEEDS CALIBRATED AND SMOOTHED.
// ROUTINE DES DONNEES COURANTES QUI ONT BESOIN TOUJOURS D'ETRE CALIBRES ET LISSES.
// Je pense que j'ai aussi besoin d'une horloge plus précise.
// Il peut également être nécessaire d'étudier le réglage des modes de puce BNO055.
CurrentRateOfTurn = 500*(float)(CurrentHeading - PreviousHeading)/(intervalMicros);
// Lecture des angles Euler du paquet d'octets du AHRS
if (Serial2.available() > 11)
{ // Nous avons un paquet d'octets, proceder au decodage
//Serial2.read(); N'est plus nécessaire
CP.binary[3] = Serial2.read(); CP.binary[2] = Serial2.read(); CP.binary[1] = Serial2.read(); CP.binary[0] = Serial2.read(); // Va aller dans CurrentHeading
CH.binary[3] = Serial2.read(); CH.binary[2] = Serial2.read(); CH.binary[1] = Serial2.read(); CH.binary[0] = Serial2.read(); // Va aller dans CurrentPitch
CR.binary[3] = Serial2.read(); CR.binary[2] = Serial2.read(); CR.binary[1] = Serial2.read(); CR.binary[0] = Serial2.read(); // Va aller dans CurrentRoll
// CalibrationStatus = Serial2.read(); N'est plus disponible
CurrentHeading = CH.floatingPoint * 57.2958 + CompassOffset; // Note: HDG est en degrés, Pitch et Roll en radians.
CurrentPitch = CP.floatingPoint + PitchOffset; //- (0.008726 * 26.0); // Pitch en radians.
CurrentRoll = -1*CR.floatingPoint + RollOffset; //+ (0.008726 * 4.5); // Roll en radians.
if (CurrentHeading > 360) CurrentHeading = CurrentHeading - 360; // Correction pour toutes les valeurs hors limites.
if (CurrentHeading <= 0) CurrentHeading = CurrentHeading + 360; // Correction pour toutes les valeurs hors limites.
SensorError = false; // Nous avons pu faire une lecture
ErrorStreak = 0; // et cela met fin à toute "séquence" de mauvaises lectures ...
}
else
{
SensorError = true; // Nous avons donc échoué à lire le capteur
ErrorStreak = ErrorStreak + 1; // et prolongé toute séquence d'échecs qui pourrait se produire
}
if (ErrorStreak > 25) ErrorStreak = 25; // Cela indique à l'écran de se déconnecter en cas d'erreur de capteur à long terme
// Section de correction et de demande de données série - Communication avec la puce AHRS
while (Serial2.available()) Serial2.read(); // Vider toutes les erreurs de réception du port série. Cela doit être fait afin de restaurer les communications perdues.
Serial2.write(247); // Start of Packet Byte
Serial2.write(1); // Command Byte - "Read Filtered, Tared Orientation data in Euler Angle Format"
Serial2.write(1); // Checksum Byte - in this case the exact same as the message, because it only sums the message byte.
// Mise à jour l'écran
DrawScreen();
} // Fin loop()