diff --git a/Alarms.cpp b/Alarms.cpp old mode 100644 new mode 100755 diff --git a/Alarms.h b/Alarms.h old mode 100644 new mode 100755 diff --git a/Bluetooth.cpp b/Bluetooth.cpp new file mode 100644 index 00000000..363f599f --- /dev/null +++ b/Bluetooth.cpp @@ -0,0 +1,94 @@ +#define REMOTEXY_MODE__SOFTSERIAL + +#include "Arduino.h" +#include "config.h" +#include "def.h" +#include "types.h" +#include "MultiWii.h" + +#include +#include + +// RemoteXY connection settings +#define REMOTEXY_SERIAL_RX 8 +#define REMOTEXY_SERIAL_TX 7 +#define REMOTEXY_SERIAL_SPEED 9600 + +#if defined(Bluetooth) + +#pragma pack(push, 1) +uint8_t RemoteXY_CONF[] = + { 255,7,0,1,0,80,0,8,162,0, + 5,37,68,12,42,42,38,26,31,5, + 2,-11,13,41,41,64,26,16,2,1, + 52,4,22,11,38,26,31,31,79,78, + 0,79,70,70,0,66,0,42,18,13, + 28,246,26,2,1,22,4,22,11,2, + 26,31,31,79,78,0,79,70,70,0, + 2,0,38,51,21,7,121,26,31,31, + 79,78,0,79,70,70,0 }; + +struct { + + // input variable + int8_t right_x; // =-100..100 x-coordinate joystick position + int8_t right_y; // =-100..100 y-coordinate joystick position + int8_t left_x; // =-100..100 x-coordinate joystick position + int8_t left_y; // =-100..100 y-coordinate joystick position + uint8_t aux2; // =1 if switch ON and =0 if OFF + uint8_t aux1; // =1 if switch ON and =0 if OFF + uint8_t aux3; // =1 if switch ON and =0 if OFF + + // output variable + int8_t height; // =0..100 level position + + // other variable + uint8_t connect_flag; // =1 if wire connected, else =0 + +} RemoteXY; +#pragma pack(pop) + + +int16_t bluetooth_rcData[RC_CHANS];// defined in RX.cpp + + +void resetbluetoothData() +{ + RemoteXY.left_y = -100; + RemoteXY.left_x = 0; + RemoteXY.right_y = 0; + RemoteXY.right_x = 0; + + RemoteXY.aux1 = 0; + RemoteXY.aux2 = 0; + RemoteXY.aux3 = 0; +} + + +void bluetooth_Init() { //setup + + RemoteXY_Init (); + resetbluetoothData(); + +} + +void bluetooth_Read_RC() { //loop + static unsigned long lastRecvTime = 0; + RemoteXY_Handler (); + //unsigned long now = millis(); + // if ( now - lastRecvTime > 1000 ) { + // signal lost? + // resetbluetoothData(); +// } + + bluetooth_rcData[THROTTLE] = map(RemoteXY.left_y,-100,100, 1000, 2000); //If your channels are inverted, reverse the map value. Example. From 1000 to 2000 ---> 2000 to 1000 + bluetooth_rcData[ROLL] = map(RemoteXY.right_x,-100,100, 1000, 2000); + bluetooth_rcData[PITCH] = map(RemoteXY.right_y,-100,100, 1000, 2000); + bluetooth_rcData[YAW] = map(RemoteXY.left_x,-100,100, 1000, 2000); + + bluetooth_rcData[AUX1] = map(RemoteXY.aux1, 0, 1, 1000, 2000); + bluetooth_rcData[AUX2] = map(RemoteXY.aux2, 0, 1, 1000, 2000); + bluetooth_rcData[AUX3] = map(RemoteXY.aux3, 0, 1, 1000, 2000); +} + +#endif diff --git a/Bluetooth.h b/Bluetooth.h new file mode 100644 index 00000000..dcc77dcb --- /dev/null +++ b/Bluetooth.h @@ -0,0 +1,39 @@ + +#ifndef Bluetooth_H_ +#define Bluetooth_H_ + +#include "config.h" + +#if defined(Bluetooth) + +// The sizeof this struct should not exceed 32 bytes +struct bluetoothData { + byte throttle; + byte yaw; + byte pitch; + byte roll; + byte AUX1; + byte AUX2; + byte switches; +}; + +struct bluetoothAckPayload { + float lat; + float lon; + int16_t heading; + int16_t pitch; + int16_t roll; + int32_t alt; + byte flags; +}; + +extern bluetoothData bluetoothData; +extern bluetoothAckPayload bluetoothAckPayload; +extern int16_t bluetooth_rcData[RC_CHANS]; + +void bluetooth_Init(); +void bluetooth_Read_RC(); + +#endif + +#endif /* Bluetooth_H_ */ diff --git a/CREDITS.txt b/CREDITS.txt deleted file mode 100644 index 6a419fe1..00000000 --- a/CREDITS.txt +++ /dev/null @@ -1,315 +0,0 @@ - This is at least a partial credits-file of people that have - contributed to the MultiWii project. It is incomplete. - Policy is to leave marks here and not scattered all over the code. - In the best of all worlds a name mentioned here was synonymous for - the person taking the responsibility of support and maintaining the code. - But we accept the fluctuational concept of participation in an - Open Source project. - The initial version of this file was created by moving the info - from comments in the code. - If you do not find your name here, it is not meant as a disrespect - of your work. Please, feel free to add your name here. - - Thanks, - - Hamburger ----------- -Format: It is sorted by short description and - formatted to allow easy grepping and beautification by - scripts. The fields are: name (N), email (E), web-address - (W), description (D), and short description (S). - - -S: AIRPLANE mode - Experimental -D: -N: PatrikE -E: -W: http://fotoflygarn.blogspot.com/2012/03/how-to-setup-multiwii-airplane-same.html - -S: RCSERIAL -D: input RC signal with the serial port -N: Luis Correia -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=18&t=828 - -S: Configuration Menu over LCD -D: Use of attached LCD (serial,i2c) via cable or BT to tune parameters -N: Alex -N: ??? editing via TX-Sticks -N: Hamburger (auxN settings, multiline/fullscreen, VT100 type) -E: -W: - -S: Eagle Tree Power Panel -D: Added an i2c, and daylight transflective, LCD pandl to existing LCD menu -N: Danal -E: danal.estes at gmail.com -W: - -S: Extended motor range -D: support for special ESC firmware with range 2 - 252 -N: ziss_dm -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=13&t=516 - -S: Failsave -D: -N: MIS -N: Jevermeister (Nils Ivo von Aswege) -E: -W: - -S: Flying Wing model -D: support for model with 1 motor and 2 servos for 2 combined control surfaces (aileron+elevator) -N: Alex (initial layout) -N: Hamburger -E: -W: - -S: Global Buzzer Handling (buzzer.pde) -D: -N: Jevermeister (Nils Ivo von Aswege) -E: -W:http://ncopters.blogspot.com - -S: GPS Autosense for ProMini -D: Switch between Gps & Gui on comport. -N: PatrikE -E: -W: - -S: GPS_altitude,GPS_speed -D: -N: Mis -E: -W: - -S: GPS G-command -D: -N: Mis -E: -W: - -S: Helicopter mode - Experimental -D: Support for 120 & 90 degree swash -N: PatrikE -E: -W: http://fotoflygarn.blogspot.se/2012/04/multiwii-helicopter.html - -S: i2c LCD03 -D: Support for the cheap i2c LCD03 -N: Th0rsten -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1094 - -S: i2c OLED 128x64 display -D: support for OLED as alternatative to LCD -N: howardhb -E: howard@automatrix.co.za -W: - -S: I2C Barometer MS561101BA -D: -N: Fabio -N: Alex (September 2011) -E: -W: - -S: I2C Accelerometer BMA180 -D: contribution initially from opie11 (rc-groups) -N: adaptation from C2po (may 2011) -N: ziss_dm (June 2011) -N: ToLuSe (Jully 2011) -N: Alex (December 2011) -E: -W: - -S: I2C Accelerometer BMA020 -N: Point65 and mgros (rc-groups) -N: ziss_dm (June 2011) -N: oLuSe (Jully 2011) -D: -N: -E: -W: - -S: LIS3LV02 I2C Accelerometer -N: adver -D: -N: -E: -W: http://multiwii.com/forum/viewtopic.php?f=8&t=451 - -S: I2C Accelerometer LSM303DLx -D: -N: wektorx -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=8&t=863 - -S: I2C Gyroscope L3G4200D -D: -N: Ciskje -E: -W: - -S: I2C Compass AK8975 -D: -N: EOSBandi -E: -W: - -S: Inflight ACC Calibration -D: -N: Jevermeister (Nils Ivo von Aswege) -E: -W: http://ncopters.blogspot.com - -S: OpenLRS Designed by Melih Karakelle on 2010-2012 -D: Version Number : 1.11 -D: Latest Code Update : 2012-03-25 -D: Supported Hardware : OpenLRS Rx boards (store.flytron.com) -D: Project Forum : http://forum.flytron.com/viewforum.php?f=7 -D: Google Code Page : http://code.google.com/p/openlrs/ -N: Melih Karakelle (http://www.flytron.com) (forum nick name: Flytron) -N: Jan-Dirk Schuitemaker (http://www.schuitemaker.org/) (forum nick name: CrashingDutchman) -N: Etienne Saint-Paul (http://www.gameseed.fr) (forum nick name: Etienne) -E: -W: http://code.google.com/p/openlrs - -S: OpenLRS - RFM22B/Si4432 control functions -D: Latest Code Update : 2011-09-26 -N: Melih Karakelle (http://www.flytron.com) (forum nick name: Flytron) -E: -W: - -S: OSD O-command -D: -N: MIS -E: -W: - -S: Powermeter -D: estimate consumed power either in software only or with hardware current sensor -N: Hamburger -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=8&t=167 - -S: PROGMEM -D: 11/21/2001 memory saving Cut RAM about in half -N: Danal -E: danal.estes@gmail.com -W: - -S: Sensors -D: Added GY_521 Chinese sensor board (which is an MPU6050) -D: Added MPU3050 support (register maps) -N: Danal -E: danal.estes@gmail.com -W: - -S: Simplified IMU -D: 19/04/2011 Version: V1.1 -N: ziss_dm -E: -W: - -S: Moving Average Gyros -D: status beta -N: Magnetron1 (Michele Ardito -E: -W: - -S: SBUS -D: -N: Captain IxI -N: Zaggo -E: -W: - -S: Spektrum Satellite Receiver -D: Allows Spektrum Satellite as only receiver; 1024, 2048, 22 or 11ms all supported -D: Major rewrite of spektrum code in mid 2012 to have faster interrupts and be more compatible with new MultiWii serial -D: Added Spektrum support for more than 8 channels in September 2012 -N: Danal -E: danal.estes@gmail.com -W: - -S: Telemetry over LCD/OLED serial/i2c -D: -N: Hamburger -E: -W: - -S: Textstar LCD -D: support for LCD 2x16 from Cat's Whisker -N: Luca Brizzi aka gtrick90 @ RCG for initial support -N: Hamburger (bargraph) -E: -W: - -S: LED Flasher Code -D: initial support -N: Stefan Tomanek -N: Jevermeister (Nils Ivo von Aswege) -E: -W:http://ncopters.blogspot.com - -S: Atmega32u4 support -D: so it works on Promicro, & Teensy20 -N: Felix Niessen (ronco) -E: felix.niessen@googlemail.com -W: - -S: 11-bit PWM -D: Better Signal resolution for the ESC's on MEGA's and 32u4's -N: Felix Niessen (ronco) -E: felix.niessen@googlemail.com -W: - -S: Hexa with Servos and Octo on Promini -D: -N: Felix Niessen (ronco) -E: felix.niessen@googlemail.com -W: - -S: Faster Servo refreshrate & 11-bit PWM (for servos on the MEGA & 32u4) -D: -N: Felix Niessen (ronco) -E: felix.niessen@googlemail.com -W: - -S: X-Aircraft PilotLamp -D: Alarm-Handling and control of PilotLamp -N: mr.rc-cam, jevermeister, doughboy -E: -W: - -S: Variometer -D: audio output to indicate rising/falling copter. Output over (wireless) serial to vt100 terminal program -N: Hamburger -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=7&t=2754&start=0#p26774 - -S: RSSI -D: code for RSSI PIN -N: kataventos -E: -W: http://www.kvteamosd.com/ - -S: OSD SWITCH -D: adding OSD BOX -N: Itai -E: -W: - -S: Heli tail precomp -D: for large collective input, increase tail (yaw) signals -N: sachiho -E: -W: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1562&p=58400&hilit=piro+compensation#p57337 - -S: LCD_LCD03S -D: SERIAL LCD: LCD03 whit serial 9600 baud comunication enabled -N: dibelardino -E: Marco -W: http://www.multiwii.com/forum/viewtopic.php?f=8&t=6116&p=61369&hilit=LCD03#p61369 diff --git a/EEPROM.cpp b/EEPROM.cpp old mode 100644 new mode 100755 diff --git a/EEPROM.h b/EEPROM.h old mode 100644 new mode 100755 diff --git a/GPS.cpp b/GPS.cpp old mode 100644 new mode 100755 index d1f4146e..cfb16f93 --- a/GPS.cpp +++ b/GPS.cpp @@ -1486,226 +1486,6 @@ bool GPS_newFrame(uint8_t data) { } #endif //MTK -/**************************************************************************************/ -/*********************** SkyTraq Venus838FLPx **************************/ -/************************ 57600 baud ***************************/ -/************************ update rate 5Hz ***************************/ -/**************************************************************************************/ -#if defined(VENUS8) - -// Input System Messages -#define VENUS_CONFIG_SERIAL_PORT 0x05 -#define VENUS_CONFIG_OUTPUT_MSG_FORMAT 0x09 -#define VENUS_CONFIG_POWER_MODE 0x0C -#define VENUS_CONFIG_GPS_UPDATE_RATE 0x0E - -// Input GPS Messages -#define VENUS_CONFIG_GPS_PINNING 0x39 -#define VENUS_CONFIG_GPS_PINNING_PARAMS 0x3B -#define VENUS_CONFIG_NAV_MODE 0x3C - -// Output GPS Messages -#define VENUS_GPS_LOCATION 0xA8 - -// command messages -#define VENUS8_EXT2 0x62 -#define VENUS8_EXT3 0x63 -#define VENUS8_EXT4 0x64 -#define VENUS8_CONFIG_SBAS 0x01 // w/EXT2 -#define VENUS8_CONFIG_INTERFERENCE_DETECT 0x06 // w/EXT4 -#define VENUS8_CONFIG_NAV_MODE 0x17 // w/EXT4 -#define VENUS8_CONFIG_SAGPS 0x01 // w/EXT3 - -typedef struct { - int32_t x,y,z; -} xyz32_t; - -typedef struct { - uint8_t fixmode; - uint8_t sv_count; // satellites - uint16_t gps_week; - uint32_t gps_tow; - int32_t latitude; - int32_t longitude; - uint32_t ellipsoid_alt; - uint32_t sealevel_alt; - uint16_t gdop, pdop, hdop, vdop, tdop; - xyz32_t ecef,vel; -} venus_location; - -typedef struct { - uint8_t id; // message id - union { - uint8_t body[]; - venus_location location; - }; -} venus_message; - - -typedef struct { - union { - uint8_t payload[]; - venus_message message; - }; -} venus_payload; - -static venus_payload venus_ctx; - -#define SWAP16(x) ((x&0xff)<<8 | (x>>8)) -#define SWAP32(x) ((x&0xff)<<24 | ((x&0xff00)<<8) | ((x&0xff0000)>>8) | ((x&0xff000000)>>24)) - -void VenusFixLocationEndianess() { // we only do relevant ones to save CPU time - venus_ctx.message.location.latitude = SWAP32(venus_ctx.message.location.latitude); - venus_ctx.message.location.longitude = SWAP32(venus_ctx.message.location.longitude); - venus_ctx.message.location.sealevel_alt = SWAP32(venus_ctx.message.location.sealevel_alt); -} - -void venusWrite(uint8_t length) { - uint8_t pls; - uint8_t cs=0; - SerialWrite(GPS_SERIAL,0xA0); - SerialWrite(GPS_SERIAL,0xA1); - SerialWrite(GPS_SERIAL,0); // length is never higher than 8 bits - SerialWrite(GPS_SERIAL,length); - for(pls=0; pls1 - case 4: - venus_ctx.message.id = c; - cr=c; - n=1; - state++; - break; - case 5: // read bytes of the payload - cr ^= c; // adjust checksum - if(n=2; - if (f.GPS_FIX) { - VenusFixLocationEndianess(); - GPS_coord[LAT] = venus_ctx.message.location.latitude; - GPS_coord[LON] = venus_ctx.message.location.longitude; - GPS_altitude = venus_ctx.message.location.sealevel_alt /100; // altitude in meter - } - ret=true; - } - } - return ret; -} -#endif - #endif //GPS SERIAL diff --git a/GPS.h b/GPS.h old mode 100644 new mode 100755 diff --git a/IMU.cpp b/IMU.cpp old mode 100644 new mode 100755 diff --git a/IMU.h b/IMU.h old mode 100644 new mode 100755 diff --git a/LCD.cpp b/LCD.cpp old mode 100644 new mode 100755 index 08d8f3ff..f6b7fd8e --- a/LCD.cpp +++ b/LCD.cpp @@ -534,33 +534,6 @@ void i2c_LCD03_set_cursor (byte col, byte row) { } #endif // LCD_LCD03 -#if defined(LCD_LCD03S) // LCD_LCD03S -// ********************* -// LCD03S serial primitives -// ********************* -void serial_LCD03_init () { - SerialWrite(LCD_SERIAL_PORT, 0x00 );// Command register - SerialWrite(LCD_SERIAL_PORT, 04 );// Hide cursor - SerialWrite(LCD_SERIAL_PORT, 12 );// Clear screen - SerialWrite(LCD_SERIAL_PORT, 19 );// Backlight on -} -void serial_LCD03_send_cmd (byte c) { - SerialWrite(LCD_SERIAL_PORT, 0x00 );// Command register - SerialWrite(LCD_SERIAL_PORT, c ); -} -void serial_LCD03_send_char (char c) { - //SerialWrite(LCD_SERIAL_PORT, 0x00 );// Command register - SerialWrite(LCD_SERIAL_PORT, c ); -} -void serial_LCD03_set_cursor (byte col, byte row) { - row = min(row,1); - col = min(col,15); - serial_LCD03_send_cmd(03); // set cursor (row, column) - serial_LCD03_send_char(row+1); - serial_LCD03_send_char(col+1); -} -#endif // LCD_LCD03S - #if defined(OLED_DIGOLE) // OLED_DIGOLE #define OLED_DIGOLE_ADDRESS 0x27 // 7bit address // ********************* @@ -654,8 +627,6 @@ void LCDprint(uint8_t i) { i2c_ETPP_send_char(i); #elif defined(LCD_LCD03) i2c_LCD03_send_char(i); - #elif defined(LCD_LCD03S) - serial_LCD03_send_char(i); #elif defined(OLED_I2C_128x64) i2c_OLED_send_char(i); #elif defined(OLED_DIGOLE) @@ -698,8 +669,6 @@ void LCDclear() { for (byte i = 0; i<80; i++) i2c_ETPP_send_char(' ');// Blanks for all 80 bytes of RAM in the controller, not just the 2x16 display #elif defined(LCD_LCD03) i2c_LCD03_send_cmd(12); // clear screen - #elif defined(LCD_LCD03S) - serial_LCD03_send_cmd(12); // clear screen #elif defined(OLED_I2C_128x64) i2c_clear_OLED(); #elif defined(OLED_DIGOLE) @@ -739,8 +708,6 @@ void LCDsetLine(byte line) { // Line = 1 or 2 - vt100 has lines 1-99 i2c_ETPP_set_cursor(0,line-1); #elif defined(LCD_LCD03) i2c_LCD03_set_cursor(0,line-1); - #elif defined(LCD_LCD03S) - serial_LCD03_set_cursor(0,line-1); #elif defined(OLED_I2C_128x64) // #ifndef DEBUG // sanity check for production only. Debug runs with all possible side effects // if (line<1 || line>(MULTILINE_PRE+MULTILINE_POST)) line = 1; @@ -824,10 +791,6 @@ void initLCD() { // LCD03 - I2C LCD // http://www.robot-electronics.co.uk/htm/Lcd03tech.htm i2c_LCD03_init(); - #elif defined(LCD_LCD03S) - // LCD03 - SERIAL LCD - // http://www.robot-electronics.co.uk/htm/Lcd03tech.htm - serial_LCD03_init(); #elif defined(OLED_I2C_128x64) i2c_OLED_init(); #ifndef SUPPRESS_OLED_I2C_128x64LOGO @@ -1667,7 +1630,7 @@ void configurationLoop() { LCDsetLine(2); strcpy_P(line1,PSTR("Exit")); LCDprintChar(line1); - #if defined(LCD_LCD03) || defined(LCD_LCD03S) + #if defined(LCD_LCD03) delay(2000); // wait for two seconds then clear screen and show initial message initLCD(); #endif @@ -1708,7 +1671,7 @@ void LCDbar(uint8_t n,uint8_t v) { for (i=j; i< n; i++) LCDprint( '.' ); #elif defined(LCD_ETPP) ETPP_barGraph(n,v); - #elif defined(LCD_LCD03) || defined(LCD_LCD03S) + #elif defined(LCD_LCD03) for (uint8_t i=0; i< n; i++) LCDprint((i -*/ - -#include - -#include "Arduino.h" -#include "config.h" -#include "def.h" -#include "types.h" -#include "MultiWii.h" -#include "Alarms.h" -#include "EEPROM.h" -#include "IMU.h" -#include "LCD.h" -#include "Output.h" -#include "RX.h" -#include "Sensors.h" -#include "Serial.h" -#include "GPS.h" -#include "Protocol.h" -#include "Telemetry.h" - -#include - -/*********** RC alias *****************/ - -const char pidnames[] PROGMEM = - "ROLL;" - "PITCH;" - "YAW;" - "ALT;" - "Pos;" - "PosR;" - "NavR;" - "LEVEL;" - "MAG;" - "VEL;" -; - -const char boxnames[] PROGMEM = // names for dynamic generation of config GUI - "ARM;" - #if ACC - "ANGLE;" - "HORIZON;" - #endif - #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) - "BARO;" - #endif - #ifdef VARIOMETER - "VARIO;" - #endif - "MAG;" - #if defined(HEADFREE) - "HEADFREE;" - "HEADADJ;" -#endif -#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) - "CAMSTAB;" -#endif -#if defined(CAMTRIG) - "CAMTRIG;" -#endif -#if GPS - "GPS HOME;" - "GPS HOLD;" -#endif -#if defined(FIXEDWING) || defined(HELICOPTER) - "PASSTHRU;" -#endif -#if defined(BUZZER) - "BEEPER;" -#endif -#if defined(LED_FLASHER) - "LEDMAX;" - "LEDLOW;" -#endif -#if defined(LANDING_LIGHTS_DDR) - "LLIGHTS;" -#endif -#ifdef INFLIGHT_ACC_CALIBRATION - "CALIB;" -#endif -#ifdef GOVERNOR_P - "GOVERNOR;" -#endif -#ifdef OSD_SWITCH - "OSD SW;" -#endif -#if GPS - "MISSION;" - "LAND;" -#endif - ; - -const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. - 0, //"ARM;" - #if ACC - 1, //"ANGLE;" - 2, //"HORIZON;" - #endif - #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) - 3, //"BARO;" - #endif - #ifdef VARIOMETER - 4, //"VARIO;" - #endif - 5, //"MAG;" -#if defined(HEADFREE) - 6, //"HEADFREE;" - 7, //"HEADADJ;" -#endif -#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) - 8, //"CAMSTAB;" -#endif -#if defined(CAMTRIG) - 9, //"CAMTRIG;" -#endif -#if GPS - 10, //"GPS HOME;" - 11, //"GPS HOLD;" -#endif -#if defined(FIXEDWING) || defined(HELICOPTER) - 12, //"PASSTHRU;" -#endif -#if defined(BUZZER) - 13, //"BEEPER;" -#endif -#if defined(LED_FLASHER) - 14, //"LEDMAX;" - 15, //"LEDLOW;" -#endif -#if defined(LANDING_LIGHTS_DDR) - 16, //"LLIGHTS;" -#endif -#ifdef INFLIGHT_ACC_CALIBRATION - 17, //"CALIB;" -#endif -#ifdef GOVERNOR_P - 18, //"GOVERNOR;" -#endif -#ifdef OSD_SWITCH - 19, //"OSD_SWITCH;" -#endif -#if GPS - 20, //"MISSION;" - 21, //"LAND;" -#endif - }; - - -uint32_t currentTime = 0; -uint16_t previousTime = 0; -uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop -uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. -uint16_t calibratingB = 0; // baro calibration = get new ground pressure value -uint16_t calibratingG; -int16_t magHold,headFreeModeHold; // [-180;+180] -uint8_t vbatMin = VBATNOMINAL; // lowest battery voltage in 0.1V steps -uint8_t rcOptions[CHECKBOXITEMS]; -int32_t AltHold; // in cm -int16_t sonarAlt; -int16_t BaroPID = 0; -int16_t errorAltitudeI = 0; - -// ************** -// gyro+acc IMU -// ************** -int16_t gyroZero[3] = {0,0,0}; - -imu_t imu; - -analog_t analog; - -alt_t alt; - -att_t att; - -#if defined(ARMEDTIMEWARNING) - uint32_t ArmedTimeWarningMicroSeconds = 0; -#endif - -int16_t debug[4]; - -flags_struct_t f; - -//for log -#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) - uint16_t cycleTimeMax = 0; // highest ever cycle timen - uint16_t cycleTimeMin = 65535; // lowest ever cycle timen - int32_t BAROaltMax; // maximum value - uint16_t GPS_speedMax = 0; // maximum speed from gps - #ifdef POWERMETER_HARD - uint16_t powerValueMaxMAH = 0; - #endif - #if defined(WATTS) - uint16_t wattsMax = 0; - #endif -#endif -#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) || defined (TELEMETRY) - uint32_t armedTime = 0; -#endif - -int16_t i2c_errors_count = 0; - - -#if defined(THROTTLE_ANGLE_CORRECTION) - int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, - int8_t cosZ = 100; // cos(angleZ)*100 -#endif - - - -// ********************** -//Automatic ACC Offset Calibration -// ********************** -#if defined(INFLIGHT_ACC_CALIBRATION) - uint16_t InflightcalibratingA = 0; - int16_t AccInflightCalibrationArmed; - uint16_t AccInflightCalibrationMeasurementDone = 0; - uint16_t AccInflightCalibrationSavetoEEProm = 0; - uint16_t AccInflightCalibrationActive = 0; -#endif - -// ********************** -// power meter -// ********************** -#if defined(POWERMETER) || ( defined(LOG_VALUES) && (LOG_VALUES >= 3) ) - uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum - uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop() - uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6] - uint16_t powerValue = 0; // last known current -#endif -uint16_t intPowerTrigger1; - -// ********************** -// telemetry -// ********************** -#if defined(LCD_TELEMETRY) - uint8_t telemetry = 0; - uint8_t telemetry_auto = 0; - int16_t annex650_overrun_count = 0; -#endif -#ifdef LCD_TELEMETRY_STEP - char telemetryStepSequence [] = LCD_TELEMETRY_STEP; - uint8_t telemetryStepIndex = 0; -#endif - -// ****************** -// rc functions -// ****************** -#define ROL_LO (1<<(2*ROLL)) -#define ROL_CE (3<<(2*ROLL)) -#define ROL_HI (2<<(2*ROLL)) -#define PIT_LO (1<<(2*PITCH)) -#define PIT_CE (3<<(2*PITCH)) -#define PIT_HI (2<<(2*PITCH)) -#define YAW_LO (1<<(2*YAW)) -#define YAW_CE (3<<(2*YAW)) -#define YAW_HI (2<<(2*YAW)) -#define THR_LO (1<<(2*THROTTLE)) -#define THR_CE (3<<(2*THROTTLE)) -#define THR_HI (2<<(2*THROTTLE)) - -int16_t failsafeEvents = 0; -volatile int16_t failsafeCnt = 0; - -int16_t rcData[RC_CHANS]; // interval [1000;2000] -int16_t rcSerial[8]; // interval [1000;2000] - is rcData coming from MSP -int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -uint8_t rcSerialCount = 0; // a counter to select legacy RX when there is no more MSP rc serial data -int16_t lookupPitchRollRC[5];// lookup table for expo & RC rate PITCH+ROLL -uint16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE - -#if defined(SERIAL_RX) - volatile uint8_t spekFrameFlags; - volatile uint32_t spekTimeLast; - uint8_t spekFrameDone; -#endif - -#if defined(OPENLRSv2MULTI) - uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages -#endif - - -// ************************* -// motor and servo functions -// ************************* -int16_t axisPID[3]; -int16_t motor[8]; -int16_t servo[8] = {1500,1500,1500,1500,1500,1500,1500,1000}; - -// ************************ -// EEPROM Layout definition -// ************************ -static uint8_t dynP8[2], dynD8[2]; - -global_conf_t global_conf; - -conf_t conf; - -#ifdef LOG_PERMANENT - plog_t plog; -#endif - -// ********************** -// GPS common variables, no need to put them in defines, since compiller will optimize out unused variables -// ********************** -#if GPS - gps_conf_struct GPS_conf; -#endif - int16_t GPS_angle[2] = { 0, 0}; // the angles that must be applied for GPS correction - int32_t GPS_coord[2]; - int32_t GPS_home[2]; - int32_t GPS_hold[2]; - int32_t GPS_prev[2]; //previous pos - int32_t GPS_poi[2]; - uint8_t GPS_numSat; - uint16_t GPS_distanceToHome; // distance to home - unit: meter - int16_t GPS_directionToHome; // direction to home - unit: degree - int32_t GPS_directionToPoi; - uint16_t GPS_altitude; // GPS altitude - unit: meter - uint16_t GPS_speed; // GPS speed - unit: cm/s - uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update - uint16_t GPS_ground_course = 0; // - unit: degree*10 - - //uint8_t GPS_mode = GPS_MODE_NONE; // contains the current selected gps flight mode --> moved to the f. structure - uint8_t NAV_state = 0; // NAV_STATE_NONE; /// State of the nav engine - uint8_t NAV_error = 0; // NAV_ERROR_NONE; - uint8_t prv_gps_modes = 0; /// GPS_checkbox items packed into 1 byte for checking GPS mode changes - uint32_t nav_timer_stop = 0; /// common timer used in navigation (contains the desired stop time in millis() - uint16_t nav_hold_time; /// time in seconds to hold position - uint8_t NAV_paused_at = 0; // This contains the mission step where poshold paused the runing mission. - - uint8_t next_step = 1; /// The mission step which is upcoming it equals with the mission_step stored in EEPROM - int16_t jump_times = -10; -#if GPS - mission_step_struct mission_step; -#endif - - // The desired bank towards North (Positive) or South (Negative) : latitude - // The desired bank towards East (Positive) or West (Negative) : longitude - int16_t nav[2]; - int16_t nav_rated[2]; //Adding a rate controller to the navigation to make it smoother - - // The orginal altitude used as base our new altitude during nav - int32_t original_altitude; - //This is the target what we want to reach - int32_t target_altitude; - //This is the interim value which is feeded into the althold controller - int32_t alt_to_hold; - - uint32_t alt_change_timer; - int8_t alt_change_flag; - uint32_t alt_change; - -uint8_t alarmArray[ALRM_FAC_SIZE]; // array - -#if BARO - int32_t baroPressure; - int16_t baroTemperature; - int32_t baroPressureSum; -#endif - -void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds - static uint32_t calibratedAccTime; - uint16_t tmp,tmp2; - uint8_t axis,prop1,prop2; - - // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value (or collective.pitch value for heli) - #ifdef HELICOPTER - #define DYN_THR_PID_CHANNEL COLLECTIVE_PITCH - #else - #define DYN_THR_PID_CHANNEL THROTTLE - #endif - prop2 = 128; // prop2 was 100, is 128 now - if (rcData[DYN_THR_PID_CHANNEL]>1500) { // breakpoint is fix: 1500 - if (rcData[DYN_THR_PID_CHANNEL]<2000) { - prop2 -= ((uint16_t)conf.dynThrPID*(rcData[DYN_THR_PID_CHANNEL]-1500)>>9); // /512 instead of /500 - } else { - prop2 -= conf.dynThrPID; - } - } - - for(axis=0;axis<3;axis++) { - tmp = min(abs(rcData[axis]-MIDRC),500); - #if defined(DEADBAND) - if (tmp>DEADBAND) { tmp -= DEADBAND; } - else { tmp=0; } - #endif - if(axis!=2) { //ROLL & PITCH - tmp2 = tmp>>7; // 500/128 = 3.9 => range [0;3] - rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7); - prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500 - prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128 prop2: max is 128 result prop1: max is 128 - dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now - dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now - } else { // YAW - rcCommand[axis] = tmp; - } - if (rcData[axis] [0;2559] - tmp2 = tmp/256; // range [0;9] - rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTLE] - #if defined(HEADFREE) - if(f.HEADFREE_MODE) { //to optimize - float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533 - float cosDiff = cos(radDiff); - float sinDiff = sin(radDiff); - int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff; - rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; - rcCommand[PITCH] = rcCommand_PITCH; - } - #endif - - // query at most one multiplexed analog channel per MWii cycle - static uint8_t analogReader =0; - switch (analogReader++ % (3+VBAT_CELLS_NUM)) { - case 0: - { - #if defined(POWERMETER_HARD) - static uint32_t lastRead = currentTime; - static uint8_t ind = 0; - static uint16_t pvec[PSENSOR_SMOOTH], psum; - uint16_t p = analogRead(PSENSORPIN); - //LCDprintInt16(p); LCDcrlf(); - //debug[0] = p; - #if PSENSOR_SMOOTH != 1 - psum += p; - psum -= pvec[ind]; - pvec[ind++] = p; - ind %= PSENSOR_SMOOTH; - p = psum / PSENSOR_SMOOTH; - #endif - powerValue = ( conf.psensornull > p ? conf.psensornull - p : p - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun - analog.amperage = ((uint32_t)powerValue * conf.pint2ma) / 100; // [100mA] //old (will overflow for 65A: powerValue * conf.pint2ma; // [1mA] - pMeter[PMOTOR_SUM] += ((currentTime-lastRead) * (uint32_t)((uint32_t)powerValue*conf.pint2ma))/100000; // [10 mA * msec] - lastRead = currentTime; - #endif // POWERMETER_HARD - break; - } - - case 1: - { - #if defined(VBAT) && !defined(VBAT_CELLS) - static uint8_t ind = 0; - static uint16_t vvec[VBAT_SMOOTH], vsum; - uint16_t v = analogRead(V_BATPIN); - #if VBAT_SMOOTH == 1 - analog.vbat = (v*VBAT_PRESCALER) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps - #else - vsum += v; - vsum -= vvec[ind]; - vvec[ind++] = v; - ind %= VBAT_SMOOTH; - #if VBAT_SMOOTH == VBAT_PRESCALER - analog.vbat = vsum / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps - #elif VBAT_SMOOTH < VBAT_PRESCALER - analog.vbat = (vsum * (VBAT_PRESCALER/VBAT_SMOOTH)) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps - #else - analog.vbat = ((vsum /VBAT_SMOOTH) * VBAT_PRESCALER) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps - #endif - #endif - #endif // VBAT - break; - } - case 2: - { - #if defined(RX_RSSI) - static uint8_t ind = 0; - static uint16_t rvec[RSSI_SMOOTH], rsum, r; - - // http://www.multiwii.com/forum/viewtopic.php?f=8&t=5530 - #if defined(RX_RSSI_CHAN) - uint16_t rssi_Input = constrain(rcData[RX_RSSI_CHAN],1000,2000); - r = map((uint16_t)rssi_Input , 1000, 2000, 0, 1023); - #else - r = analogRead(RX_RSSI_PIN); - #endif - - #if RSSI_SMOOTH == 1 - analog.rssi = r; - #else - rsum += r; - rsum -= rvec[ind]; - rvec[ind++] = r; - ind %= RSSI_SMOOTH; - r = rsum / RSSI_SMOOTH; - analog.rssi = r; - #endif - #endif // RX RSSI - break; - } - default: // here analogReader >=4, because of ++ in switch() - { - #if defined(VBAT) && defined(VBAT_CELLS) - if ( (analogReader<4) || (analogReader>4+VBAT_CELLS_NUM-1) ) break; - uint8_t ind = analogReader-4; - static uint16_t vbatcells_pins[VBAT_CELLS_NUM] = VBAT_CELLS_PINS; - static uint8_t vbatcells_offset[VBAT_CELLS_NUM] = VBAT_CELLS_OFFSETS; - static uint8_t vbatcells_div[VBAT_CELLS_NUM] = VBAT_CELLS_DIVS; - uint16_t v = analogRead(vbatcells_pins[ind]); - analog.vbatcells[ind] = vbatcells_offset[ind] + (v << 2) / vbatcells_div[ind]; // result is Vbatt in 0.1V steps - if (ind == VBAT_CELLS_NUM -1) analog.vbat = analog.vbatcells[ind]; - #endif // VBAT) && defined(VBAT_CELLS) - break; - } // end default - } // end of switch() - -#if defined( POWERMETER_HARD ) && (defined(LOG_VALUES) || defined(LCD_TELEMETRY)) - if (analog.amperage > powerValueMaxMAH) powerValueMaxMAH = analog.amperage; -#endif - -#if defined(WATTS) - analog.watts = (analog.amperage * analog.vbat) / 100; // [0.1A] * [0.1V] / 100 = [Watt] - #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) - if (analog.watts > wattsMax) wattsMax = analog.watts; - #endif -#endif - - #if defined(BUZZER) - alarmHandler(); // external buzzer routine that handles buzzer events globally now - #endif - - - if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis - LEDPIN_TOGGLE; - } else { - if (f.ACC_CALIBRATED) {LEDPIN_OFF;} - if (f.ARMED) {LEDPIN_ON;} - } - - #if defined(LED_RING) - static uint32_t LEDTime; - if ( currentTime > LEDTime ) { - LEDTime = currentTime + 50000; - i2CLedRingState(); - } - #endif - - #if defined(LED_FLASHER) - auto_switch_led_flasher(); - #endif - - if ( currentTime > calibratedAccTime ) { - if (! f.SMALL_ANGLES_25) { - // the multi uses ACC and is not calibrated or is too much inclinated - f.ACC_CALIBRATED = 0; - LEDPIN_TOGGLE; - calibratedAccTime = currentTime + 100000; - } else { - f.ACC_CALIBRATED = 1; - } - } - - #if !(defined(SERIAL_RX) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if SERIAL RX in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0. - serialCom(); - #endif - - #if defined(POWERMETER) - analog.intPowerMeterSum = (pMeter[PMOTOR_SUM]/PLEVELDIV); - intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; - #endif - - #ifdef LCD_TELEMETRY_AUTO - static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO; - static uint8_t telemetryAutoIndex = 0; - static uint16_t telemetryAutoTimer = 0; - if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) ){ - telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)]; - LCDclear(); // make sure to clear away remnants - } - #endif - #ifdef LCD_TELEMETRY - static uint16_t telemetryTimer = 0; - if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) { - #if (LCD_TELEMETRY_DEBUG+0 > 0) - telemetry = LCD_TELEMETRY_DEBUG; - #endif - if (telemetry) lcd_telemetry(); - } - #endif - - #ifdef TELEMETRY - run_telemetry(); - #endif - - #if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication - static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames - static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off - if(currentTime > GPSLEDTime) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ... - if(f.GPS_FIX && GPS_numSat >= 5) { - if(++blcnt > 2*GPS_numSat) blcnt = 0; - GPSLEDTime = currentTime + 150000; - if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;} - }else{ - if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;} - blcnt = 0; - } - } - #endif - - #if defined(LOG_VALUES) && (LOG_VALUES >= 2) - if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore - if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore - #endif - if (f.ARMED) { - #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) || defined (TELEMETRY) - armedTime += (uint32_t)cycleTime; - #endif - #if defined(VBAT) - if ( (analog.vbat > NO_VBAT) && (analog.vbat < vbatMin) ) vbatMin = analog.vbat; - #endif - #ifdef LCD_TELEMETRY - #if BARO - if ( (alt.EstAlt > BAROaltMax) ) BAROaltMax = alt.EstAlt; - #endif - #if GPS - if ( (GPS_speed > GPS_speedMax) ) GPS_speedMax = GPS_speed; - #endif - #endif - } -} - -void setup() { - SerialOpen(0,SERIAL0_COM_SPEED); - #if defined(PROMICRO) - SerialOpen(1,SERIAL1_COM_SPEED); - #endif - #if defined(MEGA) - SerialOpen(1,SERIAL1_COM_SPEED); - SerialOpen(2,SERIAL2_COM_SPEED); - SerialOpen(3,SERIAL3_COM_SPEED); - #endif - LEDPIN_PINMODE; - POWERPIN_PINMODE; - BUZZERPIN_PINMODE; - STABLEPIN_PINMODE; - POWERPIN_OFF; - initOutput(); - readGlobalSet(); - #ifndef NO_FLASH_CHECK - #if defined(MEGA) - uint16_t i = 65000; // only first ~64K for mega board due to pgm_read_byte limitation - #else - uint16_t i = 32000; - #endif - uint16_t flashsum = 0; - uint8_t pbyt; - while(i--) { - pbyt = pgm_read_byte(i); // calculate flash checksum - flashsum += pbyt; - flashsum ^= (pbyt<<8); - } - #endif - #ifdef MULTIPLE_CONFIGURATION_PROFILES - global_conf.currentSet=2; - #else - global_conf.currentSet=0; - #endif - while(1) { // check settings integrity - #ifndef NO_FLASH_CHECK - if(readEEPROM()) { // check current setting integrity - if(flashsum != global_conf.flashsum) update_constants(); // update constants if firmware is changed and integrity is OK - } - #else - readEEPROM(); // check current setting integrity - #endif - if(global_conf.currentSet == 0) break; // all checks is done - global_conf.currentSet--; // next setting for check - } - readGlobalSet(); // reload global settings for get last profile number - #ifndef NO_FLASH_CHECK - if(flashsum != global_conf.flashsum) { - global_conf.flashsum = flashsum; // new flash sum - writeGlobalSet(1); // update flash sum in global config - } - #endif - readEEPROM(); // load setting data from last used profile - blinkLED(2,40,global_conf.currentSet+1); - - #if GPS - recallGPSconf(); //Load GPS configuration parameteres - #endif - - configureReceiver(); - #if defined (PILOTLAMP) - PL_INIT; - #endif - #if defined(OPENLRSv2MULTI) - initOpenLRS(); - #endif - initSensors(); - #if GPS - GPS_set_pids(); - #endif - previousTime = micros(); - #if defined(GIMBAL) - calibratingA = 512; - #endif - calibratingG = 512; - calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles - #if defined(POWERMETER) - for(uint8_t j=0; j<=PMOTOR_SUM; j++) pMeter[j]=0; - #endif - /************************************/ - #if GPS - #if defined(GPS_SERIAL) - GPS_SerialInit(); - #endif - GPS_conf.max_wp_number = getMaxWPNumber(); - #endif - - #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(LCD_LCD03S) || defined(OLED_I2C_128x64) || defined(OLED_DIGOLE) || defined(LCD_TELEMETRY_STEP) - initLCD(); - #endif - #ifdef LCD_TELEMETRY_DEBUG - telemetry_auto = 1; - #endif - #ifdef LCD_CONF_DEBUG - configurationLoop(); - #endif - #ifdef TELEMETRY - init_telemetry(); - #endif - #ifdef LANDING_LIGHTS_DDR - init_landing_lights(); - #endif - #ifdef FASTER_ANALOG_READS - ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11 - #endif - #if defined(LED_FLASHER) - init_led_flasher(); - led_flasher_set_sequence(LED_FLASHER_SEQUENCE); - #endif - f.SMALL_ANGLES_25=1; // important for gyro only conf - #ifdef LOG_PERMANENT - // read last stored set - readPLog(); - plog.lifetime += plog.armed_time / 1000000; - plog.start++; // #powercycle/reset/initialize events - // dump plog data to terminal - #ifdef LOG_PERMANENT_SHOW_AT_STARTUP - dumpPLog(0); - #endif - plog.armed_time = 0; // lifetime in seconds - //plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut - #endif - #ifdef DEBUGMSG - debugmsg_append_str("initialization completed\n"); - #endif -} - -void go_arm() { - if(calibratingG == 0 - #if defined(ONLYARMWHENFLAT) - && f.ACC_CALIBRATED - #endif - #if defined(FAILSAFE) - && failsafeCnt < 2 - #endif - #if GPS && defined(ONLY_ALLOW_ARM_WITH_GPS_3DFIX) - && (f.GPS_FIX && GPS_numSat >= 5) - #endif - ) { - if(!f.ARMED && !f.BARO_MODE) { // arm now! - f.ARMED = 1; - #if defined(HEADFREE) - headFreeModeHold = att.heading; - #endif - magHold = att.heading; - #if defined(VBAT) - if (analog.vbat > NO_VBAT) vbatMin = analog.vbat; - #endif - #ifdef ALTITUDE_RESET_ON_ARM - #if BARO - calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) - #endif - #endif - #ifdef LCD_TELEMETRY // reset some values when arming - #if BARO - BAROaltMax = alt.EstAlt; - #endif - #if GPS - GPS_speedMax = 0; - #endif - #if defined( POWERMETER_HARD ) && (defined(LOG_VALUES) || defined(LCD_TELEMETRY)) - powerValueMaxMAH = 0; - #endif - #ifdef WATTS - wattsMax = 0; - #endif - #endif - #ifdef LOG_PERMANENT - plog.arm++; // #arm events - plog.running = 1; // toggle on arm & disarm to monitor for clean shutdown vs. powercut - // write now. - writePLog(); - #endif - } - } else if(!f.ARMED) { - blinkLED(2,255,1); - SET_ALARM(ALRM_FAC_ACC, ALRM_LVL_ON); - } -} -void go_disarm() { - if (f.ARMED) { - f.ARMED = 0; - #ifdef LOG_PERMANENT - plog.disarm++; // #disarm events - plog.armed_time = armedTime ; // lifetime in seconds - if (failsafeEvents) plog.failsafe++; // #acitve failsafe @ disarm - if (i2c_errors_count > 10) plog.i2c++; // #i2c errs @ disarm - plog.running = 0; // toggle @ arm & disarm to monitor for clean shutdown vs. powercut - // write now. - writePLog(); - #endif - } -} - -// ******** Main Loop ********* -void loop () { - static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors - static uint8_t rcSticks; // this hold sticks position for command combos - uint8_t axis,i; - int16_t error,errorAngle; - int16_t delta; - int16_t PTerm = 0,ITerm = 0,DTerm, PTermACC, ITermACC; - static int16_t lastGyro[2] = {0,0}; - static int16_t errorAngleI[2] = {0,0}; - #if PID_CONTROLLER == 1 - static int32_t errorGyroI_YAW; - static int16_t delta1[2],delta2[2]; - static int16_t errorGyroI[2] = {0,0}; - #elif PID_CONTROLLER == 2 - static int16_t delta1[3],delta2[3]; - static int32_t errorGyroI[3] = {0,0,0}; - static int16_t lastError[3] = {0,0,0}; - int16_t deltaSum; - int16_t AngleRateTmp, RateError; - #endif - static uint16_t rcTime = 0; - static int16_t initialThrottleHold; - int16_t rc; - int32_t prop = 0; - - #if defined(SERIAL_RX) - if (spekFrameFlags == 0x01) readSerial_RX(); - #endif - #if defined(OPENLRSv2MULTI) - Read_OpenLRS_RC(); - #endif - - #if defined(SERIAL_RX) - if ((spekFrameDone == 0x01) || ((int16_t)(currentTime-rcTime) >0 )) { - spekFrameDone = 0x00; - #else - if ((int16_t)(currentTime-rcTime) >0 ) { // 50Hz - #endif - rcTime = currentTime + 20000; - computeRC(); - // Failsafe routine - added by MIS - #if defined(FAILSAFE) - if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level - for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec) - rcData[THROTTLE] = conf.failsafe_throttle; - if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec) - go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents - f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm - } - failsafeEvents++; - } - if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm - go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents - f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm - } - failsafeCnt++; - #endif - // end of failsafe routine - next change is made with RcOptions setting - - // ------------------ STICKS COMMAND HANDLER -------------------- - // checking sticks positions - uint8_t stTmp = 0; - for(i=0;i<4;i++) { - stTmp >>= 2; - if(rcData[i] > MINCHECK) stTmp |= 0x80; // check for MIN - if(rcData[i] < MAXCHECK) stTmp |= 0x40; // check for MAX - } - if(stTmp == rcSticks) { - if(rcDelayCommand<250) rcDelayCommand++; - } else rcDelayCommand = 0; - rcSticks = stTmp; - - // perform actions - if (rcData[THROTTLE] <= MINCHECK) { // THROTTLE at minimum - #if !defined(FIXEDWING) - errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; - #if PID_CONTROLLER == 1 - errorGyroI_YAW = 0; - #elif PID_CONTROLLER == 2 - errorGyroI[YAW] = 0; - #endif - errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - #endif - if (conf.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX - if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm(); else if (f.ARMED) go_disarm(); - } - } - if(rcDelayCommand == 20) { - if(f.ARMED) { // actions during armed - #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW - if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); // Disarm via YAW - #endif - #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL - if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); // Disarm via ROLL - #endif - } else { // actions during not armed - i=0; - if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - calibratingG=512; - #if GPS - GPS_reset_home_position(); - #endif - #if BARO - calibratingB=10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) - #endif - } - #if defined(INFLIGHT_ACC_CALIBRATION) - else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) { // Inflight ACC calibration START/STOP - if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing - AccInflightCalibrationMeasurementDone = 0; - AccInflightCalibrationSavetoEEProm = 1; - }else{ - AccInflightCalibrationArmed = !AccInflightCalibrationArmed; - #if defined(BUZZER) - if (AccInflightCalibrationArmed) SET_ALARM_BUZZER(ALRM_FAC_TOGGLE, ALRM_LVL_TOGGLE_2); - else SET_ALARM_BUZZER(ALRM_FAC_TOGGLE, ALRM_LVL_TOGGLE_ELSE); - #endif - } - } - #endif - #ifdef MULTIPLE_CONFIGURATION_PROFILES - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; // ROLL left -> Profile 1 - else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 - else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 - if(i) { - global_conf.currentSet = i-1; - writeGlobalSet(0); - readEEPROM(); - blinkLED(2,40,i); - SET_ALARM(ALRM_FAC_TOGGLE, i); - } - #endif - if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) { // Enter LCD config - #if defined(LCD_CONF) - configurationLoop(); // beginning LCD configuration - #endif - previousTime = micros(); - } - #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW - else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); // Arm via YAW - #endif - #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL - else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); // Arm via ROLL - #endif - #ifdef LCD_TELEMETRY_AUTO - else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { // Auto telemetry ON/OFF - if (telemetry_auto) { - telemetry_auto = 0; - telemetry = 0; - } else - telemetry_auto = 1; - } - #endif - #ifdef LCD_TELEMETRY_STEP - else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { // Telemetry next step - telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)]; - #if defined( OLED_I2C_128x64) - if (telemetry != 0) i2c_OLED_init(); - #elif defined(OLED_DIGOLE) - if (telemetry != 0) i2c_OLED_DIGOLE_init(); - #endif - LCDclear(); - } - #endif - #if ACC - else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; // throttle=max, yaw=left, pitch=min - #endif - #if MAG - else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; // throttle=max, yaw=right, pitch=min - #endif - i=0; - if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;} - else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;} - else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;} - else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;} - if (i) { - writeParams(1); - rcDelayCommand = 0; // allow autorepetition - #if defined(LED_RING) - blinkLedRing(); - #endif - } - } - } - #if defined(LED_FLASHER) - led_flasher_autoselect_sequence(); - #endif - - #if defined(INFLIGHT_ACC_CALIBRATION) - if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement - InflightcalibratingA = 50; - AccInflightCalibrationArmed = 0; - } - if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored - if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ - InflightcalibratingA = 50; - } - }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ - AccInflightCalibrationMeasurementDone = 0; - AccInflightCalibrationSavetoEEProm = 1; - } - #endif - - #if defined(EXTENDED_AUX_STATES) - uint32_t auxState = 0; - for(i=0;i<4;i++) - auxState |= - (uint32_t)(rcData[AUX1+i]<1230)<<(6*i) | - (uint32_t)(12311750)<<(6*i+5); - #else - uint16_t auxState = 0; - for(i=0;i<4;i++) - auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (13001700)<<(3*i+2); - #endif - - for(i=0;i0; - - // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false - #if ACC - if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) { - // bumpless transfer to Level mode - if (!f.ANGLE_MODE) { - errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - f.ANGLE_MODE = 1; - } - } else { - if(f.ANGLE_MODE){ - errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; - } - f.ANGLE_MODE = 0; - } - if ( rcOptions[BOXHORIZON] ) { - f.ANGLE_MODE = 0; - if (!f.HORIZON_MODE) { - errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - f.HORIZON_MODE = 1; - } - } else { - if(f.HORIZON_MODE){ - errorGyroI[ROLL] = 0;errorGyroI[PITCH] = 0; - } - f.HORIZON_MODE = 0; - } - #endif - - if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; - #if !defined(GPS_LED_INDICATOR) - if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;} - #endif - - #if BARO - #if (!defined(SUPPRESS_BARO_ALTHOLD)) - #if GPS - if (GPS_conf.takeover_baro) rcOptions[BOXBARO] = (rcOptions[BOXBARO] || f.GPS_BARO_MODE); - #endif - if (rcOptions[BOXBARO]) { - if (!f.BARO_MODE) { - f.BARO_MODE = 1; - AltHold = alt.EstAlt; - #if defined(ALT_HOLD_THROTTLE_MIDPOINT) - initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT; - #else - initialThrottleHold = rcCommand[THROTTLE]; - #endif - errorAltitudeI = 0; - BaroPID=0; - } - } else { - f.BARO_MODE = 0; - } - #endif - #ifdef VARIOMETER - if (rcOptions[BOXVARIO]) { - if (!f.VARIO_MODE) { - f.VARIO_MODE = 1; - } - } else { - f.VARIO_MODE = 0; - } - #endif - #endif - if (rcOptions[BOXMAG]) { - if (!f.MAG_MODE) { - f.MAG_MODE = 1; - magHold = att.heading; - } - } else { - f.MAG_MODE = 0; - } - #if defined(HEADFREE) - if (rcOptions[BOXHEADFREE]) { - if (!f.HEADFREE_MODE) { - f.HEADFREE_MODE = 1; - } - #if defined(ADVANCED_HEADFREE) - if ((f.GPS_FIX && GPS_numSat >= 5) && (GPS_distanceToHome > ADV_HEADFREE_RANGE) ) { - if (GPS_directionToHome < 180) {headFreeModeHold = GPS_directionToHome + 180;} else {headFreeModeHold = GPS_directionToHome - 180;} - } - #endif - } else { - f.HEADFREE_MODE = 0; - } - if (rcOptions[BOXHEADADJ]) { - headFreeModeHold = att.heading; // acquire new heading - } - #endif - - #if GPS - // This handles the three rcOptions boxes - // unlike other parts of the multiwii code, it looks for changes and not based on flag settings - // by this method a priority can be established between gps option - - //Generate a packed byte of all four GPS boxes. - uint8_t gps_modes_check = (rcOptions[BOXLAND]<< 3) + (rcOptions[BOXGPSHOME]<< 2) + (rcOptions[BOXGPSHOLD]<<1) + (rcOptions[BOXGPSNAV]); - - if (f.ARMED ) { //Check GPS status and armed - //TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP and sudden speed jumps - if (f.GPS_FIX) { - if (GPS_numSat >5 ) { - if (prv_gps_modes != gps_modes_check) { //Check for change since last loop - NAV_error = NAV_ERROR_NONE; - if (rcOptions[BOXGPSHOME]) { // RTH has the priotity over everything else - init_RTH(); - } else if (rcOptions[BOXGPSHOLD]) { //Position hold has priority over mission execution //But has less priority than RTH - if (f.GPS_mode == GPS_MODE_NAV) - NAV_paused_at = mission_step.number; - f.GPS_mode = GPS_MODE_HOLD; - f.GPS_BARO_MODE = false; - GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS_coord[LON]); //hold at the current position - set_new_altitude(alt.EstAlt); //and current altitude - NAV_state = NAV_STATE_HOLD_INFINIT; - } else if (rcOptions[BOXLAND]) { //Land now (It has priority over Navigation) - f.GPS_mode = GPS_MODE_HOLD; - f.GPS_BARO_MODE = true; - GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS_coord[LON]); - set_new_altitude(alt.EstAlt); - NAV_state = NAV_STATE_LAND_START; - } else if (rcOptions[BOXGPSNAV]) { //Start navigation - f.GPS_mode = GPS_MODE_NAV; //Nav mode start - f.GPS_BARO_MODE = true; - GPS_prev[LAT] = GPS_coord[LAT]; - GPS_prev[LON] = GPS_coord[LON]; - if (NAV_paused_at != 0) { - next_step = NAV_paused_at; - NAV_paused_at = 0; //Clear paused step - } else { - next_step = 1; - jump_times = -10; //Reset jump counter - } - NAV_state = NAV_STATE_PROCESS_NEXT; - } else { //None of the GPS Boxes are switched on - f.GPS_mode = GPS_MODE_NONE; - f.GPS_BARO_MODE = false; - f.THROTTLE_IGNORED = false; - f.LAND_IN_PROGRESS = 0; - f.THROTTLE_IGNORED = 0; - NAV_state = NAV_STATE_NONE; - GPS_reset_nav(); - } - prv_gps_modes = gps_modes_check; - } - } else { //numSat>5 - //numSat dropped below 5 during navigation - if (f.GPS_mode == GPS_MODE_NAV) { - NAV_paused_at = mission_step.number; - f.GPS_mode = GPS_MODE_NONE; - set_new_altitude(alt.EstAlt); //and current altitude - NAV_state = NAV_STATE_NONE; - NAV_error = NAV_ERROR_SPOILED_GPS; - prv_gps_modes = 0xff; //invalidates mode check, to allow re evaluate rcOptions when numsats raised again - } - if (f.GPS_mode == GPS_MODE_HOLD || f.GPS_mode == GPS_MODE_RTH) { - f.GPS_mode = GPS_MODE_NONE; - NAV_state = NAV_STATE_NONE; - NAV_error = NAV_ERROR_SPOILED_GPS; - prv_gps_modes = 0xff; //invalidates mode check, to allow re evaluate rcOptions when numsats raised again - } - nav[0] = 0; nav[1] = 0; - } - } else { //f.GPS_FIX - // GPS Fix dissapeared, very unlikely that we will be able to regain it, abort mission - f.GPS_mode = GPS_MODE_NONE; - NAV_state = NAV_STATE_NONE; - NAV_paused_at = 0; - NAV_error = NAV_ERROR_GPS_FIX_LOST; - GPS_reset_nav(); - prv_gps_modes = 0xff; //Gives a chance to restart mission when regain fix - } - } else { //copter is armed - //copter is disarmed - f.GPS_mode = GPS_MODE_NONE; - f.GPS_BARO_MODE = false; - f.THROTTLE_IGNORED = false; - NAV_state = NAV_STATE_NONE; - NAV_paused_at = 0; - NAV_error = NAV_ERROR_DISARMED; - GPS_reset_nav(); - } - - #endif //GPS - - #if defined(FIXEDWING) || defined(HELICOPTER) - if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;} - else {f.PASSTHRU_MODE = 0;} - #endif - - } else { // not in rc loop - static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes - switch (taskOrder) { - case 0: - taskOrder++; - #if MAG - if (Mag_getADC() != 0) break; // 320 µs - #endif - case 1: - taskOrder++; - #if BARO - if (Baro_update() != 0) break; // for MS baro: I2C set and get: 220 us - presure and temperature computation 160 us - #endif - case 2: - taskOrder++; - #if BARO - if (getEstimatedAltitude() != 0) break; // 280 us - #endif - case 3: - taskOrder++; - #if GPS - if (GPS_Compute() != 0) break; // performs computation on new frame only if present - #if defined(I2C_GPS) - if (GPS_NewData() != 0) break; // 160 us with no new data / much more with new data - #endif - #endif - case 4: - taskOrder=0; - #if SONAR - Sonar_update(); //debug[2] = sonarAlt; - #endif - #ifdef LANDING_LIGHTS_DDR - auto_switch_landing_lights(); - #endif - #ifdef VARIOMETER - if (f.VARIO_MODE) vario_signaling(); - #endif - break; - } - } - - while(1) { - currentTime = micros(); - cycleTime = currentTime - previousTime; - #if defined(LOOP_TIME) - if (cycleTime >= LOOP_TIME) break; - #else - break; - #endif - } - previousTime = currentTime; - - computeIMU(); - - //*********************************** - //**** Experimental FlightModes ***** - //*********************************** - #if defined(ACROTRAINER_MODE) - if(f.ANGLE_MODE){ - if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE ) { - f.ANGLE_MODE=0; - f.HORIZON_MODE=0; - f.MAG_MODE=0; - f.BARO_MODE=0; - GPS_mode = GPS_MODE_NONE; - } - } - #endif - - //*********************************** - // THROTTLE sticks during mission and RTH - #if GPS - if (GPS_conf.ignore_throttle == 1) { - if (f.GPS_mode == GPS_MODE_NAV || f.GPS_mode == GPS_MODE_RTH) { - //rcCommand[ROLL] = 0; - //rcCommand[PITCH] = 0; - //rcCommand[YAW] = 0; - f.THROTTLE_IGNORED = 1; - } else - f.THROTTLE_IGNORED = 0; - } - - //Heading manipulation TODO: Do heading manipulation - #endif - - if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) { - int16_t dif = att.heading - magHold; - if (dif <= - 180) dif += 360; - if (dif >= + 180) dif -= 360; - if (f.SMALL_ANGLES_25 || (f.GPS_mode != 0)) rcCommand[YAW] -= dif*conf.pid[PIDMAG].P8 >> 5; //Always correct maghold in GPS mode - } else magHold = att.heading; - - #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) - /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease - * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms) - */ - if (f.BARO_MODE) { - static uint8_t isAltHoldChanged = 0; - static int16_t AltHoldCorr = 0; - - #if GPS - if (f.LAND_IN_PROGRESS) { //If autoland is in progress then take over and decrease alt slowly - AltHoldCorr -= GPS_conf.land_speed; - if(abs(AltHoldCorr) > 512) { - AltHold += AltHoldCorr/512; - AltHoldCorr %= 512; - } - } - #endif - //IF Throttle not ignored then allow change altitude with the stick.... - if ( (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) && !f.THROTTLE_IGNORED) { - // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) - AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold; - if(abs(AltHoldCorr) > 512) { - AltHold += AltHoldCorr/512; - AltHoldCorr %= 512; - } - isAltHoldChanged = 1; - } else if (isAltHoldChanged) { - AltHold = alt.EstAlt; - isAltHoldChanged = 0; - } - rcCommand[THROTTLE] = initialThrottleHold + BaroPID; - } - #endif //BARO - - - - #if defined(THROTTLE_ANGLE_CORRECTION) - if(f.ANGLE_MODE || f.HORIZON_MODE) { - rcCommand[THROTTLE]+= throttleAngleCorrection; - } - #endif - - #if GPS - //TODO: split cos_yaw calculations into two phases (X and Y) - if (( f.GPS_mode != GPS_MODE_NONE ) && f.GPS_FIX_HOME ) { - float sin_yaw_y = sin(att.heading*0.0174532925f); - float cos_yaw_x = cos(att.heading*0.0174532925f); - GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; - GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; - } else { - GPS_angle[ROLL] = 0; - GPS_angle[PITCH] = 0; - } - - //Used to communicate back nav angles to the GPS simulator (for HIL testing) - #if defined(GPS_SIMULATOR) - SerialWrite(2,0xa5); - SerialWrite16(2,nav[LAT]+rcCommand[PITCH]); - SerialWrite16(2,nav[LON]+rcCommand[ROLL]); - SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check - #endif - - #endif //GPS - - //**** PITCH & ROLL & YAW PID **** - #if PID_CONTROLLER == 1 // evolved oldschool - if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512); - - // PITCH & ROLL - for(axis=0;axis<2;axis++) { - rc = rcCommand[axis]<<1; - error = rc - imu.gyroData[axis]; - errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here - if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0; - - ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 - - PTerm = mul(rc,conf.pid[axis].P8)>>6; - - if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC - // 50 degrees max inclination - errorAngle = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here - errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here - - PTermACC = mul(errorAngle,conf.pid[PIDLEVEL].P8)>>7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result - - int16_t limit = conf.pid[PIDLEVEL].D8*5; - PTermACC = constrain(PTermACC,-limit,+limit); - - ITermACC = mul(errorAngleI[axis],conf.pid[PIDLEVEL].I8)>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result - - ITerm = ITermACC + ((ITerm-ITermACC)*prop>>9); - PTerm = PTermACC + ((PTerm-PTermACC)*prop>>9); - } - - PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation - - delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyro[axis] = imu.gyroData[axis]; - DTerm = delta1[axis]+delta2[axis]+delta; - delta2[axis] = delta1[axis]; - delta1[axis] = delta; - - DTerm = mul(DTerm,dynD8[axis])>>5; // 32 bits is needed for calculation - - axisPID[axis] = PTerm + ITerm - DTerm; - } - - //YAW - #define GYRO_P_MAX 300 - #define GYRO_I_MAX 250 - - rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30)) >> 5; - - error = rc - imu.gyroData[YAW]; - errorGyroI_YAW += mul(error,conf.pid[YAW].I8); - errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28)); - if (abs(rc) > 50) errorGyroI_YAW = 0; - - PTerm = mul(error,conf.pid[YAW].P8)>>6; - #ifndef COPTER_WITH_SERVO - int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8; - PTerm = constrain(PTerm,-limit,+limit); - #endif - - ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX); - - axisPID[YAW] = PTerm + ITerm; - - #elif PID_CONTROLLER == 2 // alexK - #define GYRO_I_MAX 256 - #define ACC_I_MAX 256 - prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500] - - //----------PID controller---------- - for(axis=0;axis<3;axis++) { - //-----Get the desired angle rate depending on flight mode - if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC - // calculate error and limit the angle to 50 degrees max inclination - errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here - } - if (axis == 2) {//YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5); - } else { - if (!f.ANGLE_MODE) {//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4; - if (f.HORIZON_MODE) { - //mix up angle error to desired AngleRateTmp to add a little auto-level feel - AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8)>>8; - } - } else {//it's the ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8)>>4; - } - } - - //--------low-level gyro-based PID. ---------- - //Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - //-----calculate scaled error.AngleRates - //multiplication of rcCommand corresponds to changing the sticks scaling here - RateError = AngleRateTmp - imu.gyroData[axis]; - - //-----calculate P component - PTerm = ((int32_t) RateError * conf.pid[axis].P8)>>7; - - //-----calculate I component - //there should be no division before accumulating the error to integrator, because the precision would be reduced. - //Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - //Time correction (to avoid different I scaling for different builds based on average cycle time) - //is normalized to cycle time = 2048. - errorGyroI[axis] += (((int32_t) RateError * cycleTime)>>11) * conf.pid[axis].I8; - //limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - //I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -GYRO_I_MAX<<13, (int32_t) +GYRO_I_MAX<<13); - ITerm = errorGyroI[axis]>>13; - - //-----calculate D-term - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; - - //Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime>>4)))>>6; - //add moving average here to reduce noise - deltaSum = delta1[axis]+delta2[axis]+delta; - delta2[axis] = delta1[axis]; - delta1[axis] = delta; - - //DTerm = (deltaSum*conf.pid[axis].D8)>>8; - //Solve overflow in calculation above... - DTerm = ((int32_t)deltaSum*conf.pid[axis].D8)>>8; - //-----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - #else - #error "*** you must set PID_CONTROLLER to one existing implementation" - #endif - mixTable(); - // do not update servos during unarmed calibration of sensors which are sensitive to vibration - #if defined(DISABLE_SERVOS_WHEN_UNARMED) - if (f.ARMED) writeServos(); - #else - if ( (f.ARMED) || ((!calibratingG) && (!calibratingA)) ) writeServos(); - #endif - writeMotors(); -} +/* +MultiWiiCopter by Alexandre Dubus +www.multiwii.com +November 2013 V2.3 + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + any later version. see +*/ + +#include + +#include "Arduino.h" +#include "config.h" +#include "def.h" +#include "types.h" +#include "MultiWii.h" +#include "Alarms.h" +#include "EEPROM.h" +#include "IMU.h" +#include "LCD.h" +#include "Output.h" +#include "RX.h" +#include "Sensors.h" +#include "Serial.h" +#include "GPS.h" +#include "Protocol.h" +#include "Bluetooth.h" + +#include + +/*********** RC alias *****************/ + +const char pidnames[] PROGMEM = + "ROLL;" + "PITCH;" + "YAW;" + "ALT;" + "Pos;" + "PosR;" + "NavR;" + "LEVEL;" + "MAG;" + "VEL;" +; + +const char boxnames[] PROGMEM = // names for dynamic generation of config GUI + "ARM;" + #if ACC + "ANGLE;" + "HORIZON;" + #endif + #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) + "BARO;" + #endif + #ifdef VARIOMETER + "VARIO;" + #endif + "MAG;" + #if defined(HEADFREE) + "HEADFREE;" + "HEADADJ;" +#endif +#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) + "CAMSTAB;" +#endif +#if defined(CAMTRIG) + "CAMTRIG;" +#endif +#if GPS + "GPS HOME;" + "GPS HOLD;" +#endif +#if defined(FIXEDWING) || defined(HELICOPTER) + "PASSTHRU;" +#endif +#if defined(BUZZER) + "BEEPER;" +#endif +#if defined(LED_FLASHER) + "LEDMAX;" + "LEDLOW;" +#endif +#if defined(LANDING_LIGHTS_DDR) + "LLIGHTS;" +#endif +#ifdef INFLIGHT_ACC_CALIBRATION + "CALIB;" +#endif +#ifdef GOVERNOR_P + "GOVERNOR;" +#endif +#ifdef OSD_SWITCH + "OSD SW;" +#endif +#if GPS + "MISSION;" + "LAND;" +#endif + ; + +const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. + 0, //"ARM;" + #if ACC + 1, //"ANGLE;" + 2, //"HORIZON;" + #endif + #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) + 3, //"BARO;" + #endif + #ifdef VARIOMETER + 4, //"VARIO;" + #endif + 5, //"MAG;" +#if defined(HEADFREE) + 6, //"HEADFREE;" + 7, //"HEADADJ;" +#endif +#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) + 8, //"CAMSTAB;" +#endif +#if defined(CAMTRIG) + 9, //"CAMTRIG;" +#endif +#if GPS + 10, //"GPS HOME;" + 11, //"GPS HOLD;" +#endif +#if defined(FIXEDWING) || defined(HELICOPTER) + 12, //"PASSTHRU;" +#endif +#if defined(BUZZER) + 13, //"BEEPER;" +#endif +#if defined(LED_FLASHER) + 14, //"LEDMAX;" + 15, //"LEDLOW;" +#endif +#if defined(LANDING_LIGHTS_DDR) + 16, //"LLIGHTS;" +#endif +#ifdef INFLIGHT_ACC_CALIBRATION + 17, //"CALIB;" +#endif +#ifdef GOVERNOR_P + 18, //"GOVERNOR;" +#endif +#ifdef OSD_SWITCH + 19, //"OSD_SWITCH;" +#endif +#if GPS + 20, //"MISSION;" + 21, //"LAND;" +#endif + }; + + +uint32_t currentTime = 0; +uint16_t previousTime = 0; +uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop +uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. +uint16_t calibratingB = 0; // baro calibration = get new ground pressure value +uint16_t calibratingG; +int16_t magHold,headFreeModeHold; // [-180;+180] +uint8_t vbatMin = VBATNOMINAL; // lowest battery voltage in 0.1V steps +uint8_t rcOptions[CHECKBOXITEMS]; +int32_t AltHold; // in cm +int16_t sonarAlt; +int16_t BaroPID = 0; +int16_t errorAltitudeI = 0; + +// ************** +// gyro+acc IMU +// ************** +int16_t gyroZero[3] = {0,0,0}; + +imu_t imu; + +analog_t analog; + +alt_t alt; + +att_t att; + +#if defined(ARMEDTIMEWARNING) + uint32_t ArmedTimeWarningMicroSeconds = 0; +#endif + +int16_t debug[4]; + +flags_struct_t f; + +//for log +#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) + uint16_t cycleTimeMax = 0; // highest ever cycle timen + uint16_t cycleTimeMin = 65535; // lowest ever cycle timen + int32_t BAROaltMax; // maximum value + uint16_t GPS_speedMax = 0; // maximum speed from gps + #ifdef POWERMETER_HARD + uint16_t powerValueMaxMAH = 0; + #endif + #if defined(WATTS) + uint16_t wattsMax = 0; + #endif +#endif +#if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) + uint32_t armedTime = 0; +#endif + +int16_t i2c_errors_count = 0; + + +#if defined(THROTTLE_ANGLE_CORRECTION) + int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, + int8_t cosZ = 100; // cos(angleZ)*100 +#endif + + + +// ********************** +//Automatic ACC Offset Calibration +// ********************** +#if defined(INFLIGHT_ACC_CALIBRATION) + uint16_t InflightcalibratingA = 0; + int16_t AccInflightCalibrationArmed; + uint16_t AccInflightCalibrationMeasurementDone = 0; + uint16_t AccInflightCalibrationSavetoEEProm = 0; + uint16_t AccInflightCalibrationActive = 0; +#endif + +// ********************** +// power meter +// ********************** +#if defined(POWERMETER) || ( defined(LOG_VALUES) && (LOG_VALUES >= 3) ) + uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum + uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop() + uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6] + uint16_t powerValue = 0; // last known current +#endif +uint16_t intPowerTrigger1; + +// ********************** +// telemetry +// ********************** +#if defined(LCD_TELEMETRY) + uint8_t telemetry = 0; + uint8_t telemetry_auto = 0; + int16_t annex650_overrun_count = 0; +#endif +#ifdef LCD_TELEMETRY_STEP + char telemetryStepSequence [] = LCD_TELEMETRY_STEP; + uint8_t telemetryStepIndex = 0; +#endif + +// ****************** +// rc functions +// ****************** +#define ROL_LO (1<<(2*ROLL)) +#define ROL_CE (3<<(2*ROLL)) +#define ROL_HI (2<<(2*ROLL)) +#define PIT_LO (1<<(2*PITCH)) +#define PIT_CE (3<<(2*PITCH)) +#define PIT_HI (2<<(2*PITCH)) +#define YAW_LO (1<<(2*YAW)) +#define YAW_CE (3<<(2*YAW)) +#define YAW_HI (2<<(2*YAW)) +#define THR_LO (1<<(2*THROTTLE)) +#define THR_CE (3<<(2*THROTTLE)) +#define THR_HI (2<<(2*THROTTLE)) + +int16_t failsafeEvents = 0; +volatile int16_t failsafeCnt = 0; + +int16_t rcData[RC_CHANS]; // interval [1000;2000] +int16_t rcSerial[8]; // interval [1000;2000] - is rcData coming from MSP +int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW +uint8_t rcSerialCount = 0; // a counter to select legacy RX when there is no more MSP rc serial data +int16_t lookupPitchRollRC[5];// lookup table for expo & RC rate PITCH+ROLL +uint16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE + +#if defined(SERIAL_RX) + volatile uint8_t spekFrameFlags; + volatile uint32_t spekTimeLast; + uint8_t spekFrameDone; +#endif + +#if defined(OPENLRSv2MULTI) + uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages +#endif + + +// ************************* +// motor and servo functions +// ************************* +int16_t axisPID[3]; +int16_t motor[8]; +int16_t servo[8] = {1500,1500,1500,1500,1500,1500,1500,1000}; + +// ************************ +// EEPROM Layout definition +// ************************ +static uint8_t dynP8[2], dynD8[2]; + +global_conf_t global_conf; + +conf_t conf; + +#ifdef LOG_PERMANENT + plog_t plog; +#endif + +// ********************** +// GPS common variables, no need to put them in defines, since compiller will optimize out unused variables +// ********************** +#if GPS + gps_conf_struct GPS_conf; +#endif + int16_t GPS_angle[2] = { 0, 0}; // the angles that must be applied for GPS correction + int32_t GPS_coord[2]; + int32_t GPS_home[2]; + int32_t GPS_hold[2]; + int32_t GPS_prev[2]; //previous pos + int32_t GPS_poi[2]; + uint8_t GPS_numSat; + uint16_t GPS_distanceToHome; // distance to home - unit: meter + int16_t GPS_directionToHome; // direction to home - unit: degree + int32_t GPS_directionToPoi; + uint16_t GPS_altitude; // GPS altitude - unit: meter + uint16_t GPS_speed; // GPS speed - unit: cm/s + uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update + uint16_t GPS_ground_course = 0; // - unit: degree*10 + + //uint8_t GPS_mode = GPS_MODE_NONE; // contains the current selected gps flight mode --> moved to the f. structure + uint8_t NAV_state = 0; // NAV_STATE_NONE; /// State of the nav engine + uint8_t NAV_error = 0; // NAV_ERROR_NONE; + uint8_t prv_gps_modes = 0; /// GPS_checkbox items packed into 1 byte for checking GPS mode changes + uint32_t nav_timer_stop = 0; /// common timer used in navigation (contains the desired stop time in millis() + uint16_t nav_hold_time; /// time in seconds to hold position + uint8_t NAV_paused_at = 0; // This contains the mission step where poshold paused the runing mission. + + uint8_t next_step = 1; /// The mission step which is upcoming it equals with the mission_step stored in EEPROM + int16_t jump_times = -10; +#if GPS + mission_step_struct mission_step; +#endif + + // The desired bank towards North (Positive) or South (Negative) : latitude + // The desired bank towards East (Positive) or West (Negative) : longitude + int16_t nav[2]; + int16_t nav_rated[2]; //Adding a rate controller to the navigation to make it smoother + + // The orginal altitude used as base our new altitude during nav + int32_t original_altitude; + //This is the target what we want to reach + int32_t target_altitude; + //This is the interim value which is feeded into the althold controller + int32_t alt_to_hold; + + uint32_t alt_change_timer; + int8_t alt_change_flag; + uint32_t alt_change; + +uint8_t alarmArray[ALRM_FAC_SIZE]; // array + +#if BARO + int32_t baroPressure; + int16_t baroTemperature; + int32_t baroPressureSum; +#endif + +void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds + static uint32_t calibratedAccTime; + uint16_t tmp,tmp2; + uint8_t axis,prop1,prop2; + + // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value (or collective.pitch value for heli) + #ifdef HELICOPTER + #define DYN_THR_PID_CHANNEL COLLECTIVE_PITCH + #else + #define DYN_THR_PID_CHANNEL THROTTLE + #endif + prop2 = 128; // prop2 was 100, is 128 now + if (rcData[DYN_THR_PID_CHANNEL]>1500) { // breakpoint is fix: 1500 + if (rcData[DYN_THR_PID_CHANNEL]<2000) { + prop2 -= ((uint16_t)conf.dynThrPID*(rcData[DYN_THR_PID_CHANNEL]-1500)>>9); // /512 instead of /500 + } else { + prop2 -= conf.dynThrPID; + } + } + + for(axis=0;axis<3;axis++) { + tmp = min(abs(rcData[axis]-MIDRC),500); + #if defined(DEADBAND) + if (tmp>DEADBAND) { tmp -= DEADBAND; } + else { tmp=0; } + #endif + if(axis!=2) { //ROLL & PITCH + tmp2 = tmp>>7; // 500/128 = 3.9 => range [0;3] + rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7); + prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500 + prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128 prop2: max is 128 result prop1: max is 128 + dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now + dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now + } else { // YAW + rcCommand[axis] = tmp; + } + if (rcData[axis] [0;2559] + tmp2 = tmp/256; // range [0;9] + rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [conf.minthrottle;MAXTHROTTLE] + #if defined(HEADFREE) + if(f.HEADFREE_MODE) { //to optimize + float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533 + float cosDiff = cos(radDiff); + float sinDiff = sin(radDiff); + int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff; + rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; + rcCommand[PITCH] = rcCommand_PITCH; + } + #endif + + // query at most one multiplexed analog channel per MWii cycle + static uint8_t analogReader =0; + switch (analogReader++ % (3+VBAT_CELLS_NUM)) { + case 0: + { + #if defined(POWERMETER_HARD) + static uint32_t lastRead = currentTime; + static uint8_t ind = 0; + static uint16_t pvec[PSENSOR_SMOOTH], psum; + uint16_t p = analogRead(PSENSORPIN); + //LCDprintInt16(p); LCDcrlf(); + //debug[0] = p; + #if PSENSOR_SMOOTH != 1 + psum += p; + psum -= pvec[ind]; + pvec[ind++] = p; + ind %= PSENSOR_SMOOTH; + p = psum / PSENSOR_SMOOTH; + #endif + powerValue = ( conf.psensornull > p ? conf.psensornull - p : p - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun + analog.amperage = ((uint32_t)powerValue * conf.pint2ma) / 100; // [100mA] //old (will overflow for 65A: powerValue * conf.pint2ma; // [1mA] + pMeter[PMOTOR_SUM] += ((currentTime-lastRead) * (uint32_t)((uint32_t)powerValue*conf.pint2ma))/100000; // [10 mA * msec] + lastRead = currentTime; + #endif // POWERMETER_HARD + break; + } + + case 1: + { + #if defined(VBAT) && !defined(VBAT_CELLS) + static uint8_t ind = 0; + static uint16_t vvec[VBAT_SMOOTH], vsum; + uint16_t v = analogRead(V_BATPIN); + #if VBAT_SMOOTH == 1 + analog.vbat = (v<<4) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps + #else + vsum += v; + vsum -= vvec[ind]; + vvec[ind++] = v; + ind %= VBAT_SMOOTH; + #if VBAT_SMOOTH == 16 + analog.vbat = vsum / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps + #elif VBAT_SMOOTH < 16 + analog.vbat = (vsum * (16/VBAT_SMOOTH)) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps + #else + analog.vbat = ((vsum /VBAT_SMOOTH) * 16) / conf.vbatscale + VBAT_OFFSET; // result is Vbatt in 0.1V steps + #endif + #endif + #endif // VBAT + break; + } + case 2: + { + #if defined(RX_RSSI) + static uint8_t ind = 0; + static uint16_t rvec[RSSI_SMOOTH], rsum, r; + + // http://www.multiwii.com/forum/viewtopic.php?f=8&t=5530 + #if defined(RX_RSSI_CHAN) + uint16_t rssi_Input = constrain(rcData[RX_RSSI_CHAN],1000,2000); + r = map((uint16_t)rssi_Input , 1000, 2000, 0, 1023); + #else + r = analogRead(RX_RSSI_PIN); + #endif + + #if RSSI_SMOOTH == 1 + analog.rssi = r; + #else + rsum += r; + rsum -= rvec[ind]; + rvec[ind++] = r; + ind %= RSSI_SMOOTH; + r = rsum / RSSI_SMOOTH; + analog.rssi = r; + #endif + #endif // RX RSSI + break; + } + default: // here analogReader >=4, because of ++ in switch() + { + #if defined(VBAT) && defined(VBAT_CELLS) + if ( (analogReader<4) || (analogReader>4+VBAT_CELLS_NUM-1) ) break; + uint8_t ind = analogReader-4; + static uint16_t vbatcells_pins[VBAT_CELLS_NUM] = VBAT_CELLS_PINS; + static uint8_t vbatcells_offset[VBAT_CELLS_NUM] = VBAT_CELLS_OFFSETS; + static uint8_t vbatcells_div[VBAT_CELLS_NUM] = VBAT_CELLS_DIVS; + uint16_t v = analogRead(vbatcells_pins[ind]); + analog.vbatcells[ind] = vbatcells_offset[ind] + (v << 2) / vbatcells_div[ind]; // result is Vbatt in 0.1V steps + if (ind == VBAT_CELLS_NUM -1) analog.vbat = analog.vbatcells[ind]; + #endif // VBAT) && defined(VBAT_CELLS) + break; + } // end default + } // end of switch() + +#if defined( POWERMETER_HARD ) && (defined(LOG_VALUES) || defined(LCD_TELEMETRY)) + if (analog.amperage > powerValueMaxMAH) powerValueMaxMAH = analog.amperage; +#endif + +#if defined(WATTS) + analog.watts = (analog.amperage * analog.vbat) / 100; // [0.1A] * [0.1V] / 100 = [Watt] + #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) + if (analog.watts > wattsMax) wattsMax = analog.watts; + #endif +#endif + + #if defined(BUZZER) + alarmHandler(); // external buzzer routine that handles buzzer events globally now + #endif + + + if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis + LEDPIN_TOGGLE; + } else { + if (f.ACC_CALIBRATED) {LEDPIN_OFF;} + if (f.ARMED) {LEDPIN_ON;} + } + + #if defined(LED_RING) + static uint32_t LEDTime; + if ( currentTime > LEDTime ) { + LEDTime = currentTime + 50000; + i2CLedRingState(); + } + #endif + + #if defined(LED_FLASHER) + auto_switch_led_flasher(); + #endif + + if ( currentTime > calibratedAccTime ) { + if (! f.SMALL_ANGLES_25) { + // the multi uses ACC and is not calibrated or is too much inclinated + f.ACC_CALIBRATED = 0; + LEDPIN_TOGGLE; + calibratedAccTime = currentTime + 100000; + } else { + f.ACC_CALIBRATED = 1; + } + } + + #if !(defined(SERIAL_RX) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if SERIAL RX in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0. + serialCom(); + #endif + + #if defined(POWERMETER) + analog.intPowerMeterSum = (pMeter[PMOTOR_SUM]/PLEVELDIV); + intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; + #endif + + #ifdef LCD_TELEMETRY_AUTO + static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO; + static uint8_t telemetryAutoIndex = 0; + static uint16_t telemetryAutoTimer = 0; + if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) ){ + telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)]; + LCDclear(); // make sure to clear away remnants + } + #endif + #ifdef LCD_TELEMETRY + static uint16_t telemetryTimer = 0; + if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) { + #if (LCD_TELEMETRY_DEBUG+0 > 0) + telemetry = LCD_TELEMETRY_DEBUG; + #endif + if (telemetry) lcd_telemetry(); + } + #endif + + #if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication + static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames + static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off + if(currentTime > GPSLEDTime) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ... + if(f.GPS_FIX && GPS_numSat >= 5) { + if(++blcnt > 2*GPS_numSat) blcnt = 0; + GPSLEDTime = currentTime + 150000; + if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;} + }else{ + if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;} + blcnt = 0; + } + } + #endif + + #if defined(LOG_VALUES) && (LOG_VALUES >= 2) + if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore + if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore + #endif + if (f.ARMED) { + #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) + armedTime += (uint32_t)cycleTime; + #endif + #if defined(VBAT) + if ( (analog.vbat > NO_VBAT) && (analog.vbat < vbatMin) ) vbatMin = analog.vbat; + #endif + #ifdef LCD_TELEMETRY + #if BARO + if ( (alt.EstAlt > BAROaltMax) ) BAROaltMax = alt.EstAlt; + #endif + #if GPS + if ( (GPS_speed > GPS_speedMax) ) GPS_speedMax = GPS_speed; + #endif + #endif + } +} + +void setup() { + + pinMode(3,OUTPUT); + pinMode(5,OUTPUT); + pinMode(6,OUTPUT); + pinMode(9,OUTPUT); + + SerialOpen(0,SERIAL0_COM_SPEED); + #if defined(PROMICRO) + SerialOpen(1,SERIAL1_COM_SPEED); + #endif + #if defined(MEGA) + SerialOpen(1,SERIAL1_COM_SPEED); + SerialOpen(2,SERIAL2_COM_SPEED); + SerialOpen(3,SERIAL3_COM_SPEED); + #endif + LEDPIN_PINMODE; + POWERPIN_PINMODE; + BUZZERPIN_PINMODE; + STABLEPIN_PINMODE; + POWERPIN_OFF; + initOutput(); + readGlobalSet(); + #ifndef NO_FLASH_CHECK + #if defined(MEGA) + uint16_t i = 65000; // only first ~64K for mega board due to pgm_read_byte limitation + #else + uint16_t i = 32000; + #endif + uint16_t flashsum = 0; + uint8_t pbyt; + while(i--) { + pbyt = pgm_read_byte(i); // calculate flash checksum + flashsum += pbyt; + flashsum ^= (pbyt<<8); + } + #endif + #ifdef MULTIPLE_CONFIGURATION_PROFILES + global_conf.currentSet=2; + #else + global_conf.currentSet=0; + #endif + while(1) { // check settings integrity + #ifndef NO_FLASH_CHECK + if(readEEPROM()) { // check current setting integrity + if(flashsum != global_conf.flashsum) update_constants(); // update constants if firmware is changed and integrity is OK + } + #else + readEEPROM(); // check current setting integrity + #endif + if(global_conf.currentSet == 0) break; // all checks is done + global_conf.currentSet--; // next setting for check + } + readGlobalSet(); // reload global settings for get last profile number + #ifndef NO_FLASH_CHECK + if(flashsum != global_conf.flashsum) { + global_conf.flashsum = flashsum; // new flash sum + writeGlobalSet(1); // update flash sum in global config + } + #endif + readEEPROM(); // load setting data from last used profile + blinkLED(2,40,global_conf.currentSet+1); + + #if GPS + recallGPSconf(); //Load GPS configuration parameteres + #endif + + configureReceiver(); + #if defined (PILOTLAMP) + PL_INIT; + #endif + #if defined(OPENLRSv2MULTI) + initOpenLRS(); + #endif + #if defined(Bluetooth) + bluetooth_Init(); + #endif + initSensors(); + #if GPS + GPS_set_pids(); + #endif + previousTime = micros(); + #if defined(GIMBAL) + calibratingA = 512; + #endif + calibratingG = 512; + calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles + #if defined(POWERMETER) + for(uint8_t j=0; j<=PMOTOR_SUM; j++) pMeter[j]=0; + #endif + /************************************/ + #if GPS + #if defined(GPS_SERIAL) + GPS_SerialInit(); + #endif + GPS_conf.max_wp_number = getMaxWPNumber(); + #endif + + #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(OLED_DIGOLE) || defined(LCD_TELEMETRY_STEP) + initLCD(); + #endif + #ifdef LCD_TELEMETRY_DEBUG + telemetry_auto = 1; + #endif + #ifdef LCD_CONF_DEBUG + configurationLoop(); + #endif + #ifdef LANDING_LIGHTS_DDR + init_landing_lights(); + #endif + #ifdef FASTER_ANALOG_READS + ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11 + #endif + #if defined(LED_FLASHER) + init_led_flasher(); + led_flasher_set_sequence(LED_FLASHER_SEQUENCE); + #endif + f.SMALL_ANGLES_25=1; // important for gyro only conf + #ifdef LOG_PERMANENT + // read last stored set + readPLog(); + plog.lifetime += plog.armed_time / 1000000; + plog.start++; // #powercycle/reset/initialize events + // dump plog data to terminal + #ifdef LOG_PERMANENT_SHOW_AT_STARTUP + dumpPLog(0); + #endif + plog.armed_time = 0; // lifetime in seconds + //plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut + #endif + #ifdef DEBUGMSG + debugmsg_append_str("initialization completed\n"); + #endif +} + +void go_arm() { + if(calibratingG == 0 + #if defined(ONLYARMWHENFLAT) + && f.ACC_CALIBRATED + #endif + #if defined(FAILSAFE) + && failsafeCnt < 2 + #endif + #if GPS && defined(ONLY_ALLOW_ARM_WITH_GPS_3DFIX) + && (f.GPS_FIX && GPS_numSat >= 5) + #endif + ) { + if(!f.ARMED && !f.BARO_MODE) { // arm now! + f.ARMED = 1; + #if defined(HEADFREE) + headFreeModeHold = att.heading; + #endif + magHold = att.heading; + #if defined(VBAT) + if (analog.vbat > NO_VBAT) vbatMin = analog.vbat; + #endif + #ifdef ALTITUDE_RESET_ON_ARM + #if BARO + calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) + #endif + #endif + #ifdef LCD_TELEMETRY // reset some values when arming + #if BARO + BAROaltMax = alt.EstAlt; + #endif + #if GPS + GPS_speedMax = 0; + #endif + #if defined( POWERMETER_HARD ) && (defined(LOG_VALUES) || defined(LCD_TELEMETRY)) + powerValueMaxMAH = 0; + #endif + #ifdef WATTS + wattsMax = 0; + #endif + #endif + #ifdef LOG_PERMANENT + plog.arm++; // #arm events + plog.running = 1; // toggle on arm & disarm to monitor for clean shutdown vs. powercut + // write now. + writePLog(); + #endif + } + } else if(!f.ARMED) { + blinkLED(2,255,1); + SET_ALARM(ALRM_FAC_ACC, ALRM_LVL_ON); + } +} +void go_disarm() { + if (f.ARMED) { + f.ARMED = 0; + #ifdef LOG_PERMANENT + plog.disarm++; // #disarm events + plog.armed_time = armedTime ; // lifetime in seconds + if (failsafeEvents) plog.failsafe++; // #acitve failsafe @ disarm + if (i2c_errors_count > 10) plog.i2c++; // #i2c errs @ disarm + plog.running = 0; // toggle @ arm & disarm to monitor for clean shutdown vs. powercut + // write now. + writePLog(); + #endif + } +} + +// ******** Main Loop ********* +void loop () { + static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors + static uint8_t rcSticks; // this hold sticks position for command combos + uint8_t axis,i; + int16_t error,errorAngle; + int16_t delta; + int16_t PTerm = 0,ITerm = 0,DTerm, PTermACC, ITermACC; + static int16_t lastGyro[2] = {0,0}; + static int16_t errorAngleI[2] = {0,0}; + #if PID_CONTROLLER == 1 + static int32_t errorGyroI_YAW; + static int16_t delta1[2],delta2[2]; + static int16_t errorGyroI[2] = {0,0}; + #elif PID_CONTROLLER == 2 + static int16_t delta1[3],delta2[3]; + static int32_t errorGyroI[3] = {0,0,0}; + static int16_t lastError[3] = {0,0,0}; + int16_t deltaSum; + int16_t AngleRateTmp, RateError; + #endif + static uint16_t rcTime = 0; + static int16_t initialThrottleHold; + int16_t rc; + int32_t prop = 0; + + #if defined(SERIAL_RX) + if (spekFrameFlags == 0x01) readSerial_RX(); + #endif + #if defined(OPENLRSv2MULTI) + Read_OpenLRS_RC(); + #endif + #if defined(Bluetooth) + bluetooth_Read_RC(); + #endif + + #if defined(SERIAL_RX) + if ((spekFrameDone == 0x01) || ((int16_t)(currentTime-rcTime) >0 )) { + spekFrameDone = 0x00; + #else + if ((int16_t)(currentTime-rcTime) >0 ) { // 50Hz + #endif + rcTime = currentTime + 20000; + computeRC(); + // Failsafe routine - added by MIS + #if defined(FAILSAFE) + if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level + for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec) + rcData[THROTTLE] = conf.failsafe_throttle; + if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec) + go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents + f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm + } + failsafeEvents++; + } + if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm + go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents + f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm + } + failsafeCnt++; + #endif + // end of failsafe routine - next change is made with RcOptions setting + + // ------------------ STICKS COMMAND HANDLER -------------------- + // checking sticks positions + uint8_t stTmp = 0; + for(i=0;i<4;i++) { + stTmp >>= 2; + if(rcData[i] > MINCHECK) stTmp |= 0x80; // check for MIN + if(rcData[i] < MAXCHECK) stTmp |= 0x40; // check for MAX + } + if(stTmp == rcSticks) { + if(rcDelayCommand<250) rcDelayCommand++; + } else rcDelayCommand = 0; + rcSticks = stTmp; + + // perform actions + if (rcData[THROTTLE] <= MINCHECK) { // THROTTLE at minimum + #if !defined(FIXEDWING) + errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; + #if PID_CONTROLLER == 1 + errorGyroI_YAW = 0; + #elif PID_CONTROLLER == 2 + errorGyroI[YAW] = 0; + #endif + errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; + #endif + if (conf.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX + if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm(); else if (f.ARMED) go_disarm(); + } + } + if(rcDelayCommand == 20) { + if(f.ARMED) { // actions during armed + #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW + if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); // Disarm via YAW + #endif + #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL + if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); // Disarm via ROLL + #endif + } else { // actions during not armed + i=0; + if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration + calibratingG=512; + #if GPS + GPS_reset_home_position(); + #endif + #if BARO + calibratingB=10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) + #endif + } + #if defined(INFLIGHT_ACC_CALIBRATION) + else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) { // Inflight ACC calibration START/STOP + if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing + AccInflightCalibrationMeasurementDone = 0; + AccInflightCalibrationSavetoEEProm = 1; + }else{ + AccInflightCalibrationArmed = !AccInflightCalibrationArmed; + #if defined(BUZZER) + if (AccInflightCalibrationArmed) SET_ALARM_BUZZER(ALRM_FAC_TOGGLE, ALRM_LVL_TOGGLE_2); + else SET_ALARM_BUZZER(ALRM_FAC_TOGGLE, ALRM_LVL_TOGGLE_ELSE); + #endif + } + } + #endif + #ifdef MULTIPLE_CONFIGURATION_PROFILES + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; // ROLL left -> Profile 1 + else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 + else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 + if(i) { + global_conf.currentSet = i-1; + writeGlobalSet(0); + readEEPROM(); + blinkLED(2,40,i); + SET_ALARM(ALRM_FAC_TOGGLE, i); + } + #endif + if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) { // Enter LCD config + #if defined(LCD_CONF) + configurationLoop(); // beginning LCD configuration + #endif + previousTime = micros(); + } + #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW + else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); // Arm via YAW + #endif + #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL + else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); // Arm via ROLL + #endif + #ifdef LCD_TELEMETRY_AUTO + else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { // Auto telemetry ON/OFF + if (telemetry_auto) { + telemetry_auto = 0; + telemetry = 0; + } else + telemetry_auto = 1; + } + #endif + #ifdef LCD_TELEMETRY_STEP + else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { // Telemetry next step + telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)]; + #if defined( OLED_I2C_128x64) + if (telemetry != 0) i2c_OLED_init(); + #elif defined(OLED_DIGOLE) + if (telemetry != 0) i2c_OLED_DIGOLE_init(); + #endif + LCDclear(); + } + #endif + #if ACC + else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; // throttle=max, yaw=left, pitch=min + #endif + #if MAG + else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; // throttle=max, yaw=right, pitch=min + #endif + i=0; + if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;} + else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;} + else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;} + else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;} + if (i) { + writeParams(1); + rcDelayCommand = 0; // allow autorepetition + #if defined(LED_RING) + blinkLedRing(); + #endif + } + } + } + #if defined(LED_FLASHER) + led_flasher_autoselect_sequence(); + #endif + + #if defined(INFLIGHT_ACC_CALIBRATION) + if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement + InflightcalibratingA = 50; + AccInflightCalibrationArmed = 0; + } + if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored + if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ + InflightcalibratingA = 50; + } + }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ + AccInflightCalibrationMeasurementDone = 0; + AccInflightCalibrationSavetoEEProm = 1; + } + #endif + + #if defined(EXTENDED_AUX_STATES) + uint32_t auxState = 0; + for(i=0;i<4;i++) + auxState |= (rcData[AUX1+i]<1230)<<(6*i) | + (12311750)<<(6*i+5); + #else + uint16_t auxState = 0; + for(i=0;i<4;i++) + auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (13001700)<<(3*i+2); + #endif + + for(i=0;i0; + + // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false + #if ACC + if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) { + // bumpless transfer to Level mode + if (!f.ANGLE_MODE) { + errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; + f.ANGLE_MODE = 1; + } + } else { + // failsafe support + f.ANGLE_MODE = 0; + } + if ( rcOptions[BOXHORIZON] ) { + f.ANGLE_MODE = 0; + if (!f.HORIZON_MODE) { + errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; + f.HORIZON_MODE = 1; + } + } else { + f.HORIZON_MODE = 0; + } + #endif + + if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; + #if !defined(GPS_LED_INDICATOR) + if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;} + #endif + + #if BARO + #if (!defined(SUPPRESS_BARO_ALTHOLD)) + #if GPS + if (GPS_conf.takeover_baro) rcOptions[BOXBARO] = (rcOptions[BOXBARO] || f.GPS_BARO_MODE); + #endif + if (rcOptions[BOXBARO]) { + if (!f.BARO_MODE) { + f.BARO_MODE = 1; + AltHold = alt.EstAlt; + #if defined(ALT_HOLD_THROTTLE_MIDPOINT) + initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT; + #else + initialThrottleHold = rcCommand[THROTTLE]; + #endif + errorAltitudeI = 0; + BaroPID=0; + } + } else { + f.BARO_MODE = 0; + } + #endif + #ifdef VARIOMETER + if (rcOptions[BOXVARIO]) { + if (!f.VARIO_MODE) { + f.VARIO_MODE = 1; + } + } else { + f.VARIO_MODE = 0; + } + #endif + #endif + if (rcOptions[BOXMAG]) { + if (!f.MAG_MODE) { + f.MAG_MODE = 1; + magHold = att.heading; + } + } else { + f.MAG_MODE = 0; + } + #if defined(HEADFREE) + if (rcOptions[BOXHEADFREE]) { + if (!f.HEADFREE_MODE) { + f.HEADFREE_MODE = 1; + } + #if defined(ADVANCED_HEADFREE) + if ((f.GPS_FIX && GPS_numSat >= 5) && (GPS_distanceToHome > ADV_HEADFREE_RANGE) ) { + if (GPS_directionToHome < 180) {headFreeModeHold = GPS_directionToHome + 180;} else {headFreeModeHold = GPS_directionToHome - 180;} + } + #endif + } else { + f.HEADFREE_MODE = 0; + } + if (rcOptions[BOXHEADADJ]) { + headFreeModeHold = att.heading; // acquire new heading + } + #endif + + #if GPS + // This handles the three rcOptions boxes + // unlike other parts of the multiwii code, it looks for changes and not based on flag settings + // by this method a priority can be established between gps option + + //Generate a packed byte of all four GPS boxes. + uint8_t gps_modes_check = (rcOptions[BOXLAND]<< 3) + (rcOptions[BOXGPSHOME]<< 2) + (rcOptions[BOXGPSHOLD]<<1) + (rcOptions[BOXGPSNAV]); + + if (f.ARMED ) { //Check GPS status and armed + //TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP and sudden speed jumps + if (f.GPS_FIX) { + if (GPS_numSat >5 ) { + if (prv_gps_modes != gps_modes_check) { //Check for change since last loop + NAV_error = NAV_ERROR_NONE; + if (rcOptions[BOXGPSHOME]) { // RTH has the priotity over everything else + init_RTH(); + } else if (rcOptions[BOXGPSHOLD]) { //Position hold has priority over mission execution //But has less priority than RTH + if (f.GPS_mode == GPS_MODE_NAV) + NAV_paused_at = mission_step.number; + f.GPS_mode = GPS_MODE_HOLD; + GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS_coord[LON]); //hold at the current position + set_new_altitude(alt.EstAlt); //and current altitude + NAV_state = NAV_STATE_HOLD_INFINIT; + } else if (rcOptions[BOXLAND]) { //Land now (It has priority over Navigation) + f.GPS_mode = GPS_MODE_HOLD; + f.GPS_BARO_MODE = true; + GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS_coord[LON]); + set_new_altitude(alt.EstAlt); + NAV_state = NAV_STATE_LAND_START; + } else if (rcOptions[BOXGPSNAV]) { //Start navigation + f.GPS_mode = GPS_MODE_NAV; //Nav mode start + f.GPS_BARO_MODE = true; + GPS_prev[LAT] = GPS_coord[LAT]; + GPS_prev[LON] = GPS_coord[LON]; + if (NAV_paused_at != 0) { + next_step = NAV_paused_at; + NAV_paused_at = 0; //Clear paused step + } else { + next_step = 1; + jump_times = -10; //Reset jump counter + } + NAV_state = NAV_STATE_PROCESS_NEXT; + } else { //None of the GPS Boxes are switched on + f.GPS_mode = GPS_MODE_NONE; + f.GPS_BARO_MODE = false; + f.THROTTLE_IGNORED = false; + f.LAND_IN_PROGRESS = 0; + f.THROTTLE_IGNORED = 0; + NAV_state = NAV_STATE_NONE; + GPS_reset_nav(); + } + prv_gps_modes = gps_modes_check; + } + } else { //numSat>5 + //numSat dropped below 5 during navigation + if (f.GPS_mode == GPS_MODE_NAV) { + NAV_paused_at = mission_step.number; + f.GPS_mode = GPS_MODE_NONE; + set_new_altitude(alt.EstAlt); //and current altitude + NAV_state = NAV_STATE_NONE; + NAV_error = NAV_ERROR_SPOILED_GPS; + prv_gps_modes = 0xff; //invalidates mode check, to allow re evaluate rcOptions when numsats raised again + } + if (f.GPS_mode == GPS_MODE_HOLD || f.GPS_mode == GPS_MODE_RTH) { + f.GPS_mode = GPS_MODE_NONE; + NAV_state = NAV_STATE_NONE; + NAV_error = NAV_ERROR_SPOILED_GPS; + prv_gps_modes = 0xff; //invalidates mode check, to allow re evaluate rcOptions when numsats raised again + } + nav[0] = 0; nav[1] = 0; + } + } else { //f.GPS_FIX + // GPS Fix dissapeared, very unlikely that we will be able to regain it, abort mission + f.GPS_mode = GPS_MODE_NONE; + NAV_state = NAV_STATE_NONE; + NAV_paused_at = 0; + NAV_error = NAV_ERROR_GPS_FIX_LOST; + GPS_reset_nav(); + prv_gps_modes = 0xff; //Gives a chance to restart mission when regain fix + } + } else { //copter is armed + //copter is disarmed + f.GPS_mode = GPS_MODE_NONE; + f.GPS_BARO_MODE = false; + f.THROTTLE_IGNORED = false; + NAV_state = NAV_STATE_NONE; + NAV_paused_at = 0; + NAV_error = NAV_ERROR_DISARMED; + GPS_reset_nav(); + } + + #endif //GPS + + #if defined(FIXEDWING) || defined(HELICOPTER) + if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;} + else {f.PASSTHRU_MODE = 0;} + #endif + + } else { // not in rc loop + static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes + switch (taskOrder) { + case 0: + taskOrder++; + #if MAG + if (Mag_getADC() != 0) break; // 320 µs + #endif + case 1: + taskOrder++; + #if BARO + if (Baro_update() != 0) break; // for MS baro: I2C set and get: 220 us - presure and temperature computation 160 us + #endif + case 2: + taskOrder++; + #if BARO + if (getEstimatedAltitude() != 0) break; // 280 us + #endif + case 3: + taskOrder++; + #if GPS + if (GPS_Compute() != 0) break; // performs computation on new frame only if present + #if defined(I2C_GPS) + if (GPS_NewData() != 0) break; // 160 us with no new data / much more with new data + #endif + #endif + case 4: + taskOrder=0; + #if SONAR + Sonar_update(); //debug[2] = sonarAlt; + #endif + #ifdef LANDING_LIGHTS_DDR + auto_switch_landing_lights(); + #endif + #ifdef VARIOMETER + if (f.VARIO_MODE) vario_signaling(); + #endif + break; + } + } + + while(1) { + currentTime = micros(); + cycleTime = currentTime - previousTime; + #if defined(LOOP_TIME) + if (cycleTime >= LOOP_TIME) break; + #else + break; + #endif + } + previousTime = currentTime; + + computeIMU(); + + //*********************************** + //**** Experimental FlightModes ***** + //*********************************** + #if defined(ACROTRAINER_MODE) + if(f.ANGLE_MODE){ + if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE ) { + f.ANGLE_MODE=0; + f.HORIZON_MODE=0; + f.MAG_MODE=0; + f.BARO_MODE=0; + GPS_mode = GPS_MODE_NONE; + } + } + #endif + + //*********************************** + // THROTTLE sticks during mission and RTH + #if GPS + if (GPS_conf.ignore_throttle == 1) { + if (f.GPS_mode == GPS_MODE_NAV || f.GPS_mode == GPS_MODE_RTH) { + //rcCommand[ROLL] = 0; + //rcCommand[PITCH] = 0; + //rcCommand[YAW] = 0; + f.THROTTLE_IGNORED = 1; + } else + f.THROTTLE_IGNORED = 0; + } + + //Heading manipulation TODO: Do heading manipulation + #endif + + if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) { + int16_t dif = att.heading - magHold; + if (dif <= - 180) dif += 360; + if (dif >= + 180) dif -= 360; + if (f.SMALL_ANGLES_25 || (f.GPS_mode != 0)) rcCommand[YAW] -= dif*conf.pid[PIDMAG].P8 >> 5; //Always correct maghold in GPS mode + } else magHold = att.heading; + + #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) + /* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from alexmos). It's slowly increase/decrease + * altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second with cycle time about 3-4ms) + */ + if (f.BARO_MODE) { + static uint8_t isAltHoldChanged = 0; + static int16_t AltHoldCorr = 0; + + #if GPS + if (f.LAND_IN_PROGRESS) { //If autoland is in progress then take over and decrease alt slowly + AltHoldCorr -= GPS_conf.land_speed; + if(abs(AltHoldCorr) > 512) { + AltHold += AltHoldCorr/512; + AltHoldCorr %= 512; + } + } + #endif + //IF Throttle not ignored then allow change altitude with the stick.... + if ( (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) && !f.THROTTLE_IGNORED) { + // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) + AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold; + if(abs(AltHoldCorr) > 512) { + AltHold += AltHoldCorr/512; + AltHoldCorr %= 512; + } + isAltHoldChanged = 1; + } else if (isAltHoldChanged) { + AltHold = alt.EstAlt; + isAltHoldChanged = 0; + } + rcCommand[THROTTLE] = initialThrottleHold + BaroPID; + } + #endif //BARO + + + + #if defined(THROTTLE_ANGLE_CORRECTION) + if(f.ANGLE_MODE || f.HORIZON_MODE) { + rcCommand[THROTTLE]+= throttleAngleCorrection; + } + #endif + + #if GPS + //TODO: split cos_yaw calculations into two phases (X and Y) + if (( f.GPS_mode != GPS_MODE_NONE ) && f.GPS_FIX_HOME ) { + float sin_yaw_y = sin(att.heading*0.0174532925f); + float cos_yaw_x = cos(att.heading*0.0174532925f); + GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; + GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; + } else { + GPS_angle[ROLL] = 0; + GPS_angle[PITCH] = 0; + } + + //Used to communicate back nav angles to the GPS simulator (for HIL testing) + #if defined(GPS_SIMULATOR) + SerialWrite(2,0xa5); + SerialWrite16(2,nav[LAT]+rcCommand[PITCH]); + SerialWrite16(2,nav[LON]+rcCommand[ROLL]); + SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check + #endif + + #endif //GPS + + //**** PITCH & ROLL & YAW PID **** + #if PID_CONTROLLER == 1 // evolved oldschool + if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512); + + // PITCH & ROLL + for(axis=0;axis<2;axis++) { + rc = rcCommand[axis]<<1; + error = rc - imu.gyroData[axis]; + errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here + if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0; + + ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 + + PTerm = mul(rc,conf.pid[axis].P8)>>6; + + if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC + // 50 degrees max inclination + errorAngle = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here + errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here + + PTermACC = mul(errorAngle,conf.pid[PIDLEVEL].P8)>>7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result + + int16_t limit = conf.pid[PIDLEVEL].D8*5; + PTermACC = constrain(PTermACC,-limit,+limit); + + ITermACC = mul(errorAngleI[axis],conf.pid[PIDLEVEL].I8)>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result + + ITerm = ITermACC + ((ITerm-ITermACC)*prop>>9); + PTerm = PTermACC + ((PTerm-PTermACC)*prop>>9); + } + + PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation + + delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastGyro[axis] = imu.gyroData[axis]; + DTerm = delta1[axis]+delta2[axis]+delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + + DTerm = mul(DTerm,dynD8[axis])>>5; // 32 bits is needed for calculation + + axisPID[axis] = PTerm + ITerm - DTerm; + } + + //YAW + #define GYRO_P_MAX 300 + #define GYRO_I_MAX 250 + + rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30)) >> 5; + + error = rc - imu.gyroData[YAW]; + errorGyroI_YAW += mul(error,conf.pid[YAW].I8); + errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28)); + if (abs(rc) > 50) errorGyroI_YAW = 0; + + PTerm = mul(error,conf.pid[YAW].P8)>>6; + #ifndef COPTER_WITH_SERVO + int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8; + PTerm = constrain(PTerm,-limit,+limit); + #endif + + ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX); + + axisPID[YAW] = PTerm + ITerm; + + #elif PID_CONTROLLER == 2 // alexK + #define GYRO_I_MAX 256 + #define ACC_I_MAX 256 + prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500] + + //----------PID controller---------- + for(axis=0;axis<3;axis++) { + //-----Get the desired angle rate depending on flight mode + if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC + // calculate error and limit the angle to 50 degrees max inclination + errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here + } + if (axis == 2) {//YAW is always gyro-controlled (MAG correction is applied to rcCommand) + AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5); + } else { + if (!f.ANGLE_MODE) {//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID + AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4; + if (f.HORIZON_MODE) { + //mix up angle error to desired AngleRateTmp to add a little auto-level feel + AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8)>>8; + } + } else {//it's the ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8)>>4; + } + } + + //--------low-level gyro-based PID. ---------- + //Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + //-----calculate scaled error.AngleRates + //multiplication of rcCommand corresponds to changing the sticks scaling here + RateError = AngleRateTmp - imu.gyroData[axis]; + + //-----calculate P component + PTerm = ((int32_t) RateError * conf.pid[axis].P8)>>7; + + //-----calculate I component + //there should be no division before accumulating the error to integrator, because the precision would be reduced. + //Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + //Time correction (to avoid different I scaling for different builds based on average cycle time) + //is normalized to cycle time = 2048. + errorGyroI[axis] += (((int32_t) RateError * cycleTime)>>11) * conf.pid[axis].I8; + //limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + //I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -GYRO_I_MAX<<13, (int32_t) +GYRO_I_MAX<<13); + ITerm = errorGyroI[axis]>>13; + + //-----calculate D-term + delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastError[axis] = RateError; + + //Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime>>4)))>>6; + //add moving average here to reduce noise + deltaSum = delta1[axis]+delta2[axis]+delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + + //DTerm = (deltaSum*conf.pid[axis].D8)>>8; + //Solve overflow in calculation above... + DTerm = ((int32_t)deltaSum*conf.pid[axis].D8)>>8; + //-----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + #else + #error "*** you must set PID_CONTROLLER to one existing implementation" + #endif + mixTable(); + // do not update servos during unarmed calibration of sensors which are sensitive to vibration + #if defined(DISABLE_SERVOS_WHEN_UNARMED) + if (f.ARMED) writeServos(); + #else + if ( (f.ARMED) || ((!calibratingG) && (!calibratingA)) ) writeServos(); + #endif + writeMotors(); +} diff --git a/MultiWii.h b/MultiWii.h old mode 100644 new mode 100755 index 27d68f45..8101671d --- a/MultiWii.h +++ b/MultiWii.h @@ -1,7 +1,7 @@ #ifndef MULTIWII_H_ #define MULTIWII_H_ -#define VERSION 241 +#define VERSION 231 #define NAVI_VERSION 7 //This allow sync with GUI #include "types.h" #include "Alarms.h" diff --git a/MultiWii.ino b/MultiWii.ino index 6f41579b..d17e48bf 100644 --- a/MultiWii.ino +++ b/MultiWii.ino @@ -1,4 +1,6 @@ -/* + +/* + * * Welcome to MultiWii. * * If you see this message, chances are you are using the Arduino IDE. That is ok. @@ -10,4 +12,3 @@ * Have fun, and do not forget MultiWii is made possible and brought to you under the GPL License. * */ - diff --git a/MultiWiiConf/MultiWiiConf.pde b/MultiWiiConf/MultiWiiConf.pde new file mode 100644 index 00000000..3fdf2c46 --- /dev/null +++ b/MultiWiiConf/MultiWiiConf.pde @@ -0,0 +1,3128 @@ +import processing.serial.Serial; // serial library +import controlP5.*; // controlP5 library +import processing.opengl.*; +import java.lang.StringBuffer; // for efficient String concatemation +import javax.swing.SwingUtilities; // required for swing and EDT +import javax.swing.JFileChooser; // Saving dialogue +import javax.swing.filechooser.FileFilter; // for our configuration file filter "*.mwi" +import javax.swing.JOptionPane; // for message dialogue + +//Added For Processing 2.0.x compabillity +import java.util.*; +import java.io.*; +//**************************** + +// TODO add new msp : pid description with bound and scale + +PrintWriter output; +BufferedReader reader; +String portnameFile="SerialPort.txt"; // Name of file for Autoconnect. +int GUI_BaudRate = 115200; // Default. +int SerialPort; +Serial g_serial; +ControlP5 controlP5; +Textlabel txtlblWhichcom,TxtInfoMotors1; +Textlabel txtlblRates,txtlblRev,TxtLeftW,TxtRightW,TxtRevW,TxtRevR,TxtRates,TxtRev, + TxtMids,TxtMin,TxtMax,TxtSLeft,TxtSNick,TxtSRight,TxtInfo,TxtInfo1,TxtInfo2, + TxtInfo3,TxtInfo4,TxtAux,Links; + +ListBox commListbox,baudListbox; + +static int CHECKBOXITEMS=0; +static int PIDITEMS=10; +int commListMax; +int tabHeight=20; // Extra height needed for Tabs +int Centerlimits[] = {1200,1800}; // Endpoints of ServoCenterSliders + +cGraph g_graph; +int windowsX = 1000; int windowsY = 550+tabHeight; +int xGraph = 10; int yGraph = 325+tabHeight; +int xObj = 520; int yObj = 293+tabHeight; +int xCompass = 920; int yCompass = 341+tabHeight; +int xLevelObj = 920; int yLevelObj = 80+tabHeight; +int xParam = 120; int yParam = 5+tabHeight; +int xRC = 690; int yRC = 10+tabHeight; +int xMot = 690; int yMot = 155+tabHeight; +int xButton = 845; int yButton = 231+tabHeight; +int xBox = 415; int yBox = 10+tabHeight; +int xGPS = 853; int yGPS = 438+tabHeight; +int xServ = 350; int yServ = 20+tabHeight; + +int i, j; // enummerators +int byteRC_RATE,byteRC_EXPO, byteRollPitchRate,byteYawRate, + byteDynThrPID,byteThrottle_EXPO, byteThrottle_MID, byteSelectSetting, + cycleTime, i2cError, + version, versionMisMatch,horizonInstrSize, + GPS_distanceToHome, GPS_directionToHome, + GPS_numSat, GPS_fix, GPS_update, GPS_altitude, GPS_speed, + GPS_latitude, GPS_longitude, + init_com, graph_on, pMeterSum, intPowerTrigger, bytevbat, amperage, rssi ; + +int multiCapability = 0; // Bitflags stating what capabilities are/are not present in the compiled code. +int byteMP[] = new int[8]; // Motor Pins. Varies by multiType and Arduino model (pro Mini, Mega, etc). +int MConf[] = new int[10]; // Min/Maxthro etc +int byteP[] = new int[PIDITEMS], byteI[] = new int[PIDITEMS], byteD[] = new int[PIDITEMS]; +int activation[]; +int ServoMID[] = new int[8]; // Plane,ppm/pwm conv,heli +int servoRATE[] = new int[8]; +int servoDirection[] = new int[8]; +int ServoMIN[] = new int[8]; +int ServoMAX[] = new int[8]; +int wingDir[] = new int[8]; // Flying wing +int wingPos[] = new int[8]; +int In[] = new int[8]; + +Textlabel TxtMIX[] = new Textlabel[6]; + +int multiType; // 1 for tricopter, 2 for quad+, 3 for quadX, ... +// Alias for multiTypes +int TRI =1; +int QUADP =2; +int QUADX =3; +int BI =4; +int GIMBAL =5; +int Y6 =6; +int HEX6 =7; +int FLYING_WING =8; +int Y4 =9; +int HEX6X =10; +int OCTOX8 =11; +int OCTOFLATX =12; +int OCTOFLATP =13; +int AIRPLANE =14; +int HELI_120_CCPM =15; +int HELI_90_DEG =16; +int VTAIL4 =17; +int HEX6H =18; +int PPM_TO_SERVO =19; +int DUALCOPTER =20; +int SINGLECOPTER =21; + + +float gx, gy, gz, ax, ay, az, magx, magy, magz, alt, head, angx, angy, + debug1, debug2, debug3, debug4, + angyLevelControl, angCalc, + pVersion; + +float mot[] = new float[8], + servo[] = new float[8], + RCChan[] = new float[16]; + +private static final int ROLL = 0, PITCH = 1, YAW = 2, ALT = 3, VEL = 4, LEVEL = 5, MAG = 6; + +boolean axGraph =true,ayGraph=true,azGraph=true,gxGraph=true,gyGraph=true,gzGraph=true,altGraph=true,headGraph=true, magxGraph =true,magyGraph=true,magzGraph=true, + debug1Graph = false,debug2Graph = false,debug3Graph = false,debug4Graph = false,hideDraw=false,GraphicsInited=false,gimbalConfig=false,flapperons=false, + flaps=false,InitServos=true; + +boolean toggleServo=false,toggleWriteServo=false,toggleWing=false,toggleWriteWing=false,toggleLive=false,toggleWriteServoLive=false,toggleWriteWingLive=false, + toggleSaveHeli=false,toggleWaitHeli=false,toggleGimbal=false, + graphEnabled = false,Mag_=false,gimbal=false, servoStretch=false,camTrigger=false,ExportServo=false, + toggleTrigger=false,ServosActive=false; + +static int RCThro = 3, RCRoll = 0, RCPitch =1, RCYaw =2, RCAUX1=4, RCAUX2=5, RCAUX3=6, RCAUX4=7; + +cDataArray accPITCH = new cDataArray(200), accROLL = new cDataArray(200), accYAW = new cDataArray(200), + gyroPITCH = new cDataArray(200), gyroROLL = new cDataArray(200), gyroYAW = new cDataArray(200), + magxData = new cDataArray(200), magyData = new cDataArray(200), magzData = new cDataArray(200), + altData = new cDataArray(200), headData = new cDataArray(200), + debug1Data = new cDataArray(200), debug2Data = new cDataArray(200), debug3Data = new cDataArray(200), + debug4Data = new cDataArray(200); + + +Numberbox confP[] = new Numberbox[PIDITEMS], + confI[] = new Numberbox[PIDITEMS], + confD[] = new Numberbox[PIDITEMS], + confINF[] = new Numberbox[7], + VBat[] = new Numberbox[6] + ; + +Numberbox confRC_RATE, confRC_EXPO, rollPitchRate, yawRate, dynamic_THR_PID, throttle_EXPO, throttle_MID, + confPowerTrigger, confSetting, confSelectSetting; + + +Slider axSlider, aySlider, azSlider, gxSlider, gySlider, gzSlider, magxSlider, magySlider, + magzSlider, altSlider, headSlider, debug1Slider, debug2Slider, debug3Slider, debug4Slider,scaleSlider,motorControlSlider; + +Slider servoSliderH[] = new Slider[8], + servoSliderV[] = new Slider[8], + motSlider[] = new Slider[8], + TX_StickSlider[] = new Slider[8], + GimbalSlider[] = new Slider[12], + ServoSliderC[] = new Slider[8], + ServoSliderMAX[] = new Slider[8], + ServoSliderMIN[] = new Slider[8]; + + +Button buttonIMPORT, buttonSAVE, buttonREAD, buttonRESET, buttonWRITE, buttonCALIBRATE_ACC, buttonCALIBRATE_MAG, buttonSTART, buttonSTOP, buttonSETTING, + buttonAcc, buttonBaro, buttonMag, buttonGPS, buttonSonar, buttonOptic, buttonRXbind, btnQConnect,buttonExport, + btMagDecl,btMWiiHome,btDownloads; + +Button SaveSERVO,buttonSERVO,buttonWing,SaveWing,buttonLIVE,buttonCCPM,buttonGimbal,btnTrigger; + +Toggle tACC_ROLL, tACC_PITCH, tACC_Z, tGYRO_ROLL, tGYRO_PITCH, tGYRO_YAW, tBARO,tHEAD, tMAGX, tMAGY, tMAGZ, + tDEBUG1, tDEBUG2, tDEBUG3, tDEBUG4; + +// Motors Toggles +Toggle motToggle[] = new Toggle[8]; + +// Colors +color yellow_ = color(200, 200, 20), green_ = color(30, 120, 30), red_ = color(120, 30, 30), blue_ = color(50, 50, 100), + grey_ = color(30, 30, 30),black_ = color(0, 0, 0),orange_ =color(200,128,0); + +PFont font8, font9, font12, font15; + +CheckBox checkbox[], checkboxRev[]; +CheckBox Bbox, Wbox, Mbox; +Button buttonCheckbox[], BtServo[], BtAUX[]; +Numberbox RateSlider[]; + +// TODO New part +void create_GimbalGraphics(){ + if(gimbal){ + gimbalConfig = true; + int sMin=1020;int sMax=2000; + if(servoStretch) {sMin=500; sMax=2500;} + int Step=yServ-10; + GimbalSlider[0] = controlP5.addSlider("Tilt_Min" ,sMin,1500,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[1] = controlP5.addSlider("Tilt_Max" ,1500,sMax,0,xServ+150 ,Step+80,60,10).setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[2] = controlP5.addSlider("Channel " ,1200,1700,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[3] = controlP5.addSlider("Tilt_Prop",-125,125,0,xServ+100,Step+100, 60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + Step+=90; + GimbalSlider[4] = controlP5.addSlider("Roll_Min" ,sMin,1500,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[5] = controlP5.addSlider("Roll_Max" ,1500,sMax,0,xServ+150 ,Step+80,60,10).setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[6] = controlP5.addSlider("Channel" ,1200,1700,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[7] = controlP5.addSlider("Roll_Prop",-125,125,0,xServ+100,Step+100, 60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + + GimbalSlider[8] = controlP5.addSlider("Trig_LO" ,500,2000,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[9] = controlP5.addSlider("Trig_HI" ,1000,sMax,0,xServ+150 ,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[10] = controlP5.addSlider("Trigger" ,0,30000,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[11] = controlP5.addSlider("Trig_Rev",-0,1,0,xServ+100,Step+100, 40,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings") .setNumberOfTickMarks(2).setColorTickMark(0); + + buttonGimbal.show(); + if(camTrigger)btnTrigger.show(); + controlP5.getTab("ServoSettings").show(); + } +} + +void create_ServoGraphics(){ + BtServo = new Button[8]; + BtAUX = new Button[5]; + checkboxRev = new CheckBox[8]; + RateSlider = new Numberbox[8]; + GraphicsInited=true; + Bbox = controlP5.addCheckBox("Bbox").setPosition(xServ+40,yServ+40).setColorForeground(color(120)) + .setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10).setColorBackground(color(140)) + .setItemsPerRow(1).setSpacingColumn(30).setSpacingRow(10) + .addItem("0", 254).addItem("1", 254).addItem("2", 254) + .addItem("3", 254).addItem("4", 254).addItem("5", 254) + .addItem("6", 254).addItem("7", 254) + .hide().hideLabels().moveTo("ServoSettings") + ; + + //Create Common ServoSliders + int Step =0; + int sMin=1020;int sMax=2000; + if(servoStretch) {sMin=500; sMax=2500;} + for (i=0;i<8;i++) { + ServoSliderC[i] = controlP5.addSlider("Servo "+i,Centerlimits[0],Centerlimits[1],0,xServ+180,yServ+40+Step,110,12).setDecimalPrecision(0).hide().setLabel("").moveTo("ServoSettings"); + ServoSliderMIN[i] = controlP5.addSlider("MIN "+i,sMin,1500,0,xServ+80,yServ+40+Step,40,12).setDecimalPrecision(0).setLabel("").hide().moveTo("ServoSettings"); + ServoSliderMAX[i] = controlP5.addSlider("MAX "+i,1500,sMax,0,xServ+125,yServ+40+Step,40,12).setDecimalPrecision(0).setLabel("").hide().moveTo("ServoSettings"); + Step+=20; } + + // ServoGraphics For AirPlane SC & DC +if(multiType == PPM_TO_SERVO || multiType == AIRPLANE ){ + controlP5.getTab("ServoSettings").show(); + TxtRates = controlP5.addTextlabel("label","Servo Rates in %").setPosition(xServ+75,yServ+20).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset for servos").setPosition(xServ+190,yServ+20).hide().moveTo("ServoSettings"); + TxtAux = controlP5.addTextlabel("Alabel","Channel for Flaps").setPosition(xServ-120,yServ+20).hide().moveTo("ServoSettings"); + TxtRev = controlP5.addTextlabel("txtlblRev","Norm/Rev").setPosition(xServ+10,yServ+20).hide().moveTo("ServoSettings"); + +//****************** End of AirPlane ******************** +} + +if(multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER){ //ServoGraphics For FlyingWng & TRI & BI || multiType == DUALCOPTER +controlP5.getTab("ServoSettings").show(); +int LabelPos=100; + +if(multiType == FLYING_WING){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("L Roll", 254).addItem("R Roll", 254).addItem("L NICK", 254).addItem("R NICK", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Left Wing" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Right Wing").setPosition(xServ+110,yServ+130).hide().moveTo("ServoSettings"); + } + if(multiType == TRI){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(1).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("YAW", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Yaw Servo" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel"," ").setPosition(xServ+110,yServ+140).hide().moveTo("ServoSettings"); + } + if(multiType == BI){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("L Yaw", 254).addItem("R Yaw", 254).addItem("L NICK", 254).addItem("R NICK", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Left Servo" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Right Servo").setPosition(xServ+110,yServ+140).hide().moveTo("ServoSettings"); + } + if(multiType == DUALCOPTER){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(70).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("Pitch ", 254).addItem("Roll ", 254).moveTo("ServoSettings").hide(); + TxtLeftW = controlP5.addTextlabel("Label","Roll" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Nick").setPosition(xServ+110,yServ+130).hide().moveTo("ServoSettings"); + } + if(multiType == SINGLECOPTER){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-80 ,yServ+100) + .addItem(" Right ", 254).addItem("R yaw", 254) + .addItem(" Left ", 254) .addItem("L yaw", 254) + .addItem(" Front", 254) .addItem("F yaw", 254) + .addItem(" Rear", 254) .addItem(" yaw", 254) + .moveTo("ServoSettings") ; +LabelPos=60; + TxtMin = controlP5.addTextlabel("Minlabel","MIN").setPosition(xServ+85,yServ+80) .hide().moveTo("ServoSettings"); + TxtMax = controlP5.addTextlabel("Maxlabel","MAX").setPosition(xServ+130,yServ+80).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset servos").setPosition(xServ+190,yServ+80).hide().moveTo("ServoSettings"); + } + + TxtRevW = controlP5.addTextlabel("Revlabel","Change Gyro/Acc Direction" ).setPosition(xServ-165,yServ+30).hide().moveTo("ServoSettings"); + TxtRevR = controlP5.addTextlabel("Revtx","Change Dir in TX To Match" ).setPosition(xServ-170,yServ+LabelPos).hide().moveTo("ServoSettings"); + +//****************** End of FlyingWng & TRI ******************** +} + +// ServoGraphics For Heli 120 && 90 +if(multiType == HELI_120_CCPM || multiType == HELI_90_DEG ){ +controlP5.getTab("ServoSettings").show(); + TxtMin = controlP5.addTextlabel("Minlabel","MIN").setPosition(xServ+85,yServ+20) .hide().moveTo("ServoSettings"); + TxtMax = controlP5.addTextlabel("Maxlabel","MAX").setPosition(xServ+130,yServ+20).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset servos").setPosition(xServ+190,yServ+20).hide().moveTo("ServoSettings"); + TxtRates = controlP5.addTextlabel("label","Not Used").setPosition(xServ+75,yServ+20).hide().moveTo("ServoSettings"); + TxtRev = controlP5.addTextlabel("txtlblRev","Servos").setPosition(xServ+10,yServ+20).hide().moveTo("ServoSettings"); + +// CCPM settings + // Mixer Boxes + int XPos=xServ+40; + int YPos=yServ-0; + Mbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(3).setSpacingColumn(15).setSpacingRow(10).setPosition(XPos-180 ,YPos+50) + .addItem("mix3_4", 254) .addItem("mix3_2", 254).addItem("mix3_1", 254) + .addItem("mix4_4", 254) .addItem("mix4_2", 254).addItem("mix4_1", 254) + .addItem("mix6_4", 254) .addItem("mix6_2", 254).addItem("mix6_1", 254) + .moveTo("ServoSettings").hideLabels().hide(); + TxtMIX[0] = controlP5.addTextlabel("mix1","NICK") .setPosition(XPos-220,YPos+50).moveTo("ServoSettings"); + TxtMIX[1] = controlP5.addTextlabel("mix2","LEFT") .setPosition(XPos-220,YPos+70).moveTo("ServoSettings"); + TxtMIX[2] = controlP5.addTextlabel("mix3","RIGHT").setPosition(XPos-220,YPos+90).moveTo("ServoSettings"); + TxtMIX[3] = controlP5.addTextlabel("mix4","COLL") .setPosition(XPos-195,YPos+30).moveTo("ServoSettings"); + TxtMIX[4] = controlP5.addTextlabel("mix5","NICK") .setPosition(XPos-155,YPos+30).moveTo("ServoSettings"); + TxtMIX[5] = controlP5.addTextlabel("mix6","ROLL") .setPosition(XPos-115,YPos+30).moveTo("ServoSettings"); + for (i=0;i<6;i++) { TxtMIX[i].hide();} + + //******************End of Heli******************** +} +// Common Graphics for servos + Step =0; + for (i=0;i<8;i++) { // TODO Something + BtServo[i] = controlP5.addButton("CHb"+i,1,xServ-30,yServ+40+20*i,60,12).setColorBackground(green_).setLabel("Servo "+i).hide().moveTo("ServoSettings"); + + checkboxRev[i] = controlP5.addCheckBox("cbR"+i).moveTo("ServoSettings"); + checkboxRev[i].setPosition(xServ+70,yServ+40+20*i) + .setColorActive(color(255)).setColorBackground(color(120)) + .setItemHeight(10).setItemWidth(20).hide() + .moveTo("ServoSettings") + ; + + RateSlider[i] = (controlP5.Numberbox) hideLabel(controlP5.addNumberbox("Rate"+i,0,xServ+70,yServ+40+i*20,100,14)); + RateSlider[i].setColorBackground(yellow_);RateSlider[i].setDirection(Controller.HORIZONTAL); + RateSlider[i].setDecimalPrecision(0);RateSlider[i].setMultiplier(1);RateSlider[i].setMin(0).setMax(125).hide().moveTo("ServoSettings"); + } + + for (i=0;i<4;i++) BtAUX[i] = controlP5.addButton("Cau"+i,1,xServ-100,yServ+40+20*i,60,12).setColorBackground(red_).setLabel(" AUX "+(i+1)).moveTo("ServoSettings").hide(); + BtAUX[4] = controlP5.addButton("Cau4" ,1,xServ-100,yServ+120,60,12).setColorBackground(blue_).setLabel("Disable").moveTo("ServoSettings").hide(); + + //************************ End of servoGrapics********************** +} + +void create_checkboxes(String[] names) { + /* destroy old buttons */ + for (int i=0; imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && i + + + + CFBundleName + MultiWiiConf + CFBundleVersion + 1.0 + CFBundleAllowMixedLocalizations + true + CFBundleExecutable + JavaApplicationStub + CFBundleDevelopmentRegion + English + CFBundlePackageType + APPL + CFBundleSignature + ???? + CFBundleInfoDictionaryVersion + 6.0 + CFBundleIconFile + sketch.icns + CFBundleIdentifier + MultiWiiConf + + + LSUIPresentationMode + 0 + + + LSArchitecturePriority + + i386 + ppc + + + Java + + VMOptions + + MainClass + MultiWiiConf + JVMVersion + 1.5* + JVMArchs + + i386 + ppc + + ClassPath + $JAVAROOT/MultiWiiConf.jar:$JAVAROOT/core.jar:$JAVAROOT/RXTXcomm.jar:$JAVAROOT/serial.jar:$JAVAROOT/controlP5.jar:$JAVAROOT/controlP5_ori.jar:$JAVAROOT/gluegen-rt.jar:$JAVAROOT/jogl.jar:$JAVAROOT/opengl.jar + + + Properties + + apple.laf.useScreenMenuBar + true + apple.awt.showGrowBox + false + com.apple.smallTabs + true + apple.awt.Antialiasing + false + apple.awt.TextAntialiasing + true + com.apple.hwaccel + true + apple.awt.use-file-dialog-packages + false + + + + diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub new file mode 100644 index 00000000..9ae8d555 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub64 b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub64 new file mode 100644 index 00000000..a8fbda5e Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/MacOS/JavaApplicationStub64 differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/PkgInfo b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/PkgInfo new file mode 100644 index 00000000..bd04210f --- /dev/null +++ b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/PkgInfo @@ -0,0 +1 @@ +APPL???? \ No newline at end of file diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/MultiWiiConf.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/MultiWiiConf.jar new file mode 100644 index 00000000..3565960f Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/MultiWiiConf.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/RXTXcomm.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/RXTXcomm.jar new file mode 100644 index 00000000..afb4b164 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/RXTXcomm.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5.jar new file mode 100644 index 00000000..80e187a5 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5_ori.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5_ori.jar new file mode 100644 index 00000000..34c0b364 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/controlP5_ori.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/core.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/core.jar new file mode 100644 index 00000000..0b019619 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/core.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/gluegen-rt.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/gluegen-rt.jar new file mode 100644 index 00000000..7ac10a35 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/gluegen-rt.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/jogl.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/jogl.jar new file mode 100644 index 00000000..305e9987 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/jogl.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libgluegen-rt.jnilib b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libgluegen-rt.jnilib new file mode 100644 index 00000000..19562801 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libgluegen-rt.jnilib differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl.jnilib b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl.jnilib new file mode 100644 index 00000000..eb8719aa Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl.jnilib differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_awt.jnilib b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_awt.jnilib new file mode 100644 index 00000000..2f16fbfb Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_awt.jnilib differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_cg.jnilib b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_cg.jnilib new file mode 100644 index 00000000..56271210 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/libjogl_cg.jnilib differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/librxtxSerial.jnilib b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/librxtxSerial.jnilib new file mode 100644 index 00000000..6dfbdd61 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/librxtxSerial.jnilib differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/opengl.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/opengl.jar new file mode 100644 index 00000000..6f839252 Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/opengl.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/serial.jar b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/serial.jar new file mode 100644 index 00000000..3fdeab3e Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/Java/serial.jar differ diff --git a/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/sketch.icns b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/sketch.icns new file mode 100644 index 00000000..6dc82bcf Binary files /dev/null and b/MultiWiiConf/application.macosx/MultiWiiConf.app/Contents/Resources/sketch.icns differ diff --git a/MultiWiiConf/application.macosx/readme.txt b/MultiWiiConf/application.macosx/readme.txt new file mode 100644 index 00000000..6ac80793 --- /dev/null +++ b/MultiWiiConf/application.macosx/readme.txt @@ -0,0 +1,8 @@ +This application was created on Windows, which does not +properly support setting files as "executable", +a necessity for applications on Mac OS X. + +To fix this, use the Terminal on Mac OS X, and from this +directory, type the following: + +chmod +x MultiWiiConf.app/Contents/MacOS/JavaApplicationStub diff --git a/MultiWiiConf/application.macosx/source/MultiWiiConf.pde b/MultiWiiConf/application.macosx/source/MultiWiiConf.pde new file mode 100644 index 00000000..3fdf2c46 --- /dev/null +++ b/MultiWiiConf/application.macosx/source/MultiWiiConf.pde @@ -0,0 +1,3128 @@ +import processing.serial.Serial; // serial library +import controlP5.*; // controlP5 library +import processing.opengl.*; +import java.lang.StringBuffer; // for efficient String concatemation +import javax.swing.SwingUtilities; // required for swing and EDT +import javax.swing.JFileChooser; // Saving dialogue +import javax.swing.filechooser.FileFilter; // for our configuration file filter "*.mwi" +import javax.swing.JOptionPane; // for message dialogue + +//Added For Processing 2.0.x compabillity +import java.util.*; +import java.io.*; +//**************************** + +// TODO add new msp : pid description with bound and scale + +PrintWriter output; +BufferedReader reader; +String portnameFile="SerialPort.txt"; // Name of file for Autoconnect. +int GUI_BaudRate = 115200; // Default. +int SerialPort; +Serial g_serial; +ControlP5 controlP5; +Textlabel txtlblWhichcom,TxtInfoMotors1; +Textlabel txtlblRates,txtlblRev,TxtLeftW,TxtRightW,TxtRevW,TxtRevR,TxtRates,TxtRev, + TxtMids,TxtMin,TxtMax,TxtSLeft,TxtSNick,TxtSRight,TxtInfo,TxtInfo1,TxtInfo2, + TxtInfo3,TxtInfo4,TxtAux,Links; + +ListBox commListbox,baudListbox; + +static int CHECKBOXITEMS=0; +static int PIDITEMS=10; +int commListMax; +int tabHeight=20; // Extra height needed for Tabs +int Centerlimits[] = {1200,1800}; // Endpoints of ServoCenterSliders + +cGraph g_graph; +int windowsX = 1000; int windowsY = 550+tabHeight; +int xGraph = 10; int yGraph = 325+tabHeight; +int xObj = 520; int yObj = 293+tabHeight; +int xCompass = 920; int yCompass = 341+tabHeight; +int xLevelObj = 920; int yLevelObj = 80+tabHeight; +int xParam = 120; int yParam = 5+tabHeight; +int xRC = 690; int yRC = 10+tabHeight; +int xMot = 690; int yMot = 155+tabHeight; +int xButton = 845; int yButton = 231+tabHeight; +int xBox = 415; int yBox = 10+tabHeight; +int xGPS = 853; int yGPS = 438+tabHeight; +int xServ = 350; int yServ = 20+tabHeight; + +int i, j; // enummerators +int byteRC_RATE,byteRC_EXPO, byteRollPitchRate,byteYawRate, + byteDynThrPID,byteThrottle_EXPO, byteThrottle_MID, byteSelectSetting, + cycleTime, i2cError, + version, versionMisMatch,horizonInstrSize, + GPS_distanceToHome, GPS_directionToHome, + GPS_numSat, GPS_fix, GPS_update, GPS_altitude, GPS_speed, + GPS_latitude, GPS_longitude, + init_com, graph_on, pMeterSum, intPowerTrigger, bytevbat, amperage, rssi ; + +int multiCapability = 0; // Bitflags stating what capabilities are/are not present in the compiled code. +int byteMP[] = new int[8]; // Motor Pins. Varies by multiType and Arduino model (pro Mini, Mega, etc). +int MConf[] = new int[10]; // Min/Maxthro etc +int byteP[] = new int[PIDITEMS], byteI[] = new int[PIDITEMS], byteD[] = new int[PIDITEMS]; +int activation[]; +int ServoMID[] = new int[8]; // Plane,ppm/pwm conv,heli +int servoRATE[] = new int[8]; +int servoDirection[] = new int[8]; +int ServoMIN[] = new int[8]; +int ServoMAX[] = new int[8]; +int wingDir[] = new int[8]; // Flying wing +int wingPos[] = new int[8]; +int In[] = new int[8]; + +Textlabel TxtMIX[] = new Textlabel[6]; + +int multiType; // 1 for tricopter, 2 for quad+, 3 for quadX, ... +// Alias for multiTypes +int TRI =1; +int QUADP =2; +int QUADX =3; +int BI =4; +int GIMBAL =5; +int Y6 =6; +int HEX6 =7; +int FLYING_WING =8; +int Y4 =9; +int HEX6X =10; +int OCTOX8 =11; +int OCTOFLATX =12; +int OCTOFLATP =13; +int AIRPLANE =14; +int HELI_120_CCPM =15; +int HELI_90_DEG =16; +int VTAIL4 =17; +int HEX6H =18; +int PPM_TO_SERVO =19; +int DUALCOPTER =20; +int SINGLECOPTER =21; + + +float gx, gy, gz, ax, ay, az, magx, magy, magz, alt, head, angx, angy, + debug1, debug2, debug3, debug4, + angyLevelControl, angCalc, + pVersion; + +float mot[] = new float[8], + servo[] = new float[8], + RCChan[] = new float[16]; + +private static final int ROLL = 0, PITCH = 1, YAW = 2, ALT = 3, VEL = 4, LEVEL = 5, MAG = 6; + +boolean axGraph =true,ayGraph=true,azGraph=true,gxGraph=true,gyGraph=true,gzGraph=true,altGraph=true,headGraph=true, magxGraph =true,magyGraph=true,magzGraph=true, + debug1Graph = false,debug2Graph = false,debug3Graph = false,debug4Graph = false,hideDraw=false,GraphicsInited=false,gimbalConfig=false,flapperons=false, + flaps=false,InitServos=true; + +boolean toggleServo=false,toggleWriteServo=false,toggleWing=false,toggleWriteWing=false,toggleLive=false,toggleWriteServoLive=false,toggleWriteWingLive=false, + toggleSaveHeli=false,toggleWaitHeli=false,toggleGimbal=false, + graphEnabled = false,Mag_=false,gimbal=false, servoStretch=false,camTrigger=false,ExportServo=false, + toggleTrigger=false,ServosActive=false; + +static int RCThro = 3, RCRoll = 0, RCPitch =1, RCYaw =2, RCAUX1=4, RCAUX2=5, RCAUX3=6, RCAUX4=7; + +cDataArray accPITCH = new cDataArray(200), accROLL = new cDataArray(200), accYAW = new cDataArray(200), + gyroPITCH = new cDataArray(200), gyroROLL = new cDataArray(200), gyroYAW = new cDataArray(200), + magxData = new cDataArray(200), magyData = new cDataArray(200), magzData = new cDataArray(200), + altData = new cDataArray(200), headData = new cDataArray(200), + debug1Data = new cDataArray(200), debug2Data = new cDataArray(200), debug3Data = new cDataArray(200), + debug4Data = new cDataArray(200); + + +Numberbox confP[] = new Numberbox[PIDITEMS], + confI[] = new Numberbox[PIDITEMS], + confD[] = new Numberbox[PIDITEMS], + confINF[] = new Numberbox[7], + VBat[] = new Numberbox[6] + ; + +Numberbox confRC_RATE, confRC_EXPO, rollPitchRate, yawRate, dynamic_THR_PID, throttle_EXPO, throttle_MID, + confPowerTrigger, confSetting, confSelectSetting; + + +Slider axSlider, aySlider, azSlider, gxSlider, gySlider, gzSlider, magxSlider, magySlider, + magzSlider, altSlider, headSlider, debug1Slider, debug2Slider, debug3Slider, debug4Slider,scaleSlider,motorControlSlider; + +Slider servoSliderH[] = new Slider[8], + servoSliderV[] = new Slider[8], + motSlider[] = new Slider[8], + TX_StickSlider[] = new Slider[8], + GimbalSlider[] = new Slider[12], + ServoSliderC[] = new Slider[8], + ServoSliderMAX[] = new Slider[8], + ServoSliderMIN[] = new Slider[8]; + + +Button buttonIMPORT, buttonSAVE, buttonREAD, buttonRESET, buttonWRITE, buttonCALIBRATE_ACC, buttonCALIBRATE_MAG, buttonSTART, buttonSTOP, buttonSETTING, + buttonAcc, buttonBaro, buttonMag, buttonGPS, buttonSonar, buttonOptic, buttonRXbind, btnQConnect,buttonExport, + btMagDecl,btMWiiHome,btDownloads; + +Button SaveSERVO,buttonSERVO,buttonWing,SaveWing,buttonLIVE,buttonCCPM,buttonGimbal,btnTrigger; + +Toggle tACC_ROLL, tACC_PITCH, tACC_Z, tGYRO_ROLL, tGYRO_PITCH, tGYRO_YAW, tBARO,tHEAD, tMAGX, tMAGY, tMAGZ, + tDEBUG1, tDEBUG2, tDEBUG3, tDEBUG4; + +// Motors Toggles +Toggle motToggle[] = new Toggle[8]; + +// Colors +color yellow_ = color(200, 200, 20), green_ = color(30, 120, 30), red_ = color(120, 30, 30), blue_ = color(50, 50, 100), + grey_ = color(30, 30, 30),black_ = color(0, 0, 0),orange_ =color(200,128,0); + +PFont font8, font9, font12, font15; + +CheckBox checkbox[], checkboxRev[]; +CheckBox Bbox, Wbox, Mbox; +Button buttonCheckbox[], BtServo[], BtAUX[]; +Numberbox RateSlider[]; + +// TODO New part +void create_GimbalGraphics(){ + if(gimbal){ + gimbalConfig = true; + int sMin=1020;int sMax=2000; + if(servoStretch) {sMin=500; sMax=2500;} + int Step=yServ-10; + GimbalSlider[0] = controlP5.addSlider("Tilt_Min" ,sMin,1500,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[1] = controlP5.addSlider("Tilt_Max" ,1500,sMax,0,xServ+150 ,Step+80,60,10).setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[2] = controlP5.addSlider("Channel " ,1200,1700,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[3] = controlP5.addSlider("Tilt_Prop",-125,125,0,xServ+100,Step+100, 60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + Step+=90; + GimbalSlider[4] = controlP5.addSlider("Roll_Min" ,sMin,1500,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[5] = controlP5.addSlider("Roll_Max" ,1500,sMax,0,xServ+150 ,Step+80,60,10).setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[6] = controlP5.addSlider("Channel" ,1200,1700,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[7] = controlP5.addSlider("Roll_Prop",-125,125,0,xServ+100,Step+100, 60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + + GimbalSlider[8] = controlP5.addSlider("Trig_LO" ,500,2000,0,xServ+10,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[9] = controlP5.addSlider("Trig_HI" ,1000,sMax,0,xServ+150 ,Step+80,60,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[10] = controlP5.addSlider("Trigger" ,0,30000,0,xServ+100,Step+60,90,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings"); + GimbalSlider[11] = controlP5.addSlider("Trig_Rev",-0,1,0,xServ+100,Step+100, 40,10) .setDecimalPrecision(0).hide().moveTo("ServoSettings") .setNumberOfTickMarks(2).setColorTickMark(0); + + buttonGimbal.show(); + if(camTrigger)btnTrigger.show(); + controlP5.getTab("ServoSettings").show(); + } +} + +void create_ServoGraphics(){ + BtServo = new Button[8]; + BtAUX = new Button[5]; + checkboxRev = new CheckBox[8]; + RateSlider = new Numberbox[8]; + GraphicsInited=true; + Bbox = controlP5.addCheckBox("Bbox").setPosition(xServ+40,yServ+40).setColorForeground(color(120)) + .setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10).setColorBackground(color(140)) + .setItemsPerRow(1).setSpacingColumn(30).setSpacingRow(10) + .addItem("0", 254).addItem("1", 254).addItem("2", 254) + .addItem("3", 254).addItem("4", 254).addItem("5", 254) + .addItem("6", 254).addItem("7", 254) + .hide().hideLabels().moveTo("ServoSettings") + ; + + //Create Common ServoSliders + int Step =0; + int sMin=1020;int sMax=2000; + if(servoStretch) {sMin=500; sMax=2500;} + for (i=0;i<8;i++) { + ServoSliderC[i] = controlP5.addSlider("Servo "+i,Centerlimits[0],Centerlimits[1],0,xServ+180,yServ+40+Step,110,12).setDecimalPrecision(0).hide().setLabel("").moveTo("ServoSettings"); + ServoSliderMIN[i] = controlP5.addSlider("MIN "+i,sMin,1500,0,xServ+80,yServ+40+Step,40,12).setDecimalPrecision(0).setLabel("").hide().moveTo("ServoSettings"); + ServoSliderMAX[i] = controlP5.addSlider("MAX "+i,1500,sMax,0,xServ+125,yServ+40+Step,40,12).setDecimalPrecision(0).setLabel("").hide().moveTo("ServoSettings"); + Step+=20; } + + // ServoGraphics For AirPlane SC & DC +if(multiType == PPM_TO_SERVO || multiType == AIRPLANE ){ + controlP5.getTab("ServoSettings").show(); + TxtRates = controlP5.addTextlabel("label","Servo Rates in %").setPosition(xServ+75,yServ+20).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset for servos").setPosition(xServ+190,yServ+20).hide().moveTo("ServoSettings"); + TxtAux = controlP5.addTextlabel("Alabel","Channel for Flaps").setPosition(xServ-120,yServ+20).hide().moveTo("ServoSettings"); + TxtRev = controlP5.addTextlabel("txtlblRev","Norm/Rev").setPosition(xServ+10,yServ+20).hide().moveTo("ServoSettings"); + +//****************** End of AirPlane ******************** +} + +if(multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER){ //ServoGraphics For FlyingWng & TRI & BI || multiType == DUALCOPTER +controlP5.getTab("ServoSettings").show(); +int LabelPos=100; + +if(multiType == FLYING_WING){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("L Roll", 254).addItem("R Roll", 254).addItem("L NICK", 254).addItem("R NICK", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Left Wing" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Right Wing").setPosition(xServ+110,yServ+130).hide().moveTo("ServoSettings"); + } + if(multiType == TRI){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(1).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("YAW", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Yaw Servo" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel"," ").setPosition(xServ+110,yServ+140).hide().moveTo("ServoSettings"); + } + if(multiType == BI){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("L Yaw", 254).addItem("R Yaw", 254).addItem("L NICK", 254).addItem("R NICK", 254).moveTo("ServoSettings") ; + TxtLeftW = controlP5.addTextlabel("Label","Left Servo" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Right Servo").setPosition(xServ+110,yServ+140).hide().moveTo("ServoSettings"); + } + if(multiType == DUALCOPTER){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(70).setSpacingRow(10).setPosition(xServ-160 ,yServ+50) + .addItem("Pitch ", 254).addItem("Roll ", 254).moveTo("ServoSettings").hide(); + TxtLeftW = controlP5.addTextlabel("Label","Roll" ).setPosition(xServ+110,yServ+40).hide().moveTo("ServoSettings"); + TxtRightW = controlP5.addTextlabel("Rlabel","Nick").setPosition(xServ+110,yServ+130).hide().moveTo("ServoSettings"); + } + if(multiType == SINGLECOPTER){ + Wbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(2).setSpacingColumn(50).setSpacingRow(10).setPosition(xServ-80 ,yServ+100) + .addItem(" Right ", 254).addItem("R yaw", 254) + .addItem(" Left ", 254) .addItem("L yaw", 254) + .addItem(" Front", 254) .addItem("F yaw", 254) + .addItem(" Rear", 254) .addItem(" yaw", 254) + .moveTo("ServoSettings") ; +LabelPos=60; + TxtMin = controlP5.addTextlabel("Minlabel","MIN").setPosition(xServ+85,yServ+80) .hide().moveTo("ServoSettings"); + TxtMax = controlP5.addTextlabel("Maxlabel","MAX").setPosition(xServ+130,yServ+80).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset servos").setPosition(xServ+190,yServ+80).hide().moveTo("ServoSettings"); + } + + TxtRevW = controlP5.addTextlabel("Revlabel","Change Gyro/Acc Direction" ).setPosition(xServ-165,yServ+30).hide().moveTo("ServoSettings"); + TxtRevR = controlP5.addTextlabel("Revtx","Change Dir in TX To Match" ).setPosition(xServ-170,yServ+LabelPos).hide().moveTo("ServoSettings"); + +//****************** End of FlyingWng & TRI ******************** +} + +// ServoGraphics For Heli 120 && 90 +if(multiType == HELI_120_CCPM || multiType == HELI_90_DEG ){ +controlP5.getTab("ServoSettings").show(); + TxtMin = controlP5.addTextlabel("Minlabel","MIN").setPosition(xServ+85,yServ+20) .hide().moveTo("ServoSettings"); + TxtMax = controlP5.addTextlabel("Maxlabel","MAX").setPosition(xServ+130,yServ+20).hide().moveTo("ServoSettings"); + TxtMids = controlP5.addTextlabel("Mlabel","Offset servos").setPosition(xServ+190,yServ+20).hide().moveTo("ServoSettings"); + TxtRates = controlP5.addTextlabel("label","Not Used").setPosition(xServ+75,yServ+20).hide().moveTo("ServoSettings"); + TxtRev = controlP5.addTextlabel("txtlblRev","Servos").setPosition(xServ+10,yServ+20).hide().moveTo("ServoSettings"); + +// CCPM settings + // Mixer Boxes + int XPos=xServ+40; + int YPos=yServ-0; + Mbox = controlP5.addCheckBox("Wbox").setColorForeground(color(120)).setColorBackground(color(140)).setColorActive(color(255)).setColorLabel(color(255)).setSize(20, 10) + .setItemsPerRow(3).setSpacingColumn(15).setSpacingRow(10).setPosition(XPos-180 ,YPos+50) + .addItem("mix3_4", 254) .addItem("mix3_2", 254).addItem("mix3_1", 254) + .addItem("mix4_4", 254) .addItem("mix4_2", 254).addItem("mix4_1", 254) + .addItem("mix6_4", 254) .addItem("mix6_2", 254).addItem("mix6_1", 254) + .moveTo("ServoSettings").hideLabels().hide(); + TxtMIX[0] = controlP5.addTextlabel("mix1","NICK") .setPosition(XPos-220,YPos+50).moveTo("ServoSettings"); + TxtMIX[1] = controlP5.addTextlabel("mix2","LEFT") .setPosition(XPos-220,YPos+70).moveTo("ServoSettings"); + TxtMIX[2] = controlP5.addTextlabel("mix3","RIGHT").setPosition(XPos-220,YPos+90).moveTo("ServoSettings"); + TxtMIX[3] = controlP5.addTextlabel("mix4","COLL") .setPosition(XPos-195,YPos+30).moveTo("ServoSettings"); + TxtMIX[4] = controlP5.addTextlabel("mix5","NICK") .setPosition(XPos-155,YPos+30).moveTo("ServoSettings"); + TxtMIX[5] = controlP5.addTextlabel("mix6","ROLL") .setPosition(XPos-115,YPos+30).moveTo("ServoSettings"); + for (i=0;i<6;i++) { TxtMIX[i].hide();} + + //******************End of Heli******************** +} +// Common Graphics for servos + Step =0; + for (i=0;i<8;i++) { // TODO Something + BtServo[i] = controlP5.addButton("CHb"+i,1,xServ-30,yServ+40+20*i,60,12).setColorBackground(green_).setLabel("Servo "+i).hide().moveTo("ServoSettings"); + + checkboxRev[i] = controlP5.addCheckBox("cbR"+i).moveTo("ServoSettings"); + checkboxRev[i].setPosition(xServ+70,yServ+40+20*i) + .setColorActive(color(255)).setColorBackground(color(120)) + .setItemHeight(10).setItemWidth(20).hide() + .moveTo("ServoSettings") + ; + + RateSlider[i] = (controlP5.Numberbox) hideLabel(controlP5.addNumberbox("Rate"+i,0,xServ+70,yServ+40+i*20,100,14)); + RateSlider[i].setColorBackground(yellow_);RateSlider[i].setDirection(Controller.HORIZONTAL); + RateSlider[i].setDecimalPrecision(0);RateSlider[i].setMultiplier(1);RateSlider[i].setMin(0).setMax(125).hide().moveTo("ServoSettings"); + } + + for (i=0;i<4;i++) BtAUX[i] = controlP5.addButton("Cau"+i,1,xServ-100,yServ+40+20*i,60,12).setColorBackground(red_).setLabel(" AUX "+(i+1)).moveTo("ServoSettings").hide(); + BtAUX[4] = controlP5.addButton("Cau4" ,1,xServ-100,yServ+120,60,12).setColorBackground(blue_).setLabel("Disable").moveTo("ServoSettings").hide(); + + //************************ End of servoGrapics********************** +} + +void create_checkboxes(String[] names) { + /* destroy old buttons */ + for (int i=0; imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +public controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +public void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0f; else pVersion = 1.5f; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + int c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? PApplet.parseInt(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +public void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0f); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0f); + confRC_EXPO.setValue(byteRC_EXPO/100.0f); + rollPitchRate.setValue(byteRollPitchRate/100.0f); + yawRate.setValue(byteYawRate/100.0f); + dynamic_THR_PID.setValue(byteDynThrPID/100.0f); + throttle_MID.setValue(byteThrottle_MID/100.0f); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0f); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0f).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +public void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(PApplet.parseChar (PApplet.parseInt(motorControlSlider.getValue()) % 256) ); payload.add(PApplet.parseChar (PApplet.parseInt(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(PApplet.parseChar (1000 % 256) ); payload.add(PApplet.parseChar (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(PApplet.parseChar( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Servo centers + + servoRATE[i] = PApplet.parseInt(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(PApplet.parseChar(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(PApplet.parseChar(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(PApplet.parseChar(11)); }else{ payload.add(PApplet.parseChar(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Servo centers + + servoRATE[i] = PApplet.parseInt(RateSlider[i].value()); + + payload.add(PApplet.parseChar(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); // Servo centers + payload.add(PApplet.parseChar(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(PApplet.parseChar( round(confRC_RATE.value()*100)) ); + payload.add(PApplet.parseChar( round(confRC_EXPO.value()*100)) ); + payload.add(PApplet.parseChar( round(rollPitchRate.value()*100)) ); + payload.add(PApplet.parseChar( round(yawRate.value()*100)) ); + payload.add(PApplet.parseChar( round(dynamic_THR_PID.value()*100)) ); + payload.add(PApplet.parseChar( round(throttle_MID.value()*100)) ); + payload.add(PApplet.parseChar( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(PApplet.parseChar(intPowerTrigger % 256)); payload.add(PApplet.parseChar(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(PApplet.parseChar (q % 256) ); payload.add(PApplet.parseChar (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(PApplet.parseChar (nn - ((nn>>8)<<8) )); payload.add(PApplet.parseChar (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(PApplet.parseChar (nn - ((nn>>8)<<8))); payload.add(PApplet.parseChar (nn>>8)); // g 32b + payload.add(PApplet.parseChar (nn - ((nn>>16)<<16))); payload.add(PApplet.parseChar (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(PApplet.parseChar (nn - ((nn>>8)<<8) )); payload.add(PApplet.parseChar (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(PApplet.parseChar (nn)); // i + for( i=1;i<4;i++) { int q= PApplet.parseInt(VBat[i].value()*10); payload.add(PApplet.parseChar (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0f; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0f/360.0f),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5f, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55f*size, byteMP[0], 'L'); + drawMotor(+size, -0.55f*size, byteMP[1], 'R'); + drawMotor(-size, +0.55f*size, byteMP[2], 'L'); + drawMotor(-size, -0.55f*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55f*size,0,0);line(size,-0.55f*size,0,0);line(-size,+0.55f*size,0,0);line(size,+0.55f*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8f*size,size);line(1.8f*size,size,1.8f*size,size-30); line(1.8f*size,size-30,0,-1.5f*size); + line(0,0, -1.8f*size,+size);line(-1.8f*size,size,-1.8f*size,+size-30); line(-1.8f*size,size-30,0,-1.5f*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2f*size, byteMP[0], 'R'); + drawMotor(+size, -1.2f*size, byteMP[1], 'L'); + drawMotor(-size, +1.2f*size, byteMP[2], 'L'); + drawMotor(-size, -1.2f*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2f*size,+size, -1.2f*size);line(-size, +1.2f*size,-size, -1.2f*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55f*size, +size, byteMP[0], 'L'); + drawMotor(+0.55f*size, -size, byteMP[1], 'L'); + drawMotor(-0.55f*size, +size, byteMP[2], 'R'); + drawMotor(-0.55f*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55f*size,-size,0,0);line(-0.55f*size,size,0,0);line(+0.55f*size,-size,0,0);line(+0.55f*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3f; + float VingRoot = Span*0.25f; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4f),size, (size*0.4f),size); line(-(size*0.4f),size+5, (size*0.4f),size+5); + line(-(size*0.4f),size, -(size*0.4f),size+5); line((size*0.4f),size, (size*0.4f),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8f; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5f*scalesize,-5, -2,-0.5f*scalesize,-5); line(0,1.5f*scalesize,-5, 2,-0.5f*scalesize,-5); line( -2,-0.5f*scalesize,-5, 2,-0.5f*scalesize,-5); + // Fin + float finpos = scalesize * 1.3f; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3f),scalesize,-5, (scalesize*0.3f),scalesize,-5); line(-(scalesize*0.3f),scalesize+3,-5, (scalesize*0.3f),scalesize+3,-5); + line(-(scalesize*0.3f),scalesize,-5, -(scalesize*0.3f),scalesize+3,-5); line((scalesize*0.3f),scalesize,-5, (scalesize*0.3f),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8f; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5f*scalesize, -2,-0.5f*scalesize); line(0,1.5f*scalesize, 2,-0.5f*scalesize); line( -2,-0.5f*scalesize, 2,-0.5f*scalesize); + // Fin + float finpos = scalesize * 1.3f; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3f),scalesize, (scalesize*0.3f),scalesize); line(-(scalesize*0.3f),scalesize+3, (scalesize*0.3f),scalesize+3); + line(-(scalesize*0.3f),scalesize, -(scalesize*0.3f),scalesize+3); line((scalesize*0.3f),scalesize, (scalesize*0.3f),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55f*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55f*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55f*size,size,0,0);line(+0.55f*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55f*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3f; + float VingRoot = Span*0.25f; + // Stab + line(0,VingRoot, (size*0.4f),size); line(-(size*0.4f),size+5, (size*0.4f),size+5); + line(0,VingRoot, -(size*0.4f),size); + line(-(size*0.4f),size, -(size*0.4f),size+5); line((size*0.4f),size, (size*0.4f),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3f; + float VingRoot = Span*0.25f; + // Stab + line(0,VingRoot, (size*0.4f),size); line(-(size*0.4f),size+5, (size*0.4f),size+5); + line(0,VingRoot, -(size*0.4f),size); + line(-(size*0.4f),size, -(size*0.4f),size+5); line((size*0.4f),size, (size*0.4f),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "\u00b0", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "\u00b0", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5f); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5f); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5f,-size*2.5f,size*2.5f,size*2.5f); + // GPS quadrant + strokeWeight(1.5f); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4f*size);line(0,-2.4f*size, -5 ,-2.4f*size+10); line(0,-2.4f*size, +5 ,-2.4f*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5f);fill(0);stroke(0); + ellipse(0, 0, 2.6f*size+7, 2.6f*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2f, 0,-size*1.3f); line(0,-size*1.3f, -5 ,-size*1.3f+10); line(0,-size*1.3f, +5 ,-size*1.3f+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6f,yCompass+sin(angCalc)*size*1.6f); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2f,yCompass+sin(angCalc)*size*2.2f,xCompass+cos(angCalc)*size*1.9f,yCompass+sin(angCalc)*size*1.9f); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9f);text("S", xCompass-5, yCompass+32+size*0.9f); + text("W", xCompass-33-size*0.9f, yCompass+6);text("E", xCompass+21+size*0.9f, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "\u00b0",xCompass-11-(head>=10.0f ? (head>=100.0f ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "\u00b0",xCompass-6-size*2.1f,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1f,yCompass+1+size*2,xCompass+9+size*2.1f,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1f,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5f); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = PApplet.parseInt( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5f/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = PApplet.parseInt(100*a); + int expo = PApplet.parseInt(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = PApplet.parseInt( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5f/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(0xff000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +public void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0f; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +public void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +public void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +public void ACC_Z(boolean theFlag) {azGraph = theFlag;} +public void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +public void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +public void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +public void BARO(boolean theFlag) {altGraph = theFlag;} +public void HEAD(boolean theFlag) {headGraph = theFlag;} +public void MAGX(boolean theFlag) {magxGraph = theFlag;} +public void MAGY(boolean theFlag) {magyGraph = theFlag;} +public void MAGZ(boolean theFlag) {magzGraph = theFlag;} +public void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +public void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +public void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +public void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +public void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +public void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +public void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[PApplet.parseInt(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +public void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + public void Eport_Servo(){ + READ(); + ExportServo=true; + } + +public void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +public void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +public void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +public void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +public void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +public void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +public void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +public void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = PApplet.parseInt(pieces[0]); + GUI_BaudRate= PApplet.parseInt(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + public float getMinVal() { + float res = 0.0f; + for( i=0; i0 && imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && imaxlen) shortName = shortName.substring(0,(maxlen-1)/2) + "~" +shortName.substring(shortName.length()-(maxlen-(maxlen-1)/2)); + if(shortName.startsWith("cu.")) shortName = "";// only collect the corresponding tty. devices + return shortName; + */ +} + +controlP5.Controller hideLabel(controlP5.Controller c) { + c.setLabel(""); + c.setLabelVisible(false); + return c; +} + +void setup() { + // Trying to make both worlds happy.. + if( P3D == OPENGL ) pVersion = 2.0; else pVersion = 1.5; + + size(windowsX,windowsY,OPENGL); + frameRate(20); + + font8 = createFont("Arial bold",8,false); + font9 = createFont("Arial bold",9,false); + font12 = createFont("Arial bold",12,false); + font15 = createFont("Arial bold",15,false); + + controlP5 = new ControlP5(this); // initialize the GUI controls + controlP5.setControlFont(font12); + addTabs(); + + g_graph = new cGraph(xGraph+110,yGraph, 480, 200); + + // Baud list items + baudListbox = controlP5.addListBox("baudList",5,95+tabHeight,110,240).moveTo("Config"); // make a listbox with available Baudrates + baudListbox.captionLabel().set("BAUD_RATE"); + baudListbox.setColorBackground(red_); + baudListbox.setBarHeight(17); + + baudListbox.addItem("9600" ,9600); // addItem(name,value) + baudListbox.addItem("14400" ,14400); + baudListbox.addItem("19200" ,19200); + baudListbox.addItem("28800" ,28800); + baudListbox.addItem("38400" ,38400); + baudListbox.addItem("57600" ,57600); + baudListbox.addItem("115200",115200); + + // make a listbox and populate it with the available comm ports + commListbox = controlP5.addListBox("portComList",5,105+tabHeight,110,120); + commListbox.captionLabel().set("PORT COM"); + commListbox.setColorBackground(red_); + commListbox.setBarHeight(17); + + for( i=0;i0 ) commListbox.addItem(pn,i); // addItem(name,value) + commListMax = i; + //} + } + commListbox.addItem("Close Comm",++commListMax); // addItem(name,value) + // text label for which comm port selected + txtlblWhichcom = controlP5.addTextlabel("txtlblWhichcom","No Port Selected",5,65+tabHeight); // textlabel(name,text,x,y) + // Information textlabels + TxtInfo = controlP5.addTextlabel("SInf","Remember To Save Changes to Eeprom!!") .setPosition(xServ-30, yServ+210).hide().moveTo("ServoSettings"); + TxtInfo1 = controlP5.addTextlabel("xInf","Grey Values Is Set As #define In Config.h!!") .setPosition(xServ+0, yServ+210).moveTo("Config"); + TxtInfo2 = controlP5.addTextlabel("gInf","Green Values Can Be Changed Press Write To Save!!") .setPosition(xServ+0, yServ+190).moveTo("Config"); + TxtInfo3 = controlP5.addTextlabel("CsInf","Remember To Activate CAMSTAB!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + TxtInfo4 = controlP5.addTextlabel("CtInf","Remember To Activate CAMTRIG!!") .setPosition(xServ+10, yServ+20).hide().moveTo("ServoSettings"); + Links = controlP5.addTextlabel("LinkInf","Some Useful Webpages!!") .setPosition(xServ+100, yServ+20).moveTo("Config"); + + TxtInfoMotors1 = controlP5.addTextlabel("motInf1","This is a function for Balancing Propellors Dynamicly\n"+ +"Select motor(s) and control throttle.").setPosition(xServ-200, yServ+10).moveTo("Motors"); + + buttonSAVE = controlP5.addButton("bSAVE",1,5,45+tabHeight,40,19).setLabel("SAVE").setColorBackground(red_); + buttonIMPORT = controlP5.addButton("bIMPORT",1,50,45+tabHeight,40,19).setLabel("LOAD").setColorBackground(red_); + + btnQConnect = controlP5.addButton("bQCONN",1,xGraph+0,yGraph-105,100,19).setLabel(" ReConnect").setColorBackground(red_); + buttonSTART = controlP5.addButton("bSTART",1,xGraph+110,yGraph-25,45,19).setLabel("START").setColorBackground(red_); + buttonSTOP = controlP5.addButton("bSTOP",1,xGraph+160,yGraph-25,45,19).setLabel("STOP").setColorBackground(red_); + + buttonAcc = controlP5.addButton("bACC",1,xButton,yButton,45,15).setColorBackground(red_).setLabel("ACC"); + buttonBaro = controlP5.addButton("bBARO",1,xButton+50,yButton,45,15).setColorBackground(red_).setLabel("BARO"); + buttonMag = controlP5.addButton("bMAG",1,xButton+100,yButton,45,15).setColorBackground(red_).setLabel("MAG"); + buttonGPS = controlP5.addButton("bGPS",1,xButton,yButton+17,45,15).setColorBackground(red_).setLabel("GPS"); + buttonSonar = controlP5.addButton("bSonar",1,xButton+50,yButton+17,45,15).setColorBackground(red_).setLabel("SONAR"); + buttonOptic = controlP5.addButton("bOptic",1,xButton+100,yButton+17,45,15).setColorBackground(grey_).setLabel("OPTIC"); + + color c,black; + black = color(0,0,0); + int xo = xGraph-7; + int x = xGraph+40; + int y1= yGraph+10; //ACC + int y2= yGraph+55; //GYRO + int y5= yGraph+100; //MAG + int y3= yGraph+150; //ALT + int y4= yGraph+165; //HEAD + int y7= yGraph+185; //GPS + int y6= yGraph+205; //DEBUG + + tACC_ROLL = controlP5.addToggle("ACC_ROLL",true,x,y1+10,20,10).setColorActive(color(255, 0, 0)).setColorBackground(black).setLabel(""); + tACC_PITCH = controlP5.addToggle("ACC_PITCH",true,x,y1+20,20,10).setColorActive(color(0, 255, 0)).setColorBackground(black).setLabel(""); + tACC_Z = controlP5.addToggle("ACC_Z",true,x,y1+30,20,10).setColorActive(color(0, 0, 255)).setColorBackground(black).setLabel(""); + tGYRO_ROLL = controlP5.addToggle("GYRO_ROLL",true,x,y2+10,20,10).setColorActive(color(200, 200, 0)).setColorBackground(black).setLabel(""); + tGYRO_PITCH = controlP5.addToggle("GYRO_PITCH",true,x,y2+20,20,10).setColorActive(color(0, 255, 255)).setColorBackground(black).setLabel(""); + tGYRO_YAW = controlP5.addToggle("GYRO_YAW",true,x,y2+30,20,10).setColorActive(color(255, 0, 255)).setColorBackground(black).setLabel(""); + tBARO = controlP5.addToggle("BARO",true,x,y3 ,20,10).setColorActive(color(125, 125, 125)).setColorBackground(black).setLabel(""); + tHEAD = controlP5.addToggle("HEAD",true,x,y4 ,20,10).setColorActive(color(225, 225, 125)).setColorBackground(black).setLabel(""); + tMAGX = controlP5.addToggle("MAGX",true,x,y5+10,20,10).setColorActive(color(50, 100, 150)).setColorBackground(black).setLabel(""); + tMAGY = controlP5.addToggle("MAGY",true,x,y5+20,20,10).setColorActive(color(100, 50, 150)).setColorBackground(black).setLabel(""); + tMAGZ = controlP5.addToggle("MAGZ",true,x,y5+30,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel(""); + tDEBUG1 = controlP5.addToggle("DEBUG1",true,x+70,y6,20,10) .setColorActive(color(200, 50, 0)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG2 = controlP5.addToggle("DEBUG2",true,x+190,y6,20,10).setColorActive(color(0, 200, 50)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG3 = controlP5.addToggle("DEBUG3",true,x+310,y6,20,10).setColorActive(color(50, 0, 200)).setColorBackground(black).setLabel("").setValue(0); + tDEBUG4 = controlP5.addToggle("DEBUG4",true,x+430,y6,20,10).setColorActive(color(150, 100, 50)).setColorBackground(black).setLabel("").setValue(0); + + controlP5.addTextlabel( "alarmLabel", "Alarm:", xGraph -5, yGraph -32); + + controlP5.addTextlabel("acclabel","ACC",xo,y1 -4); + controlP5.addTextlabel("accrolllabel"," ROLL",xo,y1+10).setFont(font9); + controlP5.addTextlabel("accpitchlabel"," PITCH",xo,y1+20).setFont(font9); + controlP5.addTextlabel("acczlabel"," Z",xo,y1+30).setFont(font9); + controlP5.addTextlabel("gyrolabel","GYRO",xo,y2 -4); + controlP5.addTextlabel("gyrorolllabel"," ROLL",xo,y2+10).setFont(font9); + controlP5.addTextlabel("gyropitchlabel"," PITCH",xo,y2+20).setFont(font9); + controlP5.addTextlabel("gyroyawlabel"," YAW",xo,y2+30).setFont(font9); + controlP5.addTextlabel("maglabel","MAG",xo,y5 -4); + controlP5.addTextlabel("magrolllabel"," ROLL",xo,y5+10).setFont(font9); + controlP5.addTextlabel("magpitchlabel"," PITCH",xo,y5+20).setFont(font9); + controlP5.addTextlabel("magyawlabel"," YAW",xo,y5+30).setFont(font9); + controlP5.addTextlabel("altitudelabel","ALT",xo,y3 -4); + controlP5.addTextlabel("headlabel","HEAD",xo,y4 -4); + controlP5.addTextlabel("debug1","debug1",x+90,y6 -2).setFont(font9); + controlP5.addTextlabel("debug2","debug2",x+210,y6 -2).setFont(font9); + controlP5.addTextlabel("debug3","debug3",x+330,y6 -2).setFont(font9); + controlP5.addTextlabel("debug4","debug4",x+450,y6 -2).setFont(font9); + + axSlider = controlP5.addSlider("axSlider",-1000,+1000,0,x+20,y1+10,50,10).setDecimalPrecision(0).setLabel(""); + aySlider = controlP5.addSlider("aySlider",-1000,+1000,0,x+20,y1+20,50,10).setDecimalPrecision(0).setLabel(""); + azSlider = controlP5.addSlider("azSlider",-1000,+1000,0,x+20,y1+30,50,10).setDecimalPrecision(0).setLabel(""); + gxSlider = controlP5.addSlider("gxSlider",-5000,+5000,0,x+20,y2+10,50,10).setDecimalPrecision(0).setLabel(""); + gySlider = controlP5.addSlider("gySlider",-5000,+5000,0,x+20,y2+20,50,10).setDecimalPrecision(0).setLabel(""); + gzSlider = controlP5.addSlider("gzSlider",-5000,+5000,0,x+20,y2+30,50,10).setDecimalPrecision(0).setLabel(""); + altSlider = controlP5.addSlider("altSlider",-30000,+30000,0,x+20,y3 ,50,10).setDecimalPrecision(2).setLabel(""); + headSlider = controlP5.addSlider("headSlider",-200,+200,0,x+20,y4 ,50,10).setDecimalPrecision(0).setLabel(""); + magxSlider = controlP5.addSlider("magxSlider",-5000,+5000,0,x+20,y5+10,50,10).setDecimalPrecision(0).setLabel(""); + magySlider = controlP5.addSlider("magySlider",-5000,+5000,0,x+20,y5+20,50,10).setDecimalPrecision(0).setLabel(""); + magzSlider = controlP5.addSlider("magzSlider",-5000,+5000,0,x+20,y5+30,50,10).setDecimalPrecision(0).setLabel(""); + debug1Slider = controlP5.addSlider("debug1Slider",-32768,+32767,0,x+130,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug2Slider = controlP5.addSlider("debug2Slider",-32768,+32767,0,x+250,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug3Slider = controlP5.addSlider("debug3Slider",-32768,+32767,0,x+370,y6,50,10).setDecimalPrecision(0).setLabel(""); + debug4Slider = controlP5.addSlider("debug4Slider",-32768,+32767,0,x+490,y6,50,10).setDecimalPrecision(0).setLabel(""); + + for( i=0;i requestMSP(int msp) { + return requestMSP( msp, null); +} + +//send multiple msp without payload +private List requestMSP (int[] msps) { + List s = new LinkedList(); + for (int m : msps) { + s.addAll(requestMSP(m, null)); + } + return s; +} + +//send msp with payload +private List requestMSP (int msp, Character[] payload) { + if(msp < 0) { + return null; + } + List bf = new LinkedList(); + for (byte c : MSP_HEADER.getBytes()) { + bf.add( c ); + } + + byte checksum=0; + byte pl_size = (byte)((payload != null ? int(payload.length) : 0)&0xFF); + bf.add(pl_size); + checksum ^= (pl_size&0xFF); + + bf.add((byte)(msp & 0xFF)); + checksum ^= (msp&0xFF); + + if (payload != null) { + for (char c :payload){ + bf.add((byte)(c&0xFF)); + checksum ^= (c&0xFF); + } + } + bf.add(checksum); + return (bf); +} + +void sendRequestMSP(List msp) { + byte[] arr = new byte[msp.size()]; + int i = 0; + for (byte b: msp) { + arr[i++] = b; + } + g_serial.write(arr); // send the complete byte sequence in one go +} + +public void evaluateCommand(byte cmd, int dataSize) { + int i; + int icmd = (int)(cmd&0xFF); + switch(icmd) { + case MSP_IDENT: + version = read8(); + multiType = read8(); + read8(); // MSP version + multiCapability = read32();// capability + if ((multiCapability&1)>0) {buttonRXbind = controlP5.addButton("bRXbind",1,10,yGraph+205-10,55,10); buttonRXbind.setColorBackground(blue_);buttonRXbind.setLabel("RX Bind");} + if ((multiCapability&4)>0) controlP5.addTab("Motors").show(); + if ((multiCapability&8)>0) flaps=true; + if (!GraphicsInited) create_ServoGraphics(); + break; + + case MSP_STATUS: + cycleTime = read16(); + i2cError = read16(); + present = read16(); + mode = read32(); + if ((present&1) >0) {buttonAcc.setColorBackground(green_);} else {buttonAcc.setColorBackground(red_);tACC_ROLL.setState(false); tACC_PITCH.setState(false); tACC_Z.setState(false);} + if ((present&2) >0) {buttonBaro.setColorBackground(green_);} else {buttonBaro.setColorBackground(red_); tBARO.setState(false); } + if ((present&4) >0) {buttonMag.setColorBackground(green_); Mag_=true;} else {buttonMag.setColorBackground(red_); tMAGX.setState(false); tMAGY.setState(false); tMAGZ.setState(false);} + if ((present&8) >0) {buttonGPS.setColorBackground(green_);} else {buttonGPS.setColorBackground(red_); tHEAD.setState(false);} + if ((present&16)>0) {buttonSonar.setColorBackground(green_);} else {buttonSonar.setColorBackground(red_);} + + for(i=0;i0) buttonCheckbox[i].setColorBackground(green_); else buttonCheckbox[i].setColorBackground(red_);} + confSetting.setValue(read8()); + confSetting.setColorBackground(green_); + break; + case MSP_RAW_IMU: + ax = read16();ay = read16();az = read16(); + if (ActiveTab=="Motors"){ // Show unfilterd values in graph. + gx = read16();gy = read16();gz = read16(); + magx = read16();magy = read16();magz = read16(); + }else{ + gx = read16()/8;gy = read16()/8;gz = read16()/8; + magx = read16()/3;magy = read16()/3;magz = read16()/3; + }break; + case MSP_SERVO:for(i=0;i<8;i++) servo[i] = read16(); break; + case MSP_MOTOR: for(i=0;i<8;i++){ mot[i] = read16();} + if (multiType == SINGLECOPTER)servo[7]=mot[0]; + if (multiType == DUALCOPTER){servo[7]=mot[0];servo[6]=mot[1];} + break; + case MSP_RC: + for(i=0;i<8;i++) { + RCChan[i]=read16(); + TX_StickSlider[i].setValue(RCChan[i]); + } + break; + case MSP_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); break; + case MSP_COMP_GPS: + GPS_distanceToHome = read16(); + GPS_directionToHome = read16(); + GPS_update = read8(); break; + case MSP_ATTITUDE: + angx = read16()/10;angy = read16()/10; + head = read16(); break; + case MSP_ALTITUDE: alt = read32(); break; + case MSP_ANALOG: + bytevbat = read8(); + pMeterSum = read16(); + rssi = read16(); if(rssi!=0)VBat[5].setValue(rssi).show(); // rssi + amperage = read16(); // amperage + VBat[4].setValue(bytevbat/10.0); // Volt + break; + case MSP_RC_TUNING: + byteRC_RATE = read8();byteRC_EXPO = read8();byteRollPitchRate = read8(); + byteYawRate = read8();byteDynThrPID = read8(); + byteThrottle_MID = read8();byteThrottle_EXPO = read8(); + confRC_RATE.setValue(byteRC_RATE/100.0); + confRC_EXPO.setValue(byteRC_EXPO/100.0); + rollPitchRate.setValue(byteRollPitchRate/100.0); + yawRate.setValue(byteYawRate/100.0); + dynamic_THR_PID.setValue(byteDynThrPID/100.0); + throttle_MID.setValue(byteThrottle_MID/100.0); + throttle_EXPO.setValue(byteThrottle_EXPO/100.0); + confRC_RATE.setColorBackground(green_);confRC_EXPO.setColorBackground(green_);rollPitchRate.setColorBackground(green_); + yawRate.setColorBackground(green_);dynamic_THR_PID.setColorBackground(green_); + throttle_MID.setColorBackground(green_);throttle_EXPO.setColorBackground(green_); + updateModelMSP_SET_RC_TUNING(); + break; + case MSP_ACC_CALIBRATION:break; + case MSP_MAG_CALIBRATION:break; + case MSP_PID: + for(i=0;i0) {checkbox[i].activate(aa);}else {checkbox[i].deactivate(aa);}}} break; + case MSP_BOXNAMES: + create_checkboxes(new String(inBuf, 0, dataSize).split(";"));break; + case MSP_PIDNAMES: + /* TODO create GUI elements from this message */ + //System.out.println("Got PIDNAMES: "+new String(inBuf, 0, dataSize)); + break; + case MSP_SERVO_CONF: + Bbox.deactivateAll(); + // min:2 / max:2 / middle:2 / rate:1 + for( i=0;i<8;i++){ + ServoMIN[i] = read16(); + ServoMAX[i] = read16(); + ServoMID[i] = read16(); + servoRATE[i] = read8() ; + } + if (multiType == AIRPLANE ) { // Airplane OK + if(flaps) { + //ServoSliderC[2].setMin(4).setMax(10); + if(ServoMID[2]==4) {Cau0();}else if(ServoMID[2]==5) {Cau1();}else if(ServoMID[2]==6) {Cau2();}else if(ServoMID[2]==7){Cau3();}else{CauClear();} + } + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{ + Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i])); + } + } + + } else if (multiType == FLYING_WING || multiType == TRI || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // FlyingWing & TRI & BI + int nBoxes; + for( i=0;i<8;i++){ //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + if (servoRATE[i]>127){ // Reverse/Rate servos + wingDir[i]=-1; RateSlider[i].setValue((servoRATE[i]-256)); + }else{ wingDir[i]=1; RateSlider[i].setValue(abs(servoRATE[i])); } // Servo Direction + } + + if(multiType == FLYING_WING) { //OK + if ((servoRATE[3]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + + + } else if(multiType == SINGLECOPTER) { // + if ((servoRATE[3]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // + if ((servoRATE[3]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} // + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // + if ((servoRATE[4]&2)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} // + if ((servoRATE[5]&1)<1) {Wbox.deactivate(4);}else{Wbox.activate(4);} // + if ((servoRATE[5]&2)<1) {Wbox.deactivate(5);}else{Wbox.activate(5);} // + if ((servoRATE[6]&1)<1) {Wbox.deactivate(6);}else{Wbox.activate(6);} // + if ((servoRATE[6]&2)<1) {Wbox.deactivate(7);}else{Wbox.activate(7);} // + + + } else if(multiType == DUALCOPTER) { // OK + if ((servoRATE[4]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + if ((servoRATE[5]&1)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + + + } else if (multiType == TRI) {// OK + if ((servoRATE[5]&1)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} + + } else if( multiType == BI) {// OK + if ((servoRATE[4]&2)<1) {Wbox.deactivate(0);}else{Wbox.activate(0);} // L + if ((servoRATE[5]&2)<1) {Wbox.deactivate(1);}else{Wbox.activate(1);} + if ((servoRATE[4]&1)<1) {Wbox.deactivate(2);}else{Wbox.activate(2);} // R + if ((servoRATE[5]&1)<1) {Wbox.deactivate(3);}else{Wbox.activate(3);} + } + + }else if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG) { + for( i=0;i<8;i++) { //Update sliders + ServoSliderMIN[i].setValue(ServoMIN[i]); + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i].setValue(ServoMID[i]); + + if (servoRATE[i]>127){ // Reverse/Rate servos + Bbox.deactivate(i); RateSlider[i].setValue((servoRATE[i]-256)); + }else{ Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + if ((servoRATE[5]&1)<1) {Bbox.deactivate(5);}else{Bbox.activate(5);} // YawReverse + if(multiType == HELI_120_CCPM) { //bbb + if ((servoRATE[3]&1)<1) {Mbox.deactivate(2);}else{Mbox.activate(2);} // roll + if ((servoRATE[3]&2)<1) {Mbox.deactivate(1);}else{Mbox.activate(1);} // nick + if ((servoRATE[3]&4)<1) {Mbox.deactivate(0);}else{Mbox.activate(0);} // coll + if ((servoRATE[4]&1)<1) {Mbox.deactivate(5);}else{Mbox.activate(5);} // + if ((servoRATE[4]&2)<1) {Mbox.deactivate(4);}else{Mbox.activate(4);} // + if ((servoRATE[4]&4)<1) {Mbox.deactivate(3);}else{Mbox.activate(3);} // + if ((servoRATE[6]&1)<1) {Mbox.deactivate(8);}else{Mbox.activate(8);} // + if ((servoRATE[6]&2)<1) {Mbox.deactivate(7);}else{Mbox.activate(7);} // + if ((servoRATE[6]&4)<1) {Mbox.deactivate(6);}else{Mbox.activate(6);} // + } + + }else if (multiType == PPM_TO_SERVO ) { // PPM_TO_SERVO + for( i=0;i<8;i++){ + ServoSliderMIN[i].setValue(ServoMIN[i]); //Update sliders + ServoSliderMAX[i].setValue(ServoMAX[i]); + ServoSliderC[i] .setValue(ServoMID[i]); + // Reverse/Rate servos + if (servoRATE[i]>127){ + Bbox.deactivate(i); RateSlider[i].setValue(abs(servoRATE[i]-256)); + }else{Bbox.activate(i); RateSlider[i].setValue(abs(servoRATE[i]));} + } + } + + if (gimbal){ + if(!gimbalConfig)create_GimbalGraphics(); + // Switch beween Channels or Centerpos. + if(ServoMID[0]>1200) {GimbalSlider[2] .setMin(1200).setMax(1700); }else{GimbalSlider[2] .setMin(0).setMax(12);} + if(ServoMID[1]>1200) {GimbalSlider[6] .setMin(1200).setMax(1700); }else{GimbalSlider[6] .setMin(0).setMax(12);} + if(ServoMID[2]>1000) {GimbalSlider[10].setMin(1000).setMax(30000);}else{GimbalSlider[10].setMin(0).setMax(12);} + + + i=0; + GimbalSlider[0] .setValue((int)ServoMIN[i]); + GimbalSlider[1] .setValue((int)ServoMAX[i]); + GimbalSlider[2] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[3].setValue((servoRATE[i]-256)); + }else{ GimbalSlider[3].setValue(abs(servoRATE[i]));} + + i=1; + GimbalSlider[4] .setValue((int)ServoMIN[i]); + GimbalSlider[5] .setValue((int)ServoMAX[i]); + GimbalSlider[6] .setValue((int)ServoMID[i]); + if (servoRATE[i]>127){ GimbalSlider[7].setValue((servoRATE[i]-256)); + }else{GimbalSlider[7].setValue(abs(servoRATE[i]));} + + i=2; + GimbalSlider[8] .setValue((int)ServoMIN[i]); + GimbalSlider[9] .setValue((int)ServoMAX[i]); + GimbalSlider[10].setValue((int)ServoMID[i]); + GimbalSlider[11].setValue((int)servoRATE[i]); + } + if (camTrigger){ + if(ServoMID[2]>1200) {ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]);}else{ServoSliderC[2].setMin(0).setMax(12);} + } + + if(ExportServo) SAVE_SERVO_CONFIG(); // ServoConfig to file + //****************************************************************************************** + break; + + + case MSP_MISC: + intPowerTrigger = read16(); // a + + //int minthrottle,maxthrottle,mincommand,FSthrottle,armedNum,lifetime,mag_decliniation ; + for (i=0;i<4;i++) { MConf[i]= read16(); + confINF[i].setValue((int)MConf[i]).show(); + } + if(MConf[3]<1000)confINF[3].hide(); + + // LOG_PERMANENT + MConf[4]= read16(); confINF[4].setValue((int)MConf[4]);//f + MConf[5]= read32(); confINF[5].setValue((int)MConf[5]);//g + for (i=1;i<3;i++){confINF[i].setColorBackground(grey_).setMin((int)MConf[i]).setMax((int)MConf[i]);} //? + + // hide LOG_PERMANENT + if(MConf[4]<1){confINF[5].hide();confINF[4].hide();}else{confINF[5].show();confINF[4].show();} + + //mag_decliniation + MConf[6]= read16(); confINF[6].setValue((float)MConf[6]/10).show(); //h + if(!Mag_)confINF[6].hide(); + + // VBAT + int q = read8();if(toggleVbat){VBat[0].setValue(q).setColorBackground(green_);toggleVbat=false; // i + for( i=1;i<4;i++) VBat[i].setValue(read8()/10.0).setColorBackground(green_);} // j,k,l + if(q > 1) for( i=0;i<5;i++) VBat[i].show(); + + controlP5.addTab("Config").show(); + + confPowerTrigger.setValue(intPowerTrigger); + updateModelMSP_SET_MISC(); + break; + case MSP_MOTOR_PINS: + for( i=0;i<8;i++) {byteMP[i] = read8();}break; + case MSP_DEBUGMSG: + while(dataSize-- > 0) { + char c = (char)read8(); + if (c != 0) {System.out.print( c );} + }break; + case MSP_DEBUG: + debug1 = read16();debug2 = read16();debug3 = read16();debug4 = read16(); break; + default: + //println("Don't know how to handle reply "+icmd); + } +} + +private int present = 0; +int time,time2,time3,time4,time5,time6; + +void draw() { + List payload; + int i,aa; + float val,inter,a,b,h; + int c; + if (init_com==1 && graph_on==1) { + time=millis(); + + if ((time-time4)>40) { + time4=time; + accROLL.addVal(ax);accPITCH.addVal(ay);accYAW.addVal(az);gyroROLL.addVal(gx);gyroPITCH.addVal(gy);gyroYAW.addVal(gz); + magxData.addVal(magx);magyData.addVal(magy);magzData.addVal(magz); + altData.addVal(alt);headData.addVal(head); + debug1Data.addVal(debug1);debug2Data.addVal(debug2);debug3Data.addVal(debug3);debug4Data.addVal(debug4); + } + + if (!toggleRead && !toggleWrite && !toggleSetSetting ) { + if ((time-time2)>40 ){ + time2=time; + int[] requests = {MSP_STATUS, MSP_RAW_IMU, MSP_SERVO, MSP_MOTOR, MSP_RC,MSP_DEBUG}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time3)>25 ) { + time3=time; + sendRequestMSP(requestMSP(MSP_ATTITUDE)); + } + if ((time-time5)>100 ) { + time5=time; + int[] requests = { MSP_ALTITUDE}; + sendRequestMSP(requestMSP(requests)); + } + if ((time-time6)>200 ) { + time6=time; + int[] requests = { MSP_RAW_GPS, MSP_COMP_GPS, MSP_ANALOG}; + sendRequestMSP(requestMSP(requests)); + + if (toggleMotor){ + payload = new ArrayList(); + for( i=0;i<8;i++){ + if (motToggle[i].getState()) { + payload.add(char (int(motorControlSlider.getValue()) % 256) ); payload.add(char (int(motorControlSlider.getValue()) / 256) ); + } else { + payload.add(char (1000 % 256) ); payload.add(char (1000 / 256) ); + } + } + sendRequestMSP(requestMSP(MSP_SET_MOTOR,payload.toArray( new Character[payload.size()]) )); + } + } + if (!toggleLive) { + buttonLIVE.setColorBackground(red_).setLabel(" Go Live"); + buttonRESET.show(); + controlP5.getTooltip().register("LIVE_SERVO","Enable Live changes to the Servos.") ; + }else{ + buttonLIVE.setColorBackground(green_).setLabel(" Live"); + buttonRESET.hide(); + buttonExport.show(); + controlP5.getTooltip().register("LIVE_SERVO","Disable Live changes to the Servos.") ; + if (multiType == FLYING_WING ||multiType == TRI ||multiType == BI || toggleGimbal || toggleTrigger || multiType == DUALCOPTER || multiType == SINGLECOPTER)toggleWriteWingLive=true; + if (multiType == HELI_120_CCPM || multiType == HELI_90_DEG ) toggleWriteServoLive=true; + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO ) toggleWriteServoLive=true; + } + } + if (toggleReset) { + toggleReset=false; + toggleRead=true; + sendRequestMSP(requestMSP(MSP_RESET_CONF)); + } +/*****************************************************************/ + + + if (toggleRead) { + if (!toggleLive &&( ActiveTab=="default" || ActiveTab =="Config")){// && ActiveTab=="default".. Because of checksum error on MSP_PID with servosTab + int[] requests = {MSP_IDENT ,MSP_BOXNAMES, MSP_RC_TUNING, MSP_PID, MSP_MOTOR_PINS,MSP_BOX,MSP_MISC}; // MSP_PIDNAMES, MSP_SERVO_CONF + sendRequestMSP(requestMSP(requests)); + } + if(GraphicsInited ){ // Don't call for servo conf before Graphics is created + int[] reques = { MSP_SERVO_CONF}; + sendRequestMSP(requestMSP(reques)); + } + buttonWRITE.setColorBackground(green_); + buttonSERVO.setColorBackground(green_); + buttonSETTING.setColorBackground(green_); + confSelectSetting.setColorBackground(green_); + toggleRead=false; + } + if (toggleSetSetting) { + toggleSetSetting=false; + toggleRead=true; + payload = new ArrayList(); + payload.add(char( round(confSelectSetting.value())) ); + sendRequestMSP(requestMSP(MSP_SELECT_SETTING,payload.toArray( new Character[payload.size()]) )); + } + if (toggleCalibAcc) { + toggleCalibAcc=false; + sendRequestMSP(requestMSP(MSP_ACC_CALIBRATION)); + } + if (toggleCalibMag) { + toggleCalibMag=false; + sendRequestMSP(requestMSP(MSP_MAG_CALIBRATION)); + } + + //****************************************************************************************** + if ((toggleWriteServo || toggleWriteServoLive )&& !toggleRead) { // MSP_SET_SERVO_CONF + toggleWriteServo=false; + payload = new ArrayList(); + if (multiType == AIRPLANE || multiType == PPM_TO_SERVO || multiType == HELI_120_CCPM || multiType == HELI_90_DEG){ + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + if ((int)Bbox.getArrayValue()[i]==1){ servoRATE[i] = abs(servoRATE[i]);}else{ servoRATE[i] = abs(servoRATE[i])*-1;}// Direction + + if(i==5 && ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG) )servoRATE[5] =(int)Bbox.getArrayValue()[5]; // Yaw servo Direction + if( multiType == HELI_120_CCPM ){ + if(i==3 ){servoRATE[3] = (int)Mbox.getArrayValue()[2]+(int)Mbox.getArrayValue()[1]*2+(int)Mbox.getArrayValue()[0]*4;} + if(i==4 ){servoRATE[4] = (int)Mbox.getArrayValue()[5]+(int)Mbox.getArrayValue()[4]*2+(int)Mbox.getArrayValue()[3]*4;} + if(i==6 ){servoRATE[6] = (int)Mbox.getArrayValue()[8]+(int)Mbox.getArrayValue()[7]*2+(int)Mbox.getArrayValue()[6]*4;} + //bbb + } + payload.add(char(servoRATE[i])); // servoRATE + } + } + //****************************************************************************************** + else{ + for( i=0;i<8;i++){ servoRATE[i]=round(RateSlider[i].value()); payload.add(char(servoRATE[i]));}// servoRATE + for( i=0;i<8;i++) { int q= (int)(ServoSliderC[i].value())+512; payload.add(char (q % 256) ); payload.add(char (q / 256) ); } // Servo centers... + for( i=0;i<8;i++){ if ((int)Bbox.getArrayValue()[i]==1){ payload.add(char(11)); }else{ payload.add(char(9));} } // Direction + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); + toggleWriteServoLive=false; + toggleWaitHeli=true; + } + //**************************************************************************************** + if (toggleWriteWing || toggleWriteWingLive){ // MSP_SET_SERVO_CONF + toggleWriteWing=false; toggleWriteWingLive=false; + if (multiType == TRI || multiType == FLYING_WING || multiType == BI || multiType == DUALCOPTER || multiType == SINGLECOPTER) { // TRI & Flying Wing + int nBoxes; + payload = new ArrayList(); + if (multiType == TRI) {nBoxes = 1;}else{nBoxes = 4;} + + if (multiType == FLYING_WING){ + servoRATE[3] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + } + if (multiType == SINGLECOPTER){ + servoRATE[3] = (int)Wbox.getArrayValue()[0]+(int)Wbox.getArrayValue()[1]*2; + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[3]*2; + servoRATE[5] = (int)Wbox.getArrayValue()[4]+(int)Wbox.getArrayValue()[5]*2; + servoRATE[6] = (int)Wbox.getArrayValue()[6]+(int)Wbox.getArrayValue()[7]*2; + RateSlider[3].setValue((int)servoRATE[3]); + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + RateSlider[6].setValue((int)servoRATE[6]); + } + if (multiType == DUALCOPTER){ + servoRATE[4] = (int)Wbox.getArrayValue()[0]; + servoRATE[5] = (int)Wbox.getArrayValue()[1]; + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + if(multiType == TRI){ + RateSlider[5].setValue((int)Wbox.getArrayValue()[0]); + } + if (multiType == BI){ + servoRATE[4] = (int)Wbox.getArrayValue()[2]+(int)Wbox.getArrayValue()[0]*2; // L servo + servoRATE[5] = (int)Wbox.getArrayValue()[3]+(int)Wbox.getArrayValue()[1]*2; // R servo + RateSlider[4].setValue((int)servoRATE[4]); + RateSlider[5].setValue((int)servoRATE[5]); + } + + for( i=0;i<8;i++){ + int q= (int)ServoSliderMIN[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoSliderMAX[i].value(); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)(ServoSliderC[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + + servoRATE[i] = int(RateSlider[i].value()); + + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + if(toggleGimbal || toggleTrigger){ // MSP_SET_SERVO_CONF + payload = new ArrayList(); + + ServoMIN[0]=(int)GimbalSlider[0].value(); ServoMIN[1]=(int)GimbalSlider[4].value(); ServoMIN[2]=(int)GimbalSlider[8].value(); + ServoMAX[0]=(int)GimbalSlider[1].value(); ServoMAX[1]=(int)GimbalSlider[5].value(); ServoMAX[2]=(int)GimbalSlider[9].value(); + ServoMID[0]=(int)GimbalSlider[2].value(); ServoMID[1]=(int)GimbalSlider[6].value(); ServoMID[2]=(int)GimbalSlider[10].value(); + servoRATE[0]=(int)GimbalSlider[3].value();servoRATE[1]=(int)GimbalSlider[7].value();servoRATE[2]=(int)GimbalSlider[11].value(); + + for( i=0;i<8;i++) { + int q; + q= (int)ServoMIN[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Min + q= (int)ServoMAX[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Max + q= (int)ServoMID[i]; payload.add(char (q % 256) ); payload.add(char (q / 256) ); // Servo centers + payload.add(char(servoRATE[i])); // servoRATE + } + sendRequestMSP(requestMSP(MSP_SET_SERVO_CONF,payload.toArray( new Character[payload.size()]) )); // Send settings + } + } + + if(gimbal && gimbalConfig ){ + if((int)GimbalSlider[11].value()==1) { GimbalSlider[11].setCaptionLabel("Trig_Reversed"); }else{ GimbalSlider[11].setCaptionLabel("Trig_Normal");} + int ValLow; + for( i=2;i<11;i+=4){ + if(i > 8){ValLow=1001;}else{ValLow=1201;} + if (GimbalSlider[i].value() < ValLow ) {GimbalSlider[i].setMin(0).setMax(12);} + switch((int)GimbalSlider[i].value()) { + case 0: GimbalSlider[i].setCaptionLabel("ROLL") ;break; + case 1: GimbalSlider[i].setCaptionLabel("NICK") ;break; + case 2: GimbalSlider[i].setCaptionLabel("YAW") ;break; + case 3: GimbalSlider[i].setCaptionLabel("THRO") ;break; + case 4: GimbalSlider[i].setCaptionLabel("AUX1") ;break; + case 5: GimbalSlider[i].setCaptionLabel("AUX2") ;break; + case 6: GimbalSlider[i].setCaptionLabel("AUX3") ;break; + case 7: GimbalSlider[i].setCaptionLabel("AUX4") ;break; + case 8: GimbalSlider[i].setCaptionLabel("AUX5") ;break; + case 9: GimbalSlider[i].setCaptionLabel("AUX6") ;break; + case 10:GimbalSlider[i].setCaptionLabel("AUX7") ;break; + case 11:GimbalSlider[i].setCaptionLabel("AUX8") ;break; + default:GimbalSlider[i].setCaptionLabel("MID") ; + } + if (GimbalSlider[i].value() == 12 ){ + GimbalSlider[i].setMin(ValLow-1).setMax(2000); + if (i==10){GimbalSlider[i].setMin(ValLow-1).setMax(30000);} + GimbalSlider[i].setValue(ValLow+1); + } + } + } + + if(ServosActive){ + int BreakPoint =Centerlimits[0]+1; + for( i=0;i<8;i++){ + if (ServoSliderC[i].value() < BreakPoint ) {ServoSliderC[i].setMin(0).setMax(12);} + switch((int)ServoSliderC[i].value()) { + case 0: ServoSliderC[i].setCaptionLabel("ROLL") ;break; + case 1: ServoSliderC[i].setCaptionLabel("NICK") ;break; + case 2: ServoSliderC[i].setCaptionLabel("YAW") ;break; + case 3: ServoSliderC[i].setCaptionLabel("THRO") ;break; + case 4: ServoSliderC[i].setCaptionLabel("AUX1") ;break; + case 5: ServoSliderC[i].setCaptionLabel("AUX2") ;break; + case 6: ServoSliderC[i].setCaptionLabel("AUX3") ;break; + case 7: ServoSliderC[i].setCaptionLabel("AUX4") ;break; + case 8: ServoSliderC[i].setCaptionLabel("AUX5") ;break; + case 9: ServoSliderC[i].setCaptionLabel("AUX6") ;break; + case 10:ServoSliderC[i].setCaptionLabel("AUX7") ;break; + case 11:ServoSliderC[i].setCaptionLabel("AUX8") ;break; + default:ServoSliderC[i].setCaptionLabel("MID") ; + } + if (ServoSliderC[i].value() == 12 ){ + ServoSliderC[i].setMin(BreakPoint-1).setMax(Centerlimits[1]); + ServoSliderC[i].setValue(BreakPoint+1); + } + } + } + + + if (toggleWing || toggleServo || toggleGimbal || toggleTrigger){ + buttonLIVE.show(); + SaveSERVO.show(); + ServosActive=true; + } else{ + buttonLIVE.hide(); + SaveSERVO.hide(); + buttonExport.hide(); + toggleLive=false; + ServosActive=false;} + + //****************************************************************************************** + + if (toggleWrite) { + toggleWrite=false; + + // MSP_SET_RC_TUNING + payload = new ArrayList(); + payload.add(char( round(confRC_RATE.value()*100)) ); + payload.add(char( round(confRC_EXPO.value()*100)) ); + payload.add(char( round(rollPitchRate.value()*100)) ); + payload.add(char( round(yawRate.value()*100)) ); + payload.add(char( round(dynamic_THR_PID.value()*100)) ); + payload.add(char( round(throttle_MID.value()*100)) ); + payload.add(char( round(throttle_EXPO.value()*100)) ); + sendRequestMSP(requestMSP(MSP_SET_RC_TUNING,payload.toArray( new Character[payload.size()]) )); + + // MSP_SET_PID + payload = new ArrayList(); + for(i=0;i(); + for(i=0;i(); + + intPowerTrigger = (round(confPowerTrigger.value())); + payload.add(char(intPowerTrigger % 256)); payload.add(char(intPowerTrigger / 256)); //a + + + // ThrVal minthrottle,maxthrottle,mincommand,FSthrottle b,c,d,e + for( i=0;i<4;i++) {int q= (int)(confINF[i].value()); payload.add(char (q % 256) ); payload.add(char (q / 256) ); } + + // PermanentLog + int nn= round(confINF[4].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8));// f + + + nn= round(confINF[5].value()); + payload.add(char (nn - ((nn>>8)<<8))); payload.add(char (nn>>8)); // g 32b + payload.add(char (nn - ((nn>>16)<<16))); payload.add(char (nn>>16)); + + + // MagDec + nn= round(confINF[6].value()*10); payload.add(char (nn - ((nn>>8)<<8) )); payload.add(char (nn>>8)); // h + + + // VBatscale + nn= round(VBat[0].value()); payload.add(char (nn)); // i + for( i=1;i<4;i++) { int q= int(VBat[i].value()*10); payload.add(char (q)); } // j,k,l + + sendRequestMSP(requestMSP(MSP_SET_MISC,payload.toArray(new Character[payload.size()]))); + + + // MSP_EEPROM_WRITE + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); + + updateModel(); // update model with view value + } + + if (toggleRXbind) { + toggleRXbind=false; + sendRequestMSP(requestMSP(MSP_BIND)); + bSTOP(); + InitSerial(9999); + } + + while (g_serial.available()>0) { + c = (g_serial.read()); + + if (c_state == IDLE) { + c_state = (c=='$') ? HEADER_START : IDLE; + } else if (c_state == HEADER_START) { + c_state = (c=='M') ? HEADER_M : IDLE; + } else if (c_state == HEADER_M) { + if (c == '>') { + c_state = HEADER_ARROW; + } else if (c == '!') { + c_state = HEADER_ERR; + } else { + c_state = IDLE; + } + } else if (c_state == HEADER_ARROW || c_state == HEADER_ERR) { + /* is this an error message? */ + err_rcvd = (c_state == HEADER_ERR); /* now we are expecting the payload size */ + dataSize = (c&0xFF); + /* reset index variables */ + p = 0; + offset = 0; + checksum = 0; + checksum ^= (c&0xFF); + /* the command is to follow */ + c_state = HEADER_SIZE; + } else if (c_state == HEADER_SIZE) { + cmd = (byte)(c&0xFF); + checksum ^= (c&0xFF); + c_state = HEADER_CMD; + } else if (c_state == HEADER_CMD && offset < dataSize) { + checksum ^= (c&0xFF); + inBuf[offset++] = (byte)(c&0xFF); + } else if (c_state == HEADER_CMD && offset >= dataSize) { + /* compare calculated and transferred checksum */ + if ((checksum&0xFF) == (c&0xFF)) { + if (err_rcvd) { + //System.err.println("Copter did not understand request type "+c); + } else { + /* we got a valid response packet, evaluate it */ + evaluateCommand(cmd, (int)dataSize); + } + } else { + System.out.println("invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF)); + System.out.print("<"+(cmd&0xFF)+" "+(dataSize&0xFF)+"> {"); + for (i=0; i2)ServoSliderC[i].show(); + buttonSERVO.setLabel("SERVO"); + + if( multiType == PPM_TO_SERVO){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();RateSlider[i].hide();} + + if( multiType == HELI_120_CCPM){ + if(i>2) {ServoSliderMIN[i].show();ServoSliderMAX[i].show();TxtMin.show();TxtMax.show();} + } + if( multiType == HELI_90_DEG ){ + if(i<3) { + ServoSliderC[i].show();ServoSliderMIN[i].show(); + ServoSliderMAX[i].show();TxtMin.show();TxtMax.show(); + } + ServoSliderMIN[5].show(); ServoSliderMAX[5].show(); + } + + } else { + if(i<5)BtAUX[i].hide(); + if(flaps)TxtAux.hide(); + + Bbox.hide(); +// SaveSERVO.hide(); + TxtRates.hide(); + TxtMids.hide(); + TxtRev.hide(); + RateSlider[i].hide(); + ServoSliderC[i].hide(); + BtServo[i].hide(); + checkboxRev[i].hide(); + ServoSliderMIN[i].hide();ServoSliderMAX[i].hide(); + buttonSERVO.setLabel("SERVO"); + if ( multiType == HELI_120_CCPM || multiType == HELI_90_DEG){TxtMin.hide();TxtMax.hide(); + } + } + } + } + + + stroke(255); + a=radians(angx); + if (angy<-90) {b=radians(-180 - angy);} + else if (angy>90) {b=radians(+180 - angy);} + else{ b=radians(angy);} + h=radians(head); + + // --------------------------------------------------------------------------------------------- + // DRAW MULTICOPTER TYPE + // --------------------------------------------------------------------------------------------- + float size = 38.0; + // object + fill(255,255,255); + pushMatrix(); + camera(xObj,yObj,300/tan(PI*60.0/360.0),xObj/2+30,yObj/2-40,0,0,1,0); + translate(xObj,yObj); + directionalLight(200,200,200, 0, 0, -1); + rotateZ(h);rotateX(b);rotateY(a); + stroke(150,255,150); + strokeWeight(0);sphere(size/3);strokeWeight(3); + line(0,0, 10,0,-size-5,10);line(0,-size-5,10,+size/4,-size/2,10); line(0,-size-5,10,-size/4,-size/2,10); + stroke(255); + int MotToggleMove=200; + + textFont(font12); + if (toggleGimbal || toggleTrigger) { //if (multiType == GIMBAL) + noLights();text("GIMBAL", -20,-55);camera();popMatrix(); + text("GIMBAL", xMot,yMot+25); + + servoSliderH[1].setPosition(xMot,yMot+75).setCaptionLabel("ROLL") .show(); + servoSliderH[0].setPosition(xMot,yMot+35).setCaptionLabel("PITCH").show(); + if(camTrigger)servoSliderH[2].setPosition(xMot,yMot+120).setCaptionLabel("Trigg").show(); + + } else if (multiType == TRI) { //TRI + drawMotor( 0, +size, byteMP[0], 'L'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, -size, byteMP[2], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text(" TRICOPTER", -40,-50);camera();popMatrix(); + TxtRightW .hide(); + motSlider[0].setPosition(xMot+50,yMot+15).setHeight(100).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(100).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-15).setHeight(100).setCaptionLabel("LEFT").show(); + if(InitServos ){ + InitServos=false; + } + servoSliderH[5].setPosition(xMot,yMot+135).setCaptionLabel("SERVO " ).show(); + if(toggleWing){ + int Step=yServ+10; + ServoSliderC[5] .show().setPosition(xServ+100 ,Step+60);//.setCaptionLabel("Center" +yawServo); + ServoSliderMIN[5].show().setPosition(xServ+100 ,Step+80).setCaptionLabel("Min"); + ServoSliderMAX[5].show().setPosition(xServ+180 ,Step+80).setCaptionLabel("Max"); + } + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+55).setCaptionLabel("REAR").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).setCaptionLabel("RIGHT").show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-15).setCaptionLabel("LEFT").show(); + } else if (multiType == QUADP) { //QUAD+ + drawMotor(0, +size, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + drawMotor(-size, 0, byteMP[2], 'L'); + drawMotor(0, -size, byteMP[3], 'R'); + line(-size,0, +size,0);line(0,-size, 0,+size); + noLights();text("QUADRICOPTER +", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+75).setHeight(60).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot+35).setHeight(60).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot+35).setHeight(60).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot-15).setHeight(60).setCaptionLabel("FRONT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+35).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot+35).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot-15).show(); + + + } else if (multiType == QUADX) { //QUAD X + drawMotor(+size, +size, byteMP[0], 'R'); + drawMotor(+size, -size, byteMP[1], 'L'); + drawMotor(-size, +size, byteMP[2], 'L'); + drawMotor(-size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(-size,+size, 0,0);line(+size,+size, 0,0); + noLights();text("QUADRICOPTER X", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+75).setHeight(60).setCaptionLabel("REAR_R") .show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+10,yMot+75).setHeight(60).setCaptionLabel("REAR_L") .show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + } else if (multiType == BI) { //BI + drawMotor(-size, 0, byteMP[0], 'R'); + drawMotor(+size, 0, byteMP[1], 'L'); + line(0-size,0, 0,0); line(0+size,0, 0,0);line(0,size*1.5, 0,0); + noLights();text("BICOPTER", -30,-20);camera();popMatrix(); + + motSlider[0].setPosition(xMot,yMot+30).setHeight(55).setCaptionLabel("").show(); + motSlider[1].setPosition(xMot+100,yMot+30).setHeight(55).setCaptionLabel("").show(); + servoSliderH[4].setPosition(xMot,yMot+100).setWidth(60).setCaptionLabel("").show(); + servoSliderH[5].setPosition(xMot+80,yMot+100).setWidth(60).setCaptionLabel("").show(); + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + + if(toggleWing){ + int Step=yServ+10; + int ServoN =4; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+0+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=20; + ServoN =5; + ServoSliderC[ServoN] .setPosition(xServ+100 ,Step+80+60).show().setCaptionLabel(" Center"); + ServoSliderMIN[ServoN].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[ServoN].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + }else if (multiType == Y6) { //Y6 + drawMotor( +7+0, +7+size, byteMP[0], 'L'); + drawMotor( +7+size, +7-size, byteMP[1], 'R'); + drawMotor( +7-size, +7-size, byteMP[2], 'R'); + translate(0,0,-7); + drawMotor( -7+0, -7+size, byteMP[3], 'R'); + drawMotor( -7+size, -7-size, byteMP[4], 'L'); + drawMotor( -7-size, -7-size, byteMP[5], 'L'); + line(-size,-size,0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + noLights();text("TRICOPTER Y6", -40,-55);camera();popMatrix(); + + motSlider[0].setPosition(xMot+50,yMot+23).setHeight(50).setCaptionLabel("REAR").show(); + motSlider[1].setPosition(xMot+100,yMot-18).setHeight(50).setCaptionLabel("RIGHT").show(); + motSlider[2].setPosition(xMot,yMot-18).setHeight(50).setCaptionLabel("LEFT").show(); + motSlider[3].setPosition(xMot+50,yMot+87).setHeight(50).setCaptionLabel("U_REAR").show(); + motSlider[4].setPosition(xMot+100,yMot+48).setHeight(50).setCaptionLabel("U_RIGHT").show(); + motSlider[5].setPosition(xMot,yMot+48).setHeight(50).setCaptionLabel("U_LEFT").show(); + + motToggle[0].setPosition(xMot+50-MotToggleMove,yMot+48).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-18).show(); + motToggle[2].setPosition(xMot-MotToggleMove,yMot-18).show(); + motToggle[3].setPosition(xMot+50-MotToggleMove,yMot+87).show(); + motToggle[4].setPosition(xMot+100-MotToggleMove,yMot+23).show(); + motToggle[5].setPosition(xMot-MotToggleMove,yMot+23).show(); + } else if (multiType == HEX6) { //HEX6 + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + drawMotor(+size, -0.55*size, byteMP[1], 'R'); + drawMotor(-size, +0.55*size, byteMP[2], 'L'); + drawMotor(-size, -0.55*size, byteMP[3], 'R'); + drawMotor( 0, -size, byteMP[4], 'L'); + drawMotor( 0, +size, byteMP[5], 'R'); + line(-size,-0.55*size,0,0);line(size,-0.55*size,0,0);line(-size,+0.55*size,0,0);line(size,+0.55*size,0,0);line(0,+size,0,0);line(0,-size,0,0); + noLights();text("HEXACOPTER", -40,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+90,yMot+65).setHeight(50).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+90,yMot-5).setHeight(50) .setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+5,yMot+65) .setHeight(50).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+5,yMot-5 ) .setHeight(50).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+50,yMot-20).setHeight(50).setCaptionLabel("FRONT").show(); + motSlider[5].setPosition(xMot+50,yMot+90).setHeight(50).setCaptionLabel("REAR").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-5) .show(); + motToggle[2].setPosition(xMot+5-MotToggleMove,yMot+65) .show(); + motToggle[3].setPosition(xMot+5-MotToggleMove,yMot-5 ) .show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-20).show(); + motToggle[5].setPosition(xMot+50-MotToggleMove,yMot+90).show(); + } else if (multiType == FLYING_WING) { //FLYING_WING + line(0,0, 1.8*size,size);line(1.8*size,size,1.8*size,size-30); line(1.8*size,size-30,0,-1.5*size); + line(0,0, -1.8*size,+size);line(-1.8*size,size,-1.8*size,+size-30); line(-1.8*size,size-30,0,-1.5*size); + noLights();text("FLYING WING", -40,-50);camera();popMatrix(); + + servoSliderV[3].setPosition(xMot+5,yMot+10).setCaptionLabel("LEFT").show(); + servoSliderV[4].setPosition(xMot+100,yMot+10).setCaptionLabel("RIGHT").show(); + motSlider[0].setPosition(xMot+50,yMot+30).setHeight(90).setCaptionLabel("Mot").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[3].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center"); + ServoSliderMIN[3].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[3].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center"); + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + } else if (multiType == Y4) { //Y4 + drawMotor( +15+0, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-size,-size, 0,0);line(+size,-size, 0,0);line(0,+size, 0,0); + translate(0,0,-7); + drawMotor( -15+0, +size, byteMP[2], 'L'); + noLights();text("Y4", -5,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+75).setHeight(60).setCaptionLabel("REAR_1").show(); + motSlider[1].setPosition(xMot+90,yMot-15).setHeight(60).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+30,yMot+75).setHeight(60).setCaptionLabel("REAR_2").show(); + motSlider[3].setPosition(xMot+10,yMot-15).setHeight(60).setCaptionLabel("FRONT_L").show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + + } else if (multiType == 18) { //HEX6 H + drawMotor(+size, +1.2*size, byteMP[0], 'R'); + drawMotor(+size, -1.2*size, byteMP[1], 'L'); + drawMotor(-size, +1.2*size, byteMP[2], 'L'); + drawMotor(-size, -1.2*size, byteMP[3], 'R'); + drawMotor(-size, 0, byteMP[4], 'L'); + drawMotor(+size, 0, byteMP[5], 'R'); + + line(+size, +1.2*size,+size, -1.2*size);line(-size, +1.2*size,-size, -1.2*size); + line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER H", -45,-60);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+90-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+ 5-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+ 5-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot+5 -MotToggleMove,yMot+35).show(); + } else if (multiType == HEX6X) { //HEX6 X + drawMotor(+0.55*size, +size, byteMP[0], 'L'); + drawMotor(+0.55*size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'R'); + drawMotor(-0.55*size, -size, byteMP[3], 'R'); + drawMotor( +size, 0, byteMP[4], 'R'); + drawMotor( -size, 0, byteMP[5], 'L'); + + line(-0.55*size,-size,0,0);line(-0.55*size,size,0,0);line(+0.55*size,-size,0,0);line(+0.55*size,size,0,0);line(+size,0,0,0); line(-size,0,0,0); + noLights();text("HEXACOPTER X", -45,-50);camera();popMatrix(); + + motSlider[0].setPosition(xMot+80,yMot+90).setHeight(45).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+80,yMot-20).setHeight(45).setCaptionLabel("FRONT_R").show(); + motSlider[2].setPosition(xMot+25,yMot+90).setHeight(45).setCaptionLabel("REAR_L").show(); + motSlider[3].setPosition(xMot+25,yMot-20).setHeight(45).setCaptionLabel("FRONT_L").show(); + motSlider[4].setPosition(xMot+90,yMot+35).setHeight(45).setCaptionLabel("RIGHT").show(); + motSlider[5].setPosition(xMot+5,yMot+35) .setHeight(45).setCaptionLabel("LEFT").show(); + + motToggle[0].setPosition(xMot+80-MotToggleMove,yMot+90).show(); + motToggle[1].setPosition(xMot+80-MotToggleMove,yMot-20).show(); + motToggle[2].setPosition(xMot+25-MotToggleMove,yMot+90).show(); + motToggle[3].setPosition(xMot+25-MotToggleMove,yMot-20).show(); + motToggle[4].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[5].setPosition(xMot-5 -MotToggleMove,yMot+35).show(); + } else if (multiType == OCTOX8 ) { //OCTOX8 + motToggle[0].setPosition(xMot+110-MotToggleMove,yMot+65).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-45).show(); + motToggle[2].setPosition(xMot-10-MotToggleMove,yMot+65).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-45).show(); + motToggle[4].setPosition(xMot+90-MotToggleMove,yMot+95).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot-15).show(); + motToggle[6].setPosition(xMot+10-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot-15).show(); + + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATX) { //OCTOXP + // GUI is the same for all 8 motor configs. multiType 12-13 + motToggle[0].setPosition(xMot+10-MotToggleMove,yMot-15).show(); + motToggle[1].setPosition(xMot+90-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+90-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot+75).show(); + motToggle[4].setPosition(xMot+50-MotToggleMove,yMot-35).show(); + motToggle[5].setPosition(xMot+110-MotToggleMove,yMot+35).show(); + motToggle[6].setPosition(xMot+50-MotToggleMove,yMot+95).show(); + motToggle[7].setPosition(xMot-10-MotToggleMove,yMot+35).show(); + + noLights();text("OCTOCOPTER P", -45,-50);camera();popMatrix(); + } else if (multiType == OCTOFLATP) { //OCTOXX + // GUI is the same for all 8 motor configs. multiType 12-13 + + motToggle[0].setPosition(xMot+25-MotToggleMove,yMot-30) .show();//MIDFRONT_L + motToggle[1].setPosition(xMot+110-MotToggleMove,yMot+5) .show();//FRONT_R + motToggle[2].setPosition(xMot+75-MotToggleMove,yMot+85) .show();//MIDREAR_R + motToggle[3].setPosition(xMot-10-MotToggleMove,yMot+55) .show();//REAR_L + motToggle[4].setPosition(xMot-10-MotToggleMove,yMot+5) .show();//FRONT_L + motToggle[5].setPosition(xMot+75-MotToggleMove,yMot-30) .show();//MIDFRONT_R + motToggle[6].setPosition(xMot+110-MotToggleMove,yMot+55).show();//REAR_R + motToggle[7].setPosition(xMot+25-MotToggleMove,yMot+85) .show();//MIDREAR_L + + /* Test with mororSliders on OCTO X + motSlider[0].setPosition(xMot+25,yMot-30) .setCaptionLabel("").show();//MIDFRONT_L + motSlider[1].setPosition(xMot+110,yMot+5) .setCaptionLabel("").show();//FRONT_R + motSlider[2].setPosition(xMot+75,yMot+55) .setCaptionLabel("").show();//MIDREAR_R + motSlider[3].setPosition(xMot-10,yMot+25) .setCaptionLabel("").show();//REAR_L + motSlider[4].setPosition(xMot-10,yMot+5) .setCaptionLabel("").show();//FRONT_L + motSlider[5].setPosition(xMot+75,yMot-30) .setCaptionLabel("").show();//MIDFRONT_R + motSlider[6].setPosition(xMot+110,yMot+25).setCaptionLabel("").show();//REAR_R + motSlider[7].setPosition(xMot+25,yMot+55) .setCaptionLabel("").show();//MIDREAR_L + */ + noLights();text("OCTOCOPTER X", -45,-50);camera();popMatrix(); + } else if (multiType == AIRPLANE) { //AIRPLANE + float Span = size*1.3; + float VingRoot = Span*0.25; + // Wing + line(0,0, Span,0); line(Span,0, Span, VingRoot); line(Span, VingRoot, 0,VingRoot); + line(0,0, -Span,0); line(-Span,0, -Span, VingRoot); line(-Span, VingRoot, 0,VingRoot); + // Stab + line(-(size*0.4),size, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fin + line(0,size-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + noLights(); + textFont(font12); + text("AIRPLANE", -40,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Wing 1").show(); + servoSliderH[4].setPosition(xMot,yMot+25) .setCaptionLabel("Wing 2").show(); + servoSliderH[5].setPosition(xMot,yMot+55) .setCaptionLabel("Rudd").show(); + servoSliderH[6].setPosition(xMot,yMot+85) .setCaptionLabel("Elev").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Thro").show(); + TX_StickSlider[RCPitch].setCaptionLabel("Elev"); + TX_StickSlider[RCYaw ].setCaptionLabel("Rudd"); +// if(flapperons) { BtServo[0].setLabel("Flprn 1"); BtServo[1].setLabel("Flprn 2");} + if(flaps) { BtServo[2].setLabel("Flaps").setSize(60,12).setColorBackground(green_);servoSliderH[2].setPosition(xMot,yMot+130).setCaptionLabel("Flaps").show();} + if( !flaps) {for(i=0;i<3;i++) BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_);}//!flapperons && + + BtServo[0].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[1].setLabel("").setSize(90,12).setColorBackground(black_); + BtServo[3].setLabel("Wing 1"); + BtServo[4].setLabel("Wing 2"); + BtServo[5].setLabel("Rudder"); + BtServo[6].setLabel("Elev"); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_);// + ServoSliderC[7].hide();ServoSliderMIN[7].hide();ServoSliderMAX[7].hide(); + RateSlider[0].hide(); RateSlider[1].hide(); RateSlider[7].hide(); RateSlider[2].hide(); + + }else if (multiType == HELI_120_CCPM) { // 120 CCPM + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize,-5, -2,-0.5*scalesize,-5); line(0,1.5*scalesize,-5, 2,-0.5*scalesize,-5); line( -2,-0.5*scalesize,-5, 2,-0.5*scalesize,-5); + // Fin + float finpos = scalesize * 1.3; + int HFin=0; + int LFin=15; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize,-5); line(-(scalesize*0.3),scalesize+3,-5, (scalesize*0.3),scalesize+3,-5); + line(-(scalesize*0.3),scalesize,-5, -(scalesize*0.3),scalesize+3,-5); line((scalesize*0.3),scalesize,-5, (scalesize*0.3),scalesize+3,-5); + + noLights(); + textFont(font12); + text("Heli 120 CCPM", -42,-50);camera();popMatrix(); + + servoSliderV[7].setPosition(xMot,yMot) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+40,yMot-15) .setCaptionLabel("LEFT").show(); + servoSliderV[3].setPosition(xMot+70,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130) .setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+100,yMot-15).setCaptionLabel("RIGHT").show(); + + + for (i=0;i<3;i++) {ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide();} + for (i=0;i<8;i++) {RateSlider[i].hide(); checkboxRev[i].hide();} + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide(); + TxtRates.hide(); + BtServo[0].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[1].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[2].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[7].setLabel(" ").setSize(100,12).setColorBackground(black_); + BtServo[3].setLabel(" NICK").setSize(100,12); + BtServo[4].setLabel(" LEFT").setSize(100,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" RIGHT").setSize(100,12); + + } else if (multiType == HELI_90_DEG) { //Heli 90 + + for (i=0;i<3;i++) { + ServoSliderMIN[i].hide(); ServoSliderMAX[i].hide();ServoSliderC[i].hide(); + BtServo[i].setLabel("").setSize(90,12).setColorBackground(black_); + RateSlider[i].hide(); checkboxRev[i].hide();} + + RateSlider[5].hide(); + ServoSliderMIN[7].hide(); ServoSliderMAX[7].hide();ServoSliderC[7].hide();RateSlider[7].hide(); + + BtServo[3].setLabel(" NICK").setSize(60,12); + BtServo[4].setLabel(" ROLL").setSize(60,12); + BtServo[5].setLabel(" YAW"); + BtServo[6].setLabel(" Coll").setSize(60,12); + BtServo[7].setLabel("").setSize(90,12).setColorBackground(black_); + TxtRates.hide(); + + + // HeliGraphics + float scalesize=size*0.8; + // Rotor + ellipse(0, 0, 2*scalesize, 2*scalesize); + // Body + line(0,1.5*scalesize, -2,-0.5*scalesize); line(0,1.5*scalesize, 2,-0.5*scalesize); line( -2,-0.5*scalesize, 2,-0.5*scalesize); + // Fin + float finpos = scalesize * 1.3; + int HFin=5; + int LFin=10; + line(0,finpos-3,0, 0,finpos+7,-LFin); line(0,finpos+7,-LFin, 0,finpos+10,-LFin);line(0,finpos+10,-LFin, 0,finpos+5,0); + line(0,finpos-3,0, 0,finpos,HFin); line(0,finpos,HFin, 0,finpos+5,HFin);line(0,finpos+5,HFin, 0,finpos+5,0); + + // Stab + line(-(scalesize*0.3),scalesize, (scalesize*0.3),scalesize); line(-(scalesize*0.3),scalesize+3, (scalesize*0.3),scalesize+3); + line(-(scalesize*0.3),scalesize, -(scalesize*0.3),scalesize+3); line((scalesize*0.3),scalesize, (scalesize*0.3),scalesize+3); + + noLights(); + textFont(font12); + text("Heli 90", -16,-50);camera();popMatrix(); + + // Sliders + servoSliderV[7].setPosition(xMot,yMot-15) .setCaptionLabel("Thro").show(); + servoSliderV[4].setPosition(xMot+120,yMot-15).setCaptionLabel("ROLL").show(); + servoSliderV[3].setPosition(xMot+80,yMot+10) .setCaptionLabel("Nick").show(); + servoSliderH[5].setPosition(xMot+15,yMot+130).setCaptionLabel("Yaw") .show(); + servoSliderV[6].setPosition(xMot+40,yMot) .setCaptionLabel("COLL").show(); + } else if (multiType == VTAIL4) { //Vtail + drawMotor(+0.55*size, +size, byteMP[0], 'R'); + drawMotor( +size, -size, byteMP[1], 'L'); + drawMotor(-0.55*size, +size, byteMP[2], 'L'); + drawMotor( -size, -size, byteMP[3], 'R'); + line(-0.55*size,size,0,0);line(+0.55*size,size,0,0); + line(-size,-size, 0,0); line(+size,-size, 0,0); + noLights(); + textFont(font12); + text("Vtail", -10,-50);camera();popMatrix(); + motSlider[0].setPosition(xMot+80,yMot+70 ).setHeight(60).setCaptionLabel("REAR_R").show(); + motSlider[1].setPosition(xMot+100,yMot-15).setHeight(60).setCaptionLabel("RIGHT" ).show(); + motSlider[2].setPosition(xMot+25,yMot+70 ).setHeight(60).setCaptionLabel("REAR_L").show(); + drawMotor(+size, +0.55*size, byteMP[0], 'L'); + motSlider[3].setPosition(xMot+2,yMot-15 ).setHeight(60).setCaptionLabel("LEFT" ).show(); + + motToggle[0].setPosition(xMot+70-MotToggleMove,yMot+75).show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot-15).show(); + motToggle[2].setPosition(xMot+40-MotToggleMove,yMot+75).show(); + motToggle[3].setPosition(xMot+10-MotToggleMove,yMot-15).show(); +} else if(multiType == PPM_TO_SERVO) { //PPM to 8 servos + noLights(); + textFont(font12); + text("PPM to 8 servos", -40, -50); + camera(); + popMatrix(); + int ind=-5; + for (i=0;i<8;i++) {servoSliderH[i].setPosition(xMot, yMot+ind).setCaptionLabel("CH "+(i+1)).show(); + BtServo[i].setLabel(" CH "+(i+1)); + ind+=20; } + } else if (multiType == DUALCOPTER) { //Dualcopter + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("Dualcopter", -30,-50);camera();popMatrix(); + +// servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("N/A").show(); + servoSliderH[4].setPosition(xMot,yMot+55).setCaptionLabel("PITCH").show(); + servoSliderH[5].setPosition(xMot,yMot+25).setCaptionLabel("ROLL").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("M 1").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("M 0").show(); + + if(toggleWing){ + int Step=yServ; + ServoSliderC[5].show().setPosition(xServ+100 ,Step+0+60);//.setCaptionLabel(" Center") + ServoSliderMIN[5].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+0+80); + ServoSliderMAX[5].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+0+80); + Step+=10; + ServoSliderC[4].show().setPosition(xServ+100 ,Step+80+60);// .show().setCaptionLabel(" Center") + ServoSliderMIN[4].show().setCaptionLabel(" Min") .setPosition(xServ+100 ,Step+80+80); + ServoSliderMAX[4].show().setCaptionLabel(" Max") .setPosition(xServ+180 ,Step+80+80); + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else if (multiType == SINGLECOPTER) { + float Span = size*1.3; + float VingRoot = Span*0.25; + // Stab + line(0,VingRoot, (size*0.4),size); line(-(size*0.4),size+5, (size*0.4),size+5); + line(0,VingRoot, -(size*0.4),size); + line(-(size*0.4),size, -(size*0.4),size+5); line((size*0.4),size, (size*0.4),size+5); + // Body + line(-2,size, -2,-size+5); line(2,size, 2,-size+5); line( -2,-size+5, 2,-size+5); + // Fins + line(0,VingRoot-3,0, 0,size,15); line(0,size,15, 0,size+5,15);line(0,size+5,15, 0,size+5,0); + line(0,VingRoot-3,0, 0,size,-15); line(0,size,-15, 0,size+5,-15);line(0,size+5,-15, 0,size+5,0); + noLights(); + textFont(font12); + text("SINGLECOPTER", -30,-50);camera();popMatrix(); + + servoSliderH[3].setPosition(xMot,yMot-5) .setCaptionLabel("Right").show(); + servoSliderH[4].setPosition(xMot,yMot+25).setCaptionLabel("Left").show(); + servoSliderH[5].setPosition(xMot,yMot+55).setCaptionLabel("Front").show(); + servoSliderH[6].setPosition(xMot,yMot+85).setCaptionLabel("Rear").show(); + servoSliderH[7].setPosition(xMot,yMot+115).setCaptionLabel("Motor").show(); + if(toggleWing){ + for(i=3;i<7;i++){ServoSliderC[i].show();ServoSliderMIN[i].show();ServoSliderMAX[i].show();} + } + + motToggle[0].setPosition(xMot-MotToggleMove,yMot+30).setCaptionLabel("").show(); + motToggle[1].setPosition(xMot+100-MotToggleMove,yMot+30).setCaptionLabel("").show(); + } else { + noLights();camera();popMatrix(); + } + + + + // --------------------------------------------------------------------------------------------- + // Fly Level Control Instruments + // --------------------------------------------------------------------------------------------- + // info angles + fill(255,255,127); + textFont(font12); + text((int)angy + "°", xLevelObj+38, yLevelObj+78); //pitch + text((int)angx + "°", xLevelObj-62, yLevelObj+78); //roll + + pushMatrix(); + translate(xLevelObj-34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(a); + fill(255,255,127); + textFont(font12);text("ROLL", -13, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,+30,0);line(0,0,0,-10); + popMatrix(); + + pushMatrix(); + translate(xLevelObj+34,yLevelObj+112); + fill(50,50,50); + noStroke(); + ellipse(0,0,66,66); + rotate(b); + fill(255,255,127); + textFont(font12);text("PITCH", -18, 15); + strokeWeight(1.5); + stroke(127,127,127); + line(-30,1,30,1); + stroke(255,255,255); + line(-30,0,30,0);line(30,0,30-size/6 ,size/6);line(+30,0,30-size/6 ,-size/6); + popMatrix(); + + // --------------------------------------------------------------------------------------------- + // Magnetron Combi Fly Level Control + // --------------------------------------------------------------------------------------------- + horizonInstrSize=68; + angyLevelControl=((angy<-horizonInstrSize) ? -horizonInstrSize : (angy>horizonInstrSize) ? horizonInstrSize : angy); + pushMatrix(); + translate(xLevelObj,yLevelObj); + noStroke(); + // instrument background + fill(50,50,50); + ellipse(0,0,150,150); + // full instrument + rotate(-a); + rectMode(CORNER); + // outer border + strokeWeight(1); + stroke(90,90,90); + //border ext + arc(0,0,140,140,0,TWO_PI); + stroke(190,190,190); + //border int + arc(0,0,138,138,0,TWO_PI); + // inner quadrant + strokeWeight(1); + stroke(255,255,255); + fill(124,73,31); + //earth + float angle = acos(angyLevelControl/horizonInstrSize); + arc(0,0,136,136,0,TWO_PI); + fill(38,139,224); + //sky + arc(0,0,136,136,HALF_PI-angle+PI,HALF_PI+angle+PI); + float x = sin(angle)*horizonInstrSize; + if (angy>0) + fill(124,73,31); + noStroke(); + triangle(0,0,x,-angyLevelControl,-x,-angyLevelControl); + // inner lines + strokeWeight(1); + for(i=0;i<8;i++) { + j=i*15; + if (angy<=(35-j) && angy>=(-65-j)) { + stroke(255,255,255); line(-30,-15-j-angy,30,-15-j-angy); // up line + fill(255,255,255); + textFont(font9); + text("+" + (i+1) + "0", 34, -12-j-angy); // up value + text("+" + (i+1) + "0", -48, -12-j-angy); // up value + } + if (angy<=(42-j) && angy>=(-58-j)) { + stroke(167,167,167); line(-20,-7-j-angy,20,-7-j-angy); // up semi-line + } + if (angy<=(65+j) && angy>=(-35+j)) { + stroke(255,255,255); line(-30,15+j-angy,30,15+j-angy); // down line + fill(255,255,255); + textFont(font9); + text("-" + (i+1) + "0", 34, 17+j-angy); // down value + text("-" + (i+1) + "0", -48, 17+j-angy); // down value + } + if (angy<=(58+j) && angy>=(-42+j)) { + stroke(127,127,127); line(-20,7+j-angy,20,7+j-angy); // down semi-line + } + } + strokeWeight(2); + stroke(255,255,255); + if (angy<=50 && angy>=-50) { + line(-40,-angy,40,-angy); //center line + fill(255,255,255); + textFont(font9); + text("0", 34, 4-angy); // center + text("0", -39, 4-angy); // center + } + + // lateral arrows + strokeWeight(1); + // down fixed triangle + stroke(60,60,60); + fill(180,180,180,255); + + triangle(-horizonInstrSize,-8,-horizonInstrSize,8,-55,0); + triangle(horizonInstrSize,-8,horizonInstrSize,8,55,0); + + // center + strokeWeight(1); + stroke(255,0,0); + line(-20,0,-5,0); line(-5,0,-5,5); + line(5,0,20,0); line(5,0,5,5); + line(0,-5,0,5); + popMatrix(); + + + // --------------------------------------------------------------------------------------------- + // Compass Section + // --------------------------------------------------------------------------------------------- + pushMatrix(); + translate(xCompass,yCompass); + // Compass Background + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + size=29; + rect(-size*2.5,-size*2.5,size*2.5,size*2.5); + // GPS quadrant + strokeWeight(1.5); + if (GPS_update == 1) { + fill(125);stroke(125); + } else { + fill(160);stroke(160); + } + ellipse(0, 0, 4*size+7, 4*size+7); + // GPS rotating pointer + rotate(GPS_directionToHome*PI/180); + strokeWeight(4);stroke(255,255,100);line(0,0, 0,-2.4*size);line(0,-2.4*size, -5 ,-2.4*size+10); line(0,-2.4*size, +5 ,-2.4*size+10); + rotate(-GPS_directionToHome*PI/180); + // compass quadrant + strokeWeight(1.5);fill(0);stroke(0); + ellipse(0, 0, 2.6*size+7, 2.6*size+7); + // Compass rotating pointer + stroke(255); + rotate(head*PI/180); + line(0,size*0.2, 0,-size*1.3); line(0,-size*1.3, -5 ,-size*1.3+10); line(0,-size*1.3, +5 ,-size*1.3+10); + popMatrix(); + // angles + for (i=0;i<=12;i++) { + angCalc=i*PI/6; + if (i%3!=0) { + stroke(75); + line(xCompass+cos(angCalc)*size*2,yCompass+sin(angCalc)*size*2,xCompass+cos(angCalc)*size*1.6,yCompass+sin(angCalc)*size*1.6); + } else { + stroke(255); + line(xCompass+cos(angCalc)*size*2.2,yCompass+sin(angCalc)*size*2.2,xCompass+cos(angCalc)*size*1.9,yCompass+sin(angCalc)*size*1.9); + } + } + textFont(font15); + text("N", xCompass-5, yCompass-22-size*0.9);text("S", xCompass-5, yCompass+32+size*0.9); + text("W", xCompass-33-size*0.9, yCompass+6);text("E", xCompass+21+size*0.9, yCompass+6); + // head indicator + textFont(font12); + noStroke(); + fill(80,80,80,130); + rect(xCompass-22,yCompass-8,xCompass+22,yCompass+9); + fill(255,255,127); + text(head + "°",xCompass-11-(head>=10.0 ? (head>=100.0 ? 6 : 3) : 0),yCompass+6); + // GPS direction indicator + fill(255,255,0); + text(GPS_directionToHome + "°",xCompass-6-size*2.1,yCompass+7+size*2); + // GPS fix + if (GPS_fix==0) { + fill(127,0,0); + } else { + fill(0,255,0); + } + //ellipse(xCompass+3+size*2.1,yCompass+3+size*2,12,12); + rect(xCompass-28+size*2.1,yCompass+1+size*2,xCompass+9+size*2.1,yCompass+13+size*2); + textFont(font9); + if (GPS_fix==0) { + fill(255,255,0); + } else { + fill(0,50,0); + } + text("GPS_fix",xCompass-27+size*2.1,yCompass+10+size*2); + + + // --------------------------------------------------------------------------------------------- + // GRAPH + // --------------------------------------------------------------------------------------------- + strokeWeight(1); + fill(255, 255, 255); + g_graph.drawGraphBox(); + + strokeWeight(1.5); + stroke(255, 0, 0); if (axGraph) g_graph.drawLine(accROLL, -1000, +1000); + stroke(0, 255, 0); if (ayGraph) g_graph.drawLine(accPITCH, -1000, +1000); + stroke(0, 0, 255); + if (azGraph) { + if (scaleSlider.value()<2){ g_graph.drawLine(accYAW, -1000, +1000); + } else{ g_graph.drawLine(accYAW, 200*scaleSlider.value()-1000,200*scaleSlider.value()+500);} + } + + float altMin = (altData.getMinVal() + altData.getRange() / 2) - 100; + float altMax = (altData.getMaxVal() + altData.getRange() / 2) + 100; + + stroke(200, 200, 0); if (gxGraph) g_graph.drawLine(gyroROLL, -300, +300); + stroke(0, 255, 255); if (gyGraph) g_graph.drawLine(gyroPITCH, -300, +300); + stroke(255, 0, 255); if (gzGraph) g_graph.drawLine(gyroYAW, -300, +300); + stroke(125, 125, 125);if (altGraph) g_graph.drawLine(altData, altMin, altMax); + stroke(225, 225, 125);if (headGraph) g_graph.drawLine(headData, -370, +370); + stroke(50, 100, 150); if (magxGraph) g_graph.drawLine(magxData, -500, +500); + stroke(100, 50, 150); if (magyGraph) g_graph.drawLine(magyData, -500, +500); + stroke(150, 100, 50); if (magzGraph) g_graph.drawLine(magzData, -500, +500); + + stroke(200, 50, 0); if (debug1Graph) g_graph.drawLine(debug1Data, -5000, +5000); + stroke(0, 200, 50); if (debug2Graph) g_graph.drawLine(debug2Data, -5000, +5000); + stroke(50, 0, 200); if (debug3Graph) g_graph.drawLine(debug3Data, -5000, +5000); + stroke(0, 0, 0); if (debug4Graph) g_graph.drawLine(debug4Data, -5000, +5000); + + // ------------------------------------------------------------------------ + // Draw background control boxes + // ------------------------------------------------------------------------ + fill(0, 0, 0); + strokeWeight(3);stroke(0); + rectMode(CORNERS); + // motor background + rect(xMot-5,yMot-20, xMot+145, yMot+150); + // rc background + rect(xRC-5,yRC-5, xRC+145, yRC+120); + // param background + rect(xParam,yParam, xParam+555, yParam+280); +if(!hideDraw){ + int xSens1 = xParam + 80; + int ySens1 = yParam + 220; + stroke(255); + a=min(confRC_RATE.value(),1); + b=confRC_EXPO.value(); + strokeWeight(1); + line(xSens1,ySens1,xSens1,ySens1+35); + line(xSens1,ySens1+35,xSens1+70,ySens1+35); + strokeWeight(3);stroke(30,120,30); + + int lookupR[] = new int[6]; + for(i=0;i<6;i++) lookupR[i] = int( (2500+b*100*(i*i-25))*i*a*100/2500 ); + + for(i=0;i<70;i++) { + int tmp = 500/70*i; + int tmp2 = tmp/100; + int rccommand = 2*lookupR[tmp2] + 2*(tmp-tmp2*100) * (lookupR[tmp2+1]-lookupR[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens1+i,ySens1+(70-val)*3.5/7); + } + if (confRC_RATE.value()>1) { + stroke(220,100,100); + ellipse(xSens1+70, ySens1, 7, 7); + } + + a=throttle_MID.value(); + b=throttle_EXPO.value(); + + int xSens2 = xParam + 80; + int ySens2 = yParam + 180; + strokeWeight(1);stroke(255); + line(xSens2,ySens2,xSens2,ySens2+35); + line(xSens2,ySens2+35,xSens2+70,ySens2+35); + strokeWeight(3);stroke(30,100,250); + + int lookupT[] = new int[11]; + for(i=0;i<11;i++) { + int mid = int(100*a); + int expo = int(100*b); + int tmp = 10*i-mid; + int y=1; + if (tmp>0) y = 100-mid; + if (tmp<0) y = mid; + lookupT[i] = int( 10*mid + tmp*( 100-expo+tmp*tmp*expo/(y*y) )/10 ); + } + for(i=0;i<70;i++) { + int tmp = 1000/70*i; + int tmp2 = tmp/100; + int rccommand = lookupT[tmp2] + (tmp-tmp2*100) * (lookupT[tmp2+1]-lookupT[tmp2]) / 100; + val = rccommand*70/1000; + point(xSens2+i,ySens2+(70-val)*3.5/7); + } + line(xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+25,xSens2+(max(1100,RCChan[RCThro])-1100)*70/900,ySens2+35); + + fill(255); + textFont(font15); + text("P",xParam+45,yParam+15);text("I",xParam+90,yParam+15);text("D",xParam+130,yParam+15); + textFont(font8); + text("PITCH",xSens1+2,ySens1+10); + text("ROLL",xSens1+2,ySens1+20); + text("THROT",xSens2+2,ySens2+10); + textFont(font12); + text("RATE",xParam+3,yParam+232); + text("EXPO",xParam+3,yParam+249); + text("MID",xParam+3,yParam+193); + text("EXPO",xParam+3,yParam+210); + text("RATE",xParam+160,yParam+15); + text("ROLL",xParam+3,yParam+32); + text("PITCH",xParam+3,yParam+32+1*17); + text("YAW",xParam+3,yParam+32+2*17); + text("ALT",xParam+3,yParam+32+3*17); + text("Pos",xParam+3,yParam+32+4*17); + text("PosR",xParam+3,yParam+32+5*17); + text("NavR",xParam+3,yParam+32+6*17); + text("LEVEL",xParam+1,yParam+32+7*17); + text("MAG",xParam+3,yParam+32+8*17); + text("T P A",xParam+215,yParam+15); + + text("AUX1",xBox+55,yBox+5);text("AUX2",xBox+105,yBox+5);text("AUX3",xBox+165,yBox+5);text("AUX4",xBox+218,yBox+5); + textFont(font8); + text("LOW",xBox+37,yBox+15);text("MID",xBox+57,yBox+15);text("HIGH",xBox+74,yBox+15); + text("L",xBox+100,yBox+15);text("M",xBox+118,yBox+15);text("H",xBox+136,yBox+15); + text("L",xBox+154,yBox+15);text("M",xBox+174,yBox+15);text("H",xBox+194,yBox+15); + text("L",xBox+212,yBox+15);text("M",xBox+232,yBox+15);text("H",xBox+252,yBox+15); +} + pushMatrix(); + translate(0,0,0); + popMatrix(); + if (versionMisMatch == 1) {textFont(font15);fill(#000000);text("GUI vs. Arduino: Version or Buffer size mismatch",180,420); return;} +} + +void drawMotor(float x1, float y1, int mot_num, char dir) { //Code by Danal + float size = 30.0; + pushStyle(); + float d = 0; + if (dir == 'L') {d = +5; fill(254, 221, 44);} + if (dir == 'R') {d = -5; fill(256, 152, 12);} + ellipse(x1, y1, size, size); + textFont(font15); + textAlign(CENTER); + fill(0,0,0); + text(mot_num,x1,y1+5,3); + float y2 = y1-(size/2); + stroke(255,0,0); + line(x1, y2, 3, x1+d, y2-5, 3); + line(x1, y2, 3, x1+d, y2+5, 3); + line(x1, y2, 3, x1+d*2, y2, 3); + popStyle(); +} + +void ACC_ROLL(boolean theFlag) {axGraph = theFlag;} +void ACC_PITCH(boolean theFlag) {ayGraph = theFlag;} +void ACC_Z(boolean theFlag) {azGraph = theFlag;} +void GYRO_ROLL(boolean theFlag) {gxGraph = theFlag;} +void GYRO_PITCH(boolean theFlag) {gyGraph = theFlag;} +void GYRO_YAW(boolean theFlag) {gzGraph = theFlag;} +void BARO(boolean theFlag) {altGraph = theFlag;} +void HEAD(boolean theFlag) {headGraph = theFlag;} +void MAGX(boolean theFlag) {magxGraph = theFlag;} +void MAGY(boolean theFlag) {magyGraph = theFlag;} +void MAGZ(boolean theFlag) {magzGraph = theFlag;} +void DEBUG1(boolean theFlag) {debug1Graph = theFlag;} +void DEBUG2(boolean theFlag) {debug2Graph = theFlag;} +void DEBUG3(boolean theFlag) {debug3Graph = theFlag;} +void DEBUG4(boolean theFlag) {debug4Graph = theFlag;} + +String ActiveTab="default"; +public void controlEvent(ControlEvent theEvent) { + if (theEvent.isGroup()) if (theEvent.name()=="portComList") InitSerial(theEvent.group().value()); // initialize the serial port selected + if (theEvent.isGroup()) if (theEvent.name()=="baudList") GUI_BaudRate=(int)(theEvent.group().value()); // Set GUI_BaudRate to selected. + if (theEvent.isTab()) { ActiveTab= theEvent.getTab().getName(); println("Switched to: "+ActiveTab); + int tabN= +theEvent.getTab().getId(); + scaleSlider.moveTo(ActiveTab); + + if(tabN != 4) {// Don't show in Tab 4 + btnQConnect.moveTo(ActiveTab); + buttonWRITE.moveTo(ActiveTab).hide(); + buttonREAD.moveTo(ActiveTab); + buttonRESET.moveTo(ActiveTab); + } + for( i=0;i<8;i++) { + TX_StickSlider[i].moveTo(ActiveTab); + motSlider[i] .moveTo(ActiveTab); + servoSliderH[i].moveTo(ActiveTab); + servoSliderV[i].moveTo(ActiveTab); + } + motorControlSlider.moveTo(ActiveTab); + + if(!Mag_)confINF[6].hide(); + if( tabN !=3 ) { txtlblWhichcom.moveTo(ActiveTab);commListbox.moveTo(ActiveTab);} + if( tabN ==2 && gimbal && !gimbalConfig) toggleRead=true; + if( tabN !=2 ){ toggleLive=false; buttonWRITE.show();} + if( tabN ==1 ){hideDraw=false;}else{hideDraw=true;} // Hide grapics in all other tabs + if( tabN ==4 ){ + motorControlSlider.show().setValue(1000); + toggleMotor = true; + } else { + motorControlSlider.hide(); + toggleMotor = false; + } + } +} + +public void bSTART() { + if(graphEnabled == false) {return;} + graph_on=1; + toggleRead=true; + g_serial.clear(); +} + +public void bSTOP() { + graph_on=0; +} + +public void SETTING() { + toggleSetSetting = true; +} +void GIMBAL(){ + toggleGimbal = !toggleGimbal; + if(toggleGimbal){toggleServo=false;toggleWing=false;toggleTrigger=false;} +} +void TRIGGER(){ + toggleTrigger=!toggleTrigger; + if(toggleTrigger){toggleServo=false;toggleWing=false;toggleGimbal=false;} +} + +public void READ() { +// toggleLive = false ; + toggleRead = true; + toggleVbat=true; +} + +public void RESET() { + toggleReset = true; +} + +public void WRITE() { + toggleWrite = true; +} + +public void LIVE_SERVO() { + toggleLive = !toggleLive; +} + +public void SAVE_WING() { + SAVE_Servo(); +} +public void SAVE_Servo() { + sendRequestMSP(requestMSP(MSP_EEPROM_WRITE)); +} + +public void WING(){ + toggleWing = !toggleWing; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void SERVO() { + toggleServo = !toggleServo; + toggleGimbal = false; + toggleTrigger=false; + toggleRead=true; +} + +public void CALIB_ACC() {toggleCalibAcc = true;} +public void CALIB_MAG() {toggleCalibMag = true;} + +// initialize the serial port selected in the listBox +void InitSerial(float portValue) { + if (portValue < commListMax) { + String portPos = Serial.list()[int(portValue)]; + txtlblWhichcom.setValue("COM = " + shortifyPortName(portPos, 8)); + g_serial = new Serial(this, portPos, GUI_BaudRate); + SaveSerialPort(portPos); + init_com=1; + buttonSTART.setColorBackground(green_);buttonSTOP.setColorBackground(green_);buttonREAD.setColorBackground(green_); + buttonRESET.setColorBackground(green_);commListbox.setColorBackground(green_); + buttonCALIBRATE_ACC.setColorBackground(green_); buttonCALIBRATE_MAG.setColorBackground(green_); + graphEnabled = true; + g_serial.buffer(256); + btnQConnect.hide(); + } else { + txtlblWhichcom.setValue("Comm Closed"); + init_com=0; + buttonSTART.setColorBackground(red_);buttonSTOP.setColorBackground(red_);commListbox.setColorBackground(red_);buttonSETTING.setColorBackground(red_); + graphEnabled = false; + init_com=0; + g_serial.stop(); + btnQConnect.show(); + } +} + +void SaveSerialPort(String port ) { + output = createWriter(portnameFile); + output.print( port + ';' + GUI_BaudRate); // Write the comport to the file + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + } + void Eport_Servo(){ + READ(); + ExportServo=true; + } + +void SAVE_SERVO_CONFIG() { // Save a config file for servos + ExportServo=false; + output = createWriter("Servos.txt"); + String sServo[]; + output.println( "/* Defaut Servo settings exported from MultiWiiConf."); + output.println( " Place the defines in config.h"); + output.println( " The Values will default if Eeprom is reset from Gui. */\n"); + output.print( "#define SERVO_MIN {"); for( i=0;i<7;i++) { output.print( ServoMIN[i]);output.print(", "); }output.print( ServoMIN[7]);output.println("}"); + output.print( "#define SERVO_MAX {"); for( i=0;i<7;i++) { output.print( ServoMAX[i]);output.print(", "); }output.print( ServoMAX[7]);output.println("}"); + output.print( "#define SERVO_MID {"); for( i=0;i<7;i++) { output.print( ServoMID[i]);output.print(", "); }output.print( ServoMID[7]);output.println("}"); + output.print( "#define FORCE_SERVO_RATES {"); for( i=0;i<7;i++) { output.print( servoRATE[i]);output.print(", "); }output.print( servoRATE[7]);output.println("}"); + output.flush(); // Writes the remaining data to the file + output.close(); // Finishes the file + buttonExport.hide(); +} + +public void bQCONN(){ + ReadSerialPort(); + InitSerial(SerialPort); + bSTART(); + toggleRead=true; +} + +void Cau0(){ServoSliderC[2].setMin(4).setMax(10).setValue(4);CauClear(); BtAUX[0].setColorBackground(orange_);} +void Cau1(){ServoSliderC[2].setMin(4).setMax(10).setValue(5);CauClear(); BtAUX[1].setColorBackground(orange_);} +void Cau2(){ServoSliderC[2].setMin(4).setMax(10).setValue(6);CauClear(); BtAUX[2].setColorBackground(orange_);} +void Cau3(){ServoSliderC[2].setMin(4).setMax(10).setValue(7);CauClear(); BtAUX[3].setColorBackground(orange_);} +void Cau4(){ServoSliderC[2].setMin(Centerlimits[0]).setMax(Centerlimits[1]).setValue(1500);CauClear();} +void CauClear(){ for (i=0;i<4;i++) BtAUX[i].setColorBackground(red_);} + +void ReadSerialPort() { + reader = createReader(portnameFile); + String line; + try { + line = reader.readLine(); + } catch (IOException e) { + e.printStackTrace(); + line = null; } + if (line == null) { + // Stop reading because of an error or file is empty + // Roll on with no input + btnQConnect.hide(); + return; + } else { + String[] pieces = split(line, ';'); + int Port = int(pieces[0]); + GUI_BaudRate= int(pieces[1]); + if (commListMax==1){} + String pPort=pieces[0]; + for( i=0;i res) || (i==0)) res = m_data[i]; + return res; + } + float getMinVal() { + float res = 0.0; + for( i=0; i0 && i + + +Thu Mar 14 11:23:53 CET 2013 +0 +24.0 +0.045 +3.3 +0.0 +0.0 +0.0 +0.03 +3.3 +23.0 +0.0 +4.0 +0.03 +0.0 +23.0 +0.25 +9.0 +100.0 +0.9 +0.5 +0.0 +0.01 +1.4 +100.0 +0.2 +0.65 +2.0 +0.08 +0.08 +0.0 +0.11 +0.045 +0.0 +6.4 +0.0 +0.025 +0.0 +6.8 + diff --git a/Output.cpp b/Output.cpp index 439cd533..52e44e10 100644 --- a/Output.cpp +++ b/Output.cpp @@ -17,7 +17,12 @@ void initializeServo(); // since we are uing the PWM generation in a direct way, the pin order is just to inizialie the right pins // its not possible to change a PWM output pin just by changing the order #if defined(PROMINI) - uint8_t PWM_PIN[8] = {9,10,11,3,6,5,A2,12}; //for a quad+: rear,right,left,front + #if defined(Bluetooth) + uint8_t PWM_PIN[8] = {9,6,5,3,A2,12,10,11}; //use pin 6 and 5 instead of 10 and 11 - only valid for quad! + //uint8_t PWM_PIN[8] = {3, 5, 6, 9,A2,12,7,11}; //use pin 6 and 5 instead of 10 and 11 - only valid for quad! + #else + uint8_t PWM_PIN[8] = {9,10,11,3,6,5,A2,12}; //for a quad+: rear,right,left,front + #endif #endif #if defined(PROMICRO) #if !defined(HWPWM6) @@ -46,6 +51,12 @@ void initializeServo(); /*************** Software PWM & Servo variables ********************/ /**************************************************************************************/ #if defined(PROMINI) || (defined(PROMICRO) && defined(HWPWM6)) || (defined(MEGA) && defined(MEGA_HW_PWM_SERVOS)) + #if defined(Bluetooth) + volatile uint8_t atomicPWM_PIN5_lowState; + volatile uint8_t atomicPWM_PIN5_highState; + volatile uint8_t atomicPWM_PIN6_lowState; + volatile uint8_t atomicPWM_PIN6_highState; + #endif #if (NUMBER_MOTOR > 4) //for HEX Y6 and HEX6/HEX6X/HEX6H flat for promini volatile uint8_t atomicPWM_PIN5_lowState; @@ -405,6 +416,18 @@ void writeMotors() { // [1000;2000] => [125;250] /******** Specific PWM Timers & Registers for the atmega328P (Promini) ************/ #if defined(PROMINI) + + + /* + * We substract 1000 and multiply by 2 in order to get a PWM signal + * with a duty cycle from 0 to 100% + */ + motor[0] = (motor[0]-1000) << 1; + motor[1] = (motor[1]-1000) << 1; + motor[2] = (motor[2]-1000) << 1; + motor[3] = (motor[3]-1000) << 1; + + #if (NUMBER_MOTOR > 0) #ifdef EXT_MOTOR_RANGE // 490Hz OCR1A = ((motor[0]>>2) - 250); @@ -418,30 +441,42 @@ void writeMotors() { // [1000;2000] => [125;250] OCR1A = motor[0]>>3; // pin 9 #endif #endif - #if (NUMBER_MOTOR > 1) - #ifdef EXT_MOTOR_RANGE // 490Hz - OCR1B = ((motor[1]>>2) - 250); - #elif defined(EXT_MOTOR_32KHZ) - OCR1B = (motor[1] - 1000) >> 2; // pin 10 - #elif defined(EXT_MOTOR_4KHZ) - OCR1B = (motor[1] - 1000) << 1; - #elif defined(EXT_MOTOR_1KHZ) - OCR1B = (motor[1] - 1000) << 3; + #if defined(Bluetooth) + #ifndef EXT_MOTOR_RANGE + atomicPWM_PIN6_highState = motor[1]>>3; + atomicPWM_PIN5_highState = motor[2]>>3; #else - OCR1B = motor[1]>>3; // pin 10 + atomicPWM_PIN6_highState = (motor[1]>>2) - 250; + atomicPWM_PIN5_highState = (motor[2]>>2) - 250; #endif - #endif - #if (NUMBER_MOTOR > 2) - #ifdef EXT_MOTOR_RANGE // 490Hz - OCR2A = ((motor[2]>>2) - 250); - #elif defined(EXT_MOTOR_32KHZ) - OCR2A = (motor[2] - 1000) >> 2; // pin 11 - #elif defined(EXT_MOTOR_4KHZ) - OCR2A = (motor[2] - 1000) >> 2; - #elif defined(EXT_MOTOR_1KHZ) - OCR2A = (motor[2] - 1000) >> 2; - #else - OCR2A = motor[2]>>3; // pin 11 + atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState; + atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState; + #else + #if (NUMBER_MOTOR > 1) + #ifdef EXT_MOTOR_RANGE // 490Hz + OCR1B = ((motor[1]>>2) - 250); + #elif defined(EXT_MOTOR_32KHZ) + OCR1B = (motor[1] - 1000) >> 2; // pin 10 + #elif defined(EXT_MOTOR_4KHZ) + OCR1B = (motor[1] - 1000) << 1; + #elif defined(EXT_MOTOR_1KHZ) + OCR1B = (motor[1] - 1000) << 3; + #else + OCR1B = motor[1]>>3; // pin 10 + #endif + #endif + #if (NUMBER_MOTOR > 2) + #ifdef EXT_MOTOR_RANGE // 490Hz + OCR2A = ((motor[2]>>2) - 250); + #elif defined(EXT_MOTOR_32KHZ) + OCR2A = (motor[2] - 1000) >> 2; // pin 11 + #elif defined(EXT_MOTOR_4KHZ) + OCR2A = (motor[2] - 1000) >> 2; + #elif defined(EXT_MOTOR_1KHZ) + OCR2A = (motor[2] - 1000) >> 2; + #else + OCR2A = motor[2]>>3; // pin 11 + #endif #endif #endif #if (NUMBER_MOTOR > 3) @@ -652,11 +687,15 @@ void initOutput() { #if (NUMBER_MOTOR > 0) TCCR1A |= _BV(COM1A1); // connect pin 9 to timer 1 channel A #endif - #if (NUMBER_MOTOR > 1) - TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B - #endif - #if (NUMBER_MOTOR > 2) - TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A + #if defined(Bluetooth) + initializeSoftPWM(); // use pin 6,5 instead of 10,11 for nbluetoothL01 receiver + #else + #if (NUMBER_MOTOR > 1) + TCCR1A |= _BV(COM1B1); // connect pin 10 to timer 1 channel B + #endif + #if (NUMBER_MOTOR > 2) + TCCR2A |= _BV(COM2A1); // connect pin 11 to timer 2 channel A + #endif #endif #if (NUMBER_MOTOR > 3) TCCR2A |= _BV(COM2B1); // connect pin 3 to timer 2 channel B @@ -977,8 +1016,9 @@ void initializeServo() { /************ Motor software PWM generation ******************/ /**************************************************************************************/ // SW PWM is only used if there are not enough HW PWM pins (for exampe hexa on a promini) +// It will also be used for bluetooth software serial on promini -#if (NUMBER_MOTOR > 4) && (defined(PROMINI) || defined(PROMICRO)) +#if defined(Bluetooth) || ((NUMBER_MOTOR > 4) && (defined(PROMINI) || defined(PROMICRO))) /**************** Pre define the used ISR's and Timerchannels ******************/ #if !defined(PROMICRO) @@ -1000,10 +1040,10 @@ void initializeServo() { void initializeSoftPWM(void) { #if !defined(PROMICRO) TCCR0A = 0; // normal counting mode - #if (NUMBER_MOTOR > 4) && !defined(HWPWM6) + #if defined(Bluetooth) || ((NUMBER_MOTOR > 4) && !defined(HWPWM6)) TIMSK0 |= (1< 6) || ((NUMBER_MOTOR == 6) && !defined(SERVO)) + #if defined(Bluetooth) || (NUMBER_MOTOR > 6) || ((NUMBER_MOTOR == 6) && !defined(SERVO)) TIMSK0 |= (1< takes longer to return throttle to normal. Must be >=1; /* tail precomp from collective */ - #define YAW_COLL_PRECOMP 10 // (*) proportional factor in 0.1. Higher value -> higher precomp effect. value of 10 equals no/neutral effect - #define YAW_COLL_PRECOMP_DEADBAND 120 // (*) deadband for collective pitch input signal around 0-pitch input value + //#define YAW_COLL_PRECOMP 15 // (*) proportional factor in 0.1. Higher value -> higher precomp effect. value of 10 equals no/neutral effect + //#define YAW_COLL_PRECOMP_DEADBAND 120 // (*) deadband for collective pitch input signal around 0-pitch input value //#define VOLTAGEDROP_COMPENSATION // voltage impact correction @@ -365,7 +372,6 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Robe/Hitec/Futaba //#define SERIAL_SUM_PPM ROLL,PITCH,YAW,THROTTLE,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For Multiplex //#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //For some Hitec/Sanwa/Others - //#define SERIAL_SUM_PPM THROTTLE,YAW,ROLL,PITCH,AUX1,AUX2,AUX3,AUX4,8,9,10,11 //Modelcraft // Uncommenting following line allow to connect PPM_SUM receiver to standard THROTTLE PIN on MEGA boards (eg. A8 in CRIUS AIO) //#define PPM_ON_THROTTLE @@ -397,13 +403,16 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //#define SBUS PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4,8,9,10,11,12,13,14,15,16,17 // dsm2 orangerx //#define SBUS ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4,8,9,10,11,12,13,14,15,16,17 // T14SG //#define RX_SERIAL_PORT 1 - #define SBUS_MID_OFFSET 988 //SBUS Mid-Point at 1500 + //#define SBUS_MID_OFFSET 988 //SBUS Mid-Point at 1500 /******************************* HOTT RECIVER ************************************/ /* Graupner Hott HD */ //#define SUMD PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4 //#define RX_SERIAL_PORT 1 + /******************************* bluetooth ****************************************/ + #define Bluetooth + /*************************************************************************************************/ /***************** ***************/ /**************** SECTION 4 - ALTERNATE CPUs & BOARDS *******/ @@ -685,7 +694,7 @@ At this moment you can use this function only with WinGUI 2.3 release. MultiWiiC //#define MTK_BINARY16 //#define MTK_BINARY19 //#define INIT_MTK_GPS // initialize MTK GPS for using selected speed, 5Hz update rate and GGA & RMC sentence or binary settings - //#define VENUS8 + /* I2C GPS device made with an independant arduino + GPS device including some navigation functions @@ -789,7 +798,6 @@ Also note, that maqgnetic declination changes with time, so recheck your value e //#define LCD_TTY // SERIAL LCD: useful to tweak parameters over cable with arduino IDE 'serial monitor' //#define LCD_ETPP // I2C LCD: Eagle Tree Power Panel LCD, which is i2c (not serial) //#define LCD_LCD03 // I2C LCD: LCD03, which is i2c - //#define LCD_LCD03S // SERIAL LCD: LCD03 whit serial 9600 baud comunication enabled. //#define OLED_I2C_128x64 // I2C LCD: OLED http://www.multiwii.com/forum/viewtopic.php?f=7&t=1350 //#define OLED_DIGOLE // I2C OLED from http://www.digole.com/index.php?productID=550 @@ -877,33 +885,6 @@ Also note, that maqgnetic declination changes with time, so recheck your value e //#define RX_RSSI_PIN A3 //#define RX_RSSI_CHAN 8 //RSSI injection on selected channel (for PPM, Olrs, SBUS, etc.) (Starts at 0) - /********************************************************************/ - /**** TELEMETRY ****/ - /********************************************************************/ - // select one of the two protocols depending on your receiver - //#define FRSKY_TELEMETRY // used for FRSKY twoway receivers with telemetry (D-series like D8R-II or D8R-XP) - // VBAT, Baro, MAG, GPS and POWERMETER are helpful - // VBAT_CELLS is optional for a forth screen on the display FLD-02 - //#define SPORT_TELEMETRY // for FRSKY twoway receivers with S.PORT telemetry (S-series like X4R/X6R/X8R), not implemented yet - TO BE DONE - - // FRSKY common entries - valid for both protocols - #define TELEMETRY_SERIAL 3 // change if required - - // FRSKY standard telemetry specific devices - #define FRSKY_FLD02 // send only data specific for the FRSKY display FLD-02 - //#define OPENTX // send OpenTX specific data - - // FRSKY standard telemetry specific selections - //#define COORDFORMAT_DECIMALMINUTES // uncomment to get the format DD°MM.mmmm for the coordinates - comment out to get the format DD.dddddd° for the coordinates - //#define KILOMETER_HOUR // send speed in kilometers per hour instead of knots (default) - requested by OPENTX - #define TELEMETRY_ALT_BARO // send BARO based altitude, calibrated to 0 when arming, recommended if BARO available - //#define TELEMETRY_ALT_GPS // send GPS based altitude (altitude above see level), for FLD-02 don't use together with TELEMETRY_ALT_BARO - #define TELEMETRY_COURSE_MAG // send MAG based course/heading, recommended if MAG available, but FLD-02 does not display - //#define TELEMETRY_COURSE_GPS // send GPS based course/heading, don't use together with TELEMETRY_COURSE_MAG, FLD-02 does not display - - // S.PORT specific entries - #define FRSKY_SPORT_A2_MAX 124 // A2 voltage is represented by a value in the range 0-255. A value of 16 results in 1.6V, 124 is 12.4V, etc - /********************************************************************/ /**** Buzzer ****/ /********************************************************************/ @@ -1020,8 +1001,6 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /***************** ***************/ /*************************************************************************************************/ - #define VBAT_PRESCALER 16 // set this to 8 if vbatscale would exceed 255 - /**************************************************************************************/ /******** special ESC with extended range [0-2000] microseconds ********************/ /**************************************************************************************/ @@ -1046,7 +1025,7 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /**************************************************************************************/ /* motors will not spin when the throttle command is in low position this is an alternative method to stop immediately the motors */ - //#define MOTOR_STOP + #define MOTOR_STOP /* some radios have not a neutral point centered on 1500. can be changed here */ #define MIDRC 1500 @@ -1228,4 +1207,3 @@ Also note, that maqgnetic declination changes with time, so recheck your value e /*************************************************************************************************/ #endif /* CONFIG_H_ */ - diff --git a/def.h b/def.h index 04a93dcb..1bc62809 100644 --- a/def.h +++ b/def.h @@ -125,12 +125,6 @@ #define VBAT_CELLS_PINS {A0, A1, A2 } #define VBAT_CELLS_OFFSETS {0, 50, 83 } #define VBAT_CELLS_DIVS { 75, 122, 98 } -#elif COPTERTEST == 10 - #define Y6 - #define CRIUS_AIO_PRO - #define LCD_LCD03S - #define SERIAL0_COM_SPEED 9600 - #define LCD_CONF #elif defined(COPTERTEST) #error "*** this test is not yet defined" #endif @@ -276,14 +270,14 @@ /************************** atmega328P (Promini) ************************************/ #if defined(PROMINI) #if !defined(MONGOOSE1_0) - #define LEDPIN_PINMODE DDRB |= 1<<5; // Arduino pin 13 + #define LEDPIN_PINMODE pinMode (13, OUTPUT); #define LEDPIN_TOGGLE PINB |= 1<<5; //switch LEDPIN state (digital PIN 13) #define LEDPIN_OFF PORTB &= ~(1<<5); #define LEDPIN_ON PORTB |= (1<<5); #endif #if !defined(RCAUXPIN8) #if !defined(MONGOOSE1_0) - #define BUZZERPIN_PINMODE DDRB |= 1; // Arduino pin 8 + #define BUZZERPIN_PINMODE pinMode (8, OUTPUT); #if NUMBER_MOTOR >4 #undef PILOTLAMP #endif @@ -302,7 +296,7 @@ #define RCAUXPIN #endif #if !defined(RCAUXPIN12) && !defined(DISABLE_POWER_PIN) - #define POWERPIN_PINMODE DDRB |= 1<<4; // Arduino pin 12 + #define POWERPIN_PINMODE pinMode (12, OUTPUT); #define POWERPIN_ON PORTB |= 1<<4; #define POWERPIN_OFF PORTB &= ~(1<<4); //switch OFF WMP, digital PIN 12 #else @@ -316,7 +310,7 @@ #define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL) #define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5); #if !defined(MONGOOSE1_0) - #define PINMODE_LCD DDRD |= 1; // Arduino pin 0 + #define PINMODE_LCD pinMode(0, OUTPUT); #define LCDPIN_OFF PORTD &= ~1; //switch OFF digital PIN 0 #define LCDPIN_ON PORTD |= 1; #define STABLEPIN_PINMODE ; @@ -361,30 +355,30 @@ #define SOFT_PWM_4_PIN_HIGH PORTB |= 1<<4; #define SOFT_PWM_4_PIN_LOW PORTB &= ~(1<<4); - #define SERVO_1_PINMODE DDRC |= 1<<0; // pin A0 // TILT_PITCH - WING left + #define SERVO_1_PINMODE pinMode(A0,OUTPUT); // TILT_PITCH - WING left #define SERVO_1_PIN_HIGH PORTC |= 1<<0; #define SERVO_1_PIN_LOW PORTC &= ~(1<<0); - #define SERVO_2_PINMODE DDRC |= 1<<1; // pin A1 // TILT_ROLL - WING right + #define SERVO_2_PINMODE pinMode(A1,OUTPUT); // TILT_ROLL - WING right #define SERVO_2_PIN_HIGH PORTC |= 1<<1; #define SERVO_2_PIN_LOW PORTC &= ~(1<<1); - #define SERVO_3_PINMODE DDRC |= 1<<2; // pin A2 // CAM TRIG - alt TILT_PITCH + #define SERVO_3_PINMODE pinMode(A2,OUTPUT); // CAM TRIG - alt TILT_PITCH #define SERVO_3_PIN_HIGH PORTC |= 1<<2; #define SERVO_3_PIN_LOW PORTC &= ~(1<<2); #if !defined(MONGOOSE1_0) - #define SERVO_4_PINMODE DDRB |= 1<<4; // pin 12 // new - alt TILT_ROLL + #define SERVO_4_PINMODE pinMode(12,OUTPUT); // new - alt TILT_ROLL #define SERVO_4_PIN_HIGH PORTB |= 1<<4; #define SERVO_4_PIN_LOW PORTB &= ~(1<<4); #endif - #define SERVO_5_PINMODE DDRB |= 1<<3; // pin 11 // BI LEFT + #define SERVO_5_PINMODE pinMode(11,OUTPUT); // BI LEFT #define SERVO_5_PIN_HIGH PORTB |= 1<<3; #define SERVO_5_PIN_LOW PORTB &= ~(1<<3); - #define SERVO_6_PINMODE DDRD|= 1<<3; // pin 3 // TRI REAR - BI RIGHT + #define SERVO_6_PINMODE pinMode(3,OUTPUT); // TRI REAR - BI RIGHT #define SERVO_6_PIN_HIGH PORTD|= 1<<3; #define SERVO_6_PIN_LOW PORTD &= ~(1<<3); - #define SERVO_7_PINMODE DDRB |= 1<<2; // pin 10 // new + #define SERVO_7_PINMODE pinMode(10,OUTPUT); // new #define SERVO_7_PIN_HIGH PORTB |= 1<<2; #define SERVO_7_PIN_LOW PORTB &= ~(1<<2); - #define SERVO_8_PINMODE DDRB |= 1<<1; // pin 9 // new + #define SERVO_8_PINMODE pinMode(9,OUTPUT); // new #define SERVO_8_PIN_HIGH PORTB |= 1<<1; #define SERVO_8_PIN_LOW PORTB &= ~(1<<1); #endif @@ -555,21 +549,21 @@ /************************** all the Mega types ***********************************/ #if defined(MEGA) - #define LEDPIN_PINMODE DDRB |= (1<<7); DDRC |= (1<<7); // Arduino pin 13, pin 30 + #define LEDPIN_PINMODE pinMode (13, OUTPUT);pinMode (30, OUTPUT); #define LEDPIN_TOGGLE PINB |= (1<<7); PINC |= (1<<7); #define LEDPIN_ON PORTB |= (1<<7); PORTC |= (1<<7); #define LEDPIN_OFF PORTB &= ~(1<<7);PORTC &= ~(1<<7); - #define BUZZERPIN_PINMODE DDRC |= (1<<5); // Arduino pin 32 + #define BUZZERPIN_PINMODE pinMode (32, OUTPUT); #if defined PILOTLAMP - #define PL_PIN_ON PORTC |= 1<<5; - #define PL_PIN_OFF PORTC &= ~(1<<5); + #define PL_PIN_ON PORTC |= 1<<5; + #define PL_PIN_OFF PORTC &= ~(1<<5); #else #define BUZZERPIN_ON PORTC |= 1<<5; #define BUZZERPIN_OFF PORTC &= ~(1<<5); #endif #if !defined(DISABLE_POWER_PIN) - #define POWERPIN_PINMODE DDRC |= 1<<0; // Arduino pin 37 + #define POWERPIN_PINMODE pinMode (37, OUTPUT); #define POWERPIN_ON PORTC |= 1<<0; #define POWERPIN_OFF PORTC &= ~(1<<0); #else @@ -579,10 +573,10 @@ #endif #define I2C_PULLUPS_ENABLE PORTD |= 1<<0; PORTD |= 1<<1; // PIN 20&21 (SDA&SCL) #define I2C_PULLUPS_DISABLE PORTD &= ~(1<<0); PORTD &= ~(1<<1); - #define PINMODE_LCD DDRE |= 1<<0; // Arduino pin 0 + #define PINMODE_LCD pinMode(0, OUTPUT); #define LCDPIN_OFF PORTE &= ~1; //switch OFF digital PIN 0 #define LCDPIN_ON PORTE |= 1; - #define STABLEPIN_PINMODE DDRC |= 1<<6; // Arduino pin 31 + #define STABLEPIN_PINMODE pinMode (31, OUTPUT); #define STABLEPIN_ON PORTC |= 1<<6; #define STABLEPIN_OFF PORTC &= ~(1<<6); #if defined(PPM_ON_THROTTLE) @@ -613,29 +607,29 @@ #define RX_PC_INTERRUPT PCINT2_vect #define RX_PCINT_PIN_PORT PINK - #define SERVO_1_PINMODE DDRC |= 1<<3;DDRL |= 1<<5; // Arduino pin 34 , pin 44 // TILT_PITCH - WING left + #define SERVO_1_PINMODE pinMode(34,OUTPUT);pinMode(44,OUTPUT); // TILT_PITCH - WING left #define SERVO_1_PIN_HIGH PORTC |= 1<<3;PORTL |= 1<<5; #define SERVO_1_PIN_LOW PORTC &= ~(1<<3);PORTL &= ~(1<<5); - #define SERVO_2_PINMODE DDRC |= 1<<2;DDRL |= 1<<4; // Arduino pin 35 , pin 45 // TILT_ROLL - WING right + #define SERVO_2_PINMODE pinMode(35,OUTPUT);pinMode(45,OUTPUT); // TILT_ROLL - WING right #define SERVO_2_PIN_HIGH PORTC |= 1<<2;PORTL |= 1<<4; #define SERVO_2_PIN_LOW PORTC &= ~(1<<2);PORTL &= ~(1<<4); - #define SERVO_3_PINMODE DDRC |= 1<<4;DDRL |= 1<<3; // Arduino pin 33 , pin 46 // CAM TRIG - alt TILT_PITCH + #define SERVO_3_PINMODE pinMode(33,OUTPUT); pinMode(46,OUTPUT); // CAM TRIG - alt TILT_PITCH #define SERVO_3_PIN_HIGH PORTC |= 1<<4;PORTL |= 1<<3; #define SERVO_3_PIN_LOW PORTC &= ~(1<<4);PORTL &= ~(1<<3); - #define SERVO_4_PINMODE DDRC |= 1<<0; DDRH |= 1<<4; // Arduino pin 37 , pin 7 // new - alt TILT_ROLL + #define SERVO_4_PINMODE pinMode (37, OUTPUT);pinMode(7,OUTPUT); // new - alt TILT_ROLL #define SERVO_4_PIN_HIGH PORTC |= 1<<0; PORTH |= 1<<4; #define SERVO_4_PIN_LOW PORTC &= ~(1<<0);PORTH &= ~(1<<4); - #define SERVO_5_PINMODE DDRH |= 1<<3; // Arduino pin 6 // BI LEFT + #define SERVO_5_PINMODE pinMode(6,OUTPUT); // BI LEFT #define SERVO_5_PIN_HIGH PORTH |= 1<<3; #define SERVO_5_PIN_LOW PORTH &= ~(1<<3); - #define SERVO_6_PINMODE DDRE |= 1<<4; // Arduino pin 2 // TRI REAR - BI RIGHT + #define SERVO_6_PINMODE pinMode(2,OUTPUT); // TRI REAR - BI RIGHT #define SERVO_6_PIN_HIGH PORTE |= 1<<4; #define SERVO_6_PIN_LOW PORTE &= ~(1<<4); - #define SERVO_7_PINMODE DDRE |= 1<<3; // Arduino pin 5 // new + #define SERVO_7_PINMODE pinMode(5,OUTPUT); // new #define SERVO_7_PIN_HIGH PORTE |= 1<<3; #define SERVO_7_PIN_LOW PORTE &= ~(1<<3); - #define SERVO_8_PINMODE DDRE |= 1<<5; // Arduino pin 3 // new + #define SERVO_8_PINMODE pinMode(3,OUTPUT); // new #define SERVO_8_PIN_HIGH PORTE |= 1<<5; #define SERVO_8_PIN_LOW PORTE &= ~(1<<5); #endif @@ -1021,7 +1015,7 @@ #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Y; imu.gyroADC[PITCH] = -X; imu.gyroADC[YAW] = -Z;} #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} - #define STABLEPIN_PINMODE DDRC |= (1<<2); // pin A2 + #define STABLEPIN_PINMODE pinMode (A2, OUTPUT); #define STABLEPIN_ON PORTC |= (1<<2); #define STABLEPIN_OFF PORTC &= ~(1<<2); #endif @@ -1034,37 +1028,11 @@ #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Y; imu.gyroADC[PITCH] = -X; imu.gyroADC[YAW] = -Z;} #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} - #define STABLEPIN_PINMODE DDRC |= (1<<2); // pin A2 + #define STABLEPIN_PINMODE pinMode (A2, OUTPUT); #define STABLEPIN_ON PORTC |= (1<<2); #define STABLEPIN_OFF PORTC &= ~(1<<2); #endif -#if defined(QUADRINO_NANO) - #define MPU6050 - #define MS561101BA - #define AK8975 // MPU6050 + AK8975 = MPU9150 - #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} - #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Y; imu.gyroADC[PITCH] = -X; imu.gyroADC[YAW] = -Z;} - #define MAG_ORIENTATION(Y, X, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = Z;} - - #define MPU6050_ADDRESS 0x68 - #define MS561101BA_ADDRESS 0x76 - - #define GPS_SERIAL 2 - #define VENUS8 - #define USE_MSP_WP - - #define LED1PIN_ON PORTB |= (1<<7); - #define LED1PIN_OFF PORTB &= ~(1<<7); - #define LED2PIN_ON PORTC |= (1<<7); - #define LED2PIN_OFF PORTC &= ~(1<<7); - - #undef DISABLE_POWER_PIN - #define POWERPIN_PINMODE DDRC |= 1<<1;//pinMode (36, OUTPUT); - #define POWERPIN_ON PORTC |= 1<<1; - #define POWERPIN_OFF PORTC &= ~(1<<1); -#endif - #if defined(ALLINONE) #define ITG3200 #define BMA180 @@ -1276,7 +1244,7 @@ #define GYRO_ORIENTATION(Y, X, Z) {imu.gyroADC[ROLL] = X; imu.gyroADC[PITCH] = -Y; imu.gyroADC[YAW] = -Z;} #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} #define MS561101BA_ADDRESS 0x76 - #define STABLEPIN_PINMODE DDRC |= (1<<2); // pin A2 + #define STABLEPIN_PINMODE pinMode (A2, OUTPUT); #define STABLEPIN_ON PORTC |= (1<<2); #define STABLEPIN_OFF PORTC &= ~(1<<2); #endif @@ -1290,7 +1258,7 @@ #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = Z;} #define MPU6050_I2C_AUX_MASTER #define MS561101BA_ADDRESS 0x76 - #define STABLEPIN_PINMODE DDRC |= (1<<2); // pin A2 + #define STABLEPIN_PINMODE pinMode (A2, OUTPUT); #define STABLEPIN_ON PORTC |= (1<<2); #define STABLEPIN_OFF PORTC &= ~(1<<2); #endif @@ -1819,7 +1787,7 @@ //all new Special RX's must be added here //this is to avoid confusion :) -#if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM) && !defined(SBUS) && !defined(SUMD) +#if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM) && !defined(SBUS) && !defined(SUMD) && !defined(Bluetooth) #define STANDARD_RX #endif @@ -1926,9 +1894,6 @@ #define DISPLAY_COLUMNS 16 #endif -#if defined(FRSKY_TELEMETRY) || defined(SPORT_TELEMETRY) - #define TELEMETRY -#endif /**************************************************************************************/ /*************** override defaults ********************/ @@ -1989,12 +1954,12 @@ #error "NUMBER_MOTOR is not set, most likely you have not defined any type of multicopter" #endif -#if (defined(LCD_DUMMY) || defined(LCD_SERIAL3W) || defined(LCD_TEXTSTAR) || defined(LCD_VT100) || defined(LCD_TTY) || defined(LCD_ETPP) || defined(LCD_LCD03) || defined(LCD_LCD03S) || defined(OLED_I2C_128x64) ) || defined(OLED_DIGOLE) +#if (defined(LCD_DUMMY) || defined(LCD_SERIAL3W) || defined(LCD_TEXTSTAR) || defined(LCD_VT100) || defined(LCD_TTY) || defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) ) || defined(OLED_DIGOLE) #define HAS_LCD #endif #if (defined(LCD_CONF) || defined(LCD_TELEMETRY)) && !(defined(HAS_LCD) ) - #error "LCD_CONF or LCD_TELEMETRY defined, and choice of LCD not defined. Uncomment one of LCD_SERIAL3W, LCD_TEXTSTAR, LCD_VT100, LCD_TTY or LCD_ETPP, LCD_LCD03, LCD_LCD03S, OLED_I2C_128x64, OLED_DIGOLE" + #error "LCD_CONF or LCD_TELEMETRY defined, and choice of LCD not defined. Uncomment one of LCD_SERIAL3W, LCD_TEXTSTAR, LCD_VT100, LCD_TTY or LCD_ETPP, LCD_LCD03, OLED_I2C_128x64, OLED_DIGOLE" #endif #if defined(POWERMETER_SOFT) && !(defined(VBAT)) @@ -2017,7 +1982,7 @@ #error "for your protection: A32U4_4_HW_PWM_SERVOS was not tested with your coptertype" #endif -#if GPS && !defined(NMEA) && !defined(UBLOX) && !defined(MTK_BINARY16) && !defined(MTK_BINARY19) && !defined(INIT_MTK_GPS) && !defined(I2C_GPS) && !defined(VENUS8) +#if GPS && !defined(NMEA) && !defined(UBLOX) && !defined(MTK_BINARY16) && !defined(MTK_BINARY19) && !defined(INIT_MTK_GPS) && !defined(I2C_GPS) #error "when using GPS you must specify the protocol NMEA, UBLOX..." #endif @@ -2029,4 +1994,13 @@ #error "you use one feature that is no longer supported or has undergone a name change" #endif -#endif /* DEF_H_ */ +#ifdef Bluetooth + #if (!defined(PROMINI)) + #error "Currently nbluetooth RX is only supported for ATmega328P" + #endif + #if (!defined(QUADX) && !defined(QUADP) && !defined(Y4) && !defined(TRI)) + #error "Currently bluetooth RX is only supported for QUADX, QUADP, Y4 or TRI configurations" + #endif +#endif + +#endif /* DEF_H_ */ diff --git a/types.h b/types.h old mode 100644 new mode 100755 index b7a5b3d9..419d3e93 --- a/types.h +++ b/types.h @@ -135,7 +135,7 @@ typedef struct { uint8_t VARIO_MODE :1; #endif uint8_t GPS_mode: 2; // 0-3 NONE,HOLD, HOME, NAV (see GPS_MODE_* defines -#if BARO || GPS +#if BARO uint8_t THROTTLE_IGNORED : 1; // If it is 1 then ignore throttle stick movements in baro mode; #endif #if GPS