diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..88e0877b61 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +.metadata/ +Tools/ArdupilotMegaPlanner/bin/Release/logs/ +config.mk +ArduCopter/Debug/ diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 03bf89b01e..023e0c42fe 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -1,4 +1,6 @@ -// Example config file. Take a look at confi.h. Any term define there can be overridden by defining it here. +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here. // GPS is auto-selected @@ -46,10 +48,15 @@ CH6_TRAVERSE_SPEED */ +# define CH7_OPTION DO_SET_HOVER + /* + DO_SET_HOVER + DO_FLIP + SIMPLE_MODE_CONTROL + */ + // See the config.h and defines.h files for how to set this up! // -// lets use SIMPLE mode for Roll and Pitch during Alt Hold -#define ALT_HOLD_RP ROLL_PITCH_SIMPLE // lets use Manual throttle during Loiter //#define LOITER_THR THROTTLE_MANUAL diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 541a1ec586..dacae2afbe 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,5 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#define THISFIRMWARE "ArduCopter V2.0.43 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -127,9 +128,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1; APM_BMP085_Class barometer; AP_Compass_HMC5843 compass(Parameters::k_param_compass); - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -201,6 +199,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #endif // normal dcm AP_DCM dcm(&imu, g_gps); + + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; + #endif #endif //////////////////////////////////////////////////////////////////////////////// @@ -237,7 +239,6 @@ static const char *comma = ","; static const char* flight_mode_strings[] = { "STABILIZE", "ACRO", - "SIMPLE", "ALT_HOLD", "AUTO", "GUIDED", @@ -275,6 +276,7 @@ static byte control_mode = STABILIZE; static byte old_control_mode = STABILIZE; static byte oldSwitchPosition; // for remembering the control mode switch static int motor_out[8]; +static bool do_simple = false; // Heli // ---- @@ -316,11 +318,13 @@ static bool did_ground_start = false; // have we ground started after first ar // --------------------- static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +//static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static bool xtrack_enabled = false; -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static bool xtrack_enabled = false; +//static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target + static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static long circle_angle = 0; static byte wp_control; // used to control - navgation or loiter @@ -336,7 +340,8 @@ static float cos_pitch_x = 1; static float cos_yaw_x = 1; static float sin_pitch_y, sin_yaw_y, sin_roll_y; static long initial_simple_bearing; // used for Simple mode - +static float simple_sin_y, simple_cos_x; +static float boost; // used to give a little extra to maintain altitude // Acro #if CH7_OPTION == DO_FLIP @@ -349,7 +354,7 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 +//static long bearing_error; // deg * 100 : 0 to 36000 static long altitude_error; // meters * 100 we are off in altitude static long old_altitude; static long yaw_error; // how off are we pointed @@ -465,33 +470,24 @@ static struct Location next_command; // command preloaded static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location -static struct Location optflow_offset; // optical flow base position static boolean new_location; // flag to tell us if location has been updated - // IMU variables // ------------- static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - // Performance monitoring // ---------------------- static long perf_mon_timer; //static float imu_health; // Metric based on accel gain deweighting static int G_Dt_max; // Max main loop cycle time in milliseconds static int gps_fix_count; - -// GCS -// --- -//static char GCS_buffer[53]; -//static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed +static byte gps_watchdog; // System Timers // -------------- static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static int mainLoop_count; static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops @@ -502,7 +498,6 @@ static uint8_t delta_ms_fiftyhz; static byte slow_loopCounter; static int superslow_loopCounter; -static byte alt_timer; // for limiting the execution of flight mode thingys static byte simple_timer; // for limiting the execution of flight mode thingys @@ -513,6 +508,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS static byte counter_one_herz; static bool GPS_enabled = false; static byte loop_step; +static bool new_radio_frame; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -531,17 +527,11 @@ void loop() //PORTK |= B00010000; delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); - //load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - mainLoop_count++; - - //if (delta_ms_fast_loop > 6) - // Log_Write_Performance(); // Execute the fast loop // --------------------- fast_loop(); - fast_loopTimeStamp = millis(); } //PORTK &= B11101111; @@ -566,15 +556,12 @@ void loop() } if (millis() - perf_mon_timer > 20000) { - if (mainLoop_count != 0) { + gcs.send_message(MSG_PERF_REPORT); - gcs.send_message(MSG_PERF_REPORT); + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - } + resetPerfData(); } //PORTK &= B10111111; } @@ -608,7 +595,6 @@ static void fast_loop() // --------------------------------------- update_yaw_mode(); update_roll_pitch_mode(); - update_throttle_mode(); // write out the servo PWM values // ------------------------------ @@ -628,6 +614,18 @@ static void medium_loop() medium_loopCounter++; + #ifdef OPTFLOW_ENABLED + if(g.optflow_enabled){ + optflow.read(); + optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + + // write to log + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } + } + #endif + if(GPS_enabled){ update_GPS(); } @@ -644,6 +642,10 @@ static void medium_loop() // auto_trim, uses an auto_level algorithm auto_trim(); + + // record throttle output + // ------------------------------ + throttle_integrator += g.rc_3.servo_out; break; // This case performs some navigation computations @@ -652,13 +654,6 @@ static void medium_loop() loop_step = 2; medium_loopCounter++; - // hack to stop navigation in Simple mode - if (control_mode == SIMPLE){ - // clear GPS data - g_gps->new_data = false; - break; - } - // Auto control modes: if(g_gps->new_data && g_gps->fix){ loop_step = 11; @@ -674,8 +669,8 @@ static void medium_loop() // ------------------------------------------------------ navigate(); - // control mode specific updates to nav_bearing - // -------------------------------------------- + // control mode specific updates + // ----------------------------- update_navigation(); if (g.log_bitmask & MASK_LOG_NTUN) @@ -694,17 +689,14 @@ static void medium_loop() // -------------------------- update_altitude(); - // altitude smoothing - // ------------------ - //calc_altitude_smoothing_error(); - - altitude_error = get_altitude_error(); - - //camera_stabilization(); - // invalidate the throttle hold value // ---------------------------------- invalid_throttle = true; + + // calc boost + // ---------- + boost = get_angle_boost(); + break; // This case deals with sending high rate telemetry @@ -720,11 +712,13 @@ static void medium_loop() } #if HIL_MODE != HIL_MODE_ATTITUDE - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) - Log_Write_Attitude(); + if(motor_armed){ + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) + Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_CTUN && motor_armed) - Log_Write_Control_Tuning(); + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + } #endif #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK @@ -783,9 +777,9 @@ static void medium_loop() // --------------------------- static void fifty_hz_loop() { - // record throttle output - // ------------------------------ - throttle_integrator += g.rc_3.servo_out; + // moved to slower loop + // -------------------- + update_throttle_mode(); // Read Sonar // ---------- @@ -798,19 +792,8 @@ static void fifty_hz_loop() hil.send_message(MSG_RADIO_OUT); #endif - // use Yaw to find our bearing error - calc_bearing_error(); - - //if (throttle_slew < 0) - // throttle_slew++; - //else if (throttle_slew > 0) - // throttle_slew--; - camera_stabilization(); - - //throttle_slew = min((800 - g.rc_3.control_in), throttle_slew); - # if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); @@ -885,10 +868,10 @@ static void slow_loop() #if AUTO_RESET_LOITER == 1 if(control_mode == LOITER){ - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ + //if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ // reset LOITER to current position //next_WP = current_loc; - } + //} } #endif @@ -918,11 +901,6 @@ static void slow_loop() if(g.radio_tuning > 0) tuning(); - // filter out the baro offset. - //if(baro_alt_offset > 0) baro_alt_offset--; - //if(baro_alt_offset < 0) baro_alt_offset++; - - #if MOTOR_LEDS == 1 update_motor_leds(); #endif @@ -960,8 +938,16 @@ static void update_GPS(void) //current_loc.lat = -1224318000; // Lat * 10 * *7 //current_loc.alt = 100; // alt * 10 * *7 //return; + if(gps_watchdog < 10){ + gps_watchdog++; + }else{ + // we have lost GPS signal for a moment. Reduce our error to avoid flyaways + nav_roll >>= 1; + nav_pitch >>= 1; + } if (g_gps->new_data && g_gps->fix) { + gps_watchdog = 0; // XXX We should be sending GPS data off one of the regular loops so that we send // no-GPS-fix data too @@ -1008,85 +994,6 @@ static void update_GPS(void) } } -#ifdef OPTFLOW_ENABLED -// blend gps and optical flow location -void update_location(void) -{ - //static int count = 0; - // get GPS position - if(GPS_enabled){ - update_GPS(); - } - - if( g.optflow_enabled ) { - int32_t temp_lat, temp_lng, diff_lat, diff_lng; - - // get optical flow position - optflow.read(); - optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt); - - // write to log - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); - } - - temp_lat = optflow_offset.lat + optflow.lat; - temp_lng = optflow_offset.lng + optflow.lng; - - // if we have good GPS values, don't let optical flow position stray too far - if( GPS_enabled && g_gps->fix ) { - // ensure current location is within 3m of gps location - diff_lat = g_gps->latitude - temp_lat; - diff_lng = g_gps->longitude - temp_lng; - if( diff_lat > 300 ) { - optflow_offset.lat += diff_lat - 300; - //Serial.println("lat inc!"); - } - if( diff_lat < -300 ) { - optflow_offset.lat += diff_lat + 300; - //Serial.println("lat dec!"); - } - if( diff_lng > 300 ) { - optflow_offset.lng += diff_lng - 300; - //Serial.println("lng inc!"); - } - if( diff_lng < -300 ) { - optflow_offset.lng += diff_lng + 300; - //Serial.println("lng dec!"); - } - } - - // update the current position - current_loc.lat = optflow_offset.lat + optflow.lat; - current_loc.lng = optflow_offset.lng + optflow.lng; - - /*count++; - if( count >= 20 ) { - count = 0; - Serial.println(); - Serial.print("lat:"); - Serial.print(current_loc.lat); - Serial.print("\tlng:"); - Serial.print(current_loc.lng); - Serial.print("\tr:"); - Serial.print(nav_roll); - Serial.print("\tp:"); - Serial.print(nav_pitch); - Serial.println(); - }*/ - - // indicate we have a new position for nav functions - new_location = true; - - }else{ - // get current position from gps - current_loc.lng = g_gps->longitude; // Lon * 10 * *7 - current_loc.lat = g_gps->latitude; // Lat * 10 * *7 - - new_location = g_gps->new_data; - } -} -#endif void update_yaw_mode(void) { @@ -1098,8 +1005,11 @@ void update_yaw_mode(void) case YAW_HOLD: // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); - //Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw); + if (control_mode <= STABILIZE){ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + }else{ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); + } break; case YAW_LOOK_AT_HOME: @@ -1134,6 +1044,32 @@ void update_roll_pitch_mode(void) int control_roll = 0, control_pitch = 0; + //read_radio(); + + if(do_simple && new_radio_frame){ + new_radio_frame = false; + simple_timer++; + + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + + if (simple_timer == 1){ + // roll + simple_cos_x = sin(radians(90 - delta)); + + }else if (simple_timer > 2){ + // pitch + simple_sin_y = cos(radians(90 - delta)); + simple_timer = 0; + } + + // Rotate input by the initial bearing + control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); + + g.rc_1.control_in = control_roll; + g.rc_2.control_in = control_pitch; + } + switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: @@ -1142,76 +1078,57 @@ void update_roll_pitch_mode(void) // Pitch control g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); - return; break; case ROLL_PITCH_STABLE: - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); break; - case ROLL_PITCH_SIMPLE: - simple_timer++; - if(simple_timer > 5){ - simple_timer = 0; - - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; - - // pre-calc rotation (stored in real degrees) - // roll - float cos_x = sin(radians(90 - delta)); - // pitch - float sin_y = cos(radians(90 - delta)); - - // Rotate input by the initial bearing - // roll - nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; - // pitch - nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); - } - control_roll = nav_roll; - control_pitch = nav_pitch; - break; - case ROLL_PITCH_AUTO: // mix in user control with Nav control - control_roll = g.rc_1.control_mix(nav_roll); - control_pitch = g.rc_2.control_mix(nav_pitch); + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); break; } - // Roll control - g.rc_1.servo_out = get_stabilize_roll(control_roll); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); } +// 50 hz update rate, not 250 void update_throttle_mode(void) { switch(throttle_mode){ case THROTTLE_MANUAL: - g.rc_3.servo_out = get_throttle(g.rc_3.control_in); + if (g.rc_3.control_in > 0){ + g.rc_3.servo_out = g.rc_3.control_in + boost; + }else{ + g.rc_3.servo_out = 0; + } break; case THROTTLE_HOLD: // allow interactive changing of atitude adjust_altitude(); - // fall through case THROTTLE_AUTO: + // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ + // how far off are we + altitude_error = get_altitude_error(); + // get the AP throttle nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s - // apply throttle control - g.rc_3.servo_out = get_throttle(nav_throttle); - // clear the new data flag invalid_throttle = false; } + + // apply throttle control at 200 hz + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost; break; } } @@ -1235,6 +1152,11 @@ static void update_navigation() break; case GUIDED: + wp_control = WP_MODE; + update_auto_yaw(); + update_nav_wp(); + break; + case RTL: if(wp_distance > 4){ // calculates desired Yaw @@ -1246,10 +1168,9 @@ static void update_navigation() wp_control = WP_MODE; }else{ set_mode(LOITER); - xtrack_enabled = false; + //xtrack_enabled = false; } - // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -1269,10 +1190,10 @@ static void update_navigation() update_auto_yaw(); { //circle_angle += dTnav; //1000 * (dTnav/1000); - circle_angle = wrap_360(target_bearing + 2000 + 18000); + circle_angle = wrap_360(target_bearing + 3000 + 18000); - target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle)); - target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle)); + target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100))); + target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100))); } // calc the lat and long error to the target @@ -1298,7 +1219,7 @@ static void read_AHRS(void) hil.update(); #endif - dcm.update_DCM_fast(G_Dt);//, _tog); + dcm.update_DCM_fast(); omega = dcm.get_gyro(); } @@ -1310,8 +1231,6 @@ static void update_trig(void){ yawvector.y = temp.b.x; // cos yawvector.normalize(); - cos_yaw_x = yawvector.y; // 0 x = north - sin_yaw_y = yawvector.x; // 1 y sin_pitch_y = -temp.c.x; cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x)); @@ -1319,6 +1238,9 @@ static void update_trig(void){ cos_roll_x = temp.c.z / cos_pitch_x; sin_roll_y = temp.c.y / cos_pitch_x; + cos_yaw_x = yawvector.y; // 0 x = north + sin_yaw_y = yawvector.x; // 1 y + //flat: // 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00, // 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00, @@ -1377,18 +1299,13 @@ static void update_altitude() static void adjust_altitude() { - alt_timer++; - if(alt_timer >= 2){ - alt_timer = 0; - - if(g.rc_3.control_in <= 200){ - next_WP.alt -= 1; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location - next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter - }else if (g.rc_3.control_in > 700){ - next_WP.alt += 1; // 1 meter per second - next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location - } + if(g.rc_3.control_in <= 200){ + next_WP.alt -= 1; // 1 meter per second + next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location + next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter + }else if (g.rc_3.control_in > 700){ + next_WP.alt += 1; // 1 meter per second + next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location } } @@ -1443,8 +1360,8 @@ static void tuning(){ case CH6_RELAY: g.rc_6.set_range(0,1000); - if (g.rc_6.control_in <= 600) relay_on(); - if (g.rc_6.control_in >= 400) relay_off(); + if (g.rc_6.control_in > 525) relay_on(); + if (g.rc_6.control_in < 475) relay_off(); break; case CH6_TRAVERSE_SPEED: @@ -1462,7 +1379,6 @@ static void tuning(){ static void update_nav_wp() { - // XXX Guided mode!!! if(wp_control == LOITER_MODE){ // calc a pitch to the target diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8a9512664d..ee7b041b4b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -91,7 +91,6 @@ static int get_nav_throttle(long z_error, int target_speed) { int rate_error; - int throttle; float scaler = (float)target_speed/(float)ALT_ERROR_MAX; // limit error to prevent I term run up @@ -101,11 +100,9 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - return g.throttle_cruise + throttle; + return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); } - static int get_rate_roll(long target_rate) { @@ -156,10 +153,16 @@ static void reset_hold_I(void) // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void reset_nav_I(void) +static void reset_nav(void) { + nav_throttle = 0; + invalid_throttle = true; + g.pi_nav_lat.reset_I(); g.pi_nav_lon.reset_I(); + + long_error = 0; + lat_error = 0; } @@ -169,11 +172,11 @@ throttle control // user input: // ----------- -static int get_throttle(int throttle_input) -{ - throttle_input = (float)throttle_input * angle_boost(); - return max(throttle_input, 0); -} +//static int get_throttle(int throttle_input) +//{ +// throttle_input = (float)throttle_input * angle_boost(); +// return max(throttle_input, 0); +//} static long get_nav_yaw_offset(int yaw_input, int reset) @@ -188,7 +191,7 @@ get_nav_yaw_offset(int yaw_input, int reset) // re-define nav_yaw if we have stick input if(yaw_input != 0){ // set nav_yaw + or - the current location - _yaw = (long)yaw_input + dcm.yaw_sensor; + _yaw = (long)yaw_input + dcm.yaw_sensor; // we need to wrap our value so we can be 0 to 360 (*100) return wrap_360(_yaw); @@ -210,7 +213,7 @@ static int alt_hold_velocity() } */ -static float angle_boost() +static float get_angle_boost() { float temp = cos_pitch_x * cos_roll_x; temp = 2.0 - constrain(temp, .5, 1.0); diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt new file mode 100644 index 0000000000..4e282df19b --- /dev/null +++ b/ArduCopter/CMakeLists.txt @@ -0,0 +1,161 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + + +message(STATUS "DIR: ${CMAKE_SOURCE_DIR}") + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduCopter C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME arducopter) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduCopter.pde + Attitude.pde + Camera.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + flip.pde + GCS.pde + GCS_Ardupilot.pde + #GCS_IMU_output.pde + GCS_Jason_text.pde + GCS_Mavlink.pde + GCS_Standard.pde + GCS_Xplane.pde + heli.pde + HIL_Xplane.pde + leds.pde + Log.pde + motors_hexa.pde + motors_octa.pde + motors_octa_quad.pde + motors_quad.pde + motors_tri.pde + motors_y6.pde + navigation.pde + planner.pde + radio.pde + read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index ed49230bb5..e204d747fb 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -4,19 +4,9 @@ static void init_camera() { + // ch 6 high(right) is down. g.rc_camera_pitch.set_angle(4500); - g.rc_camera_pitch.radio_min = 1200; - g.rc_camera_pitch.radio_trim = 1500; - g.rc_camera_pitch.radio_max = 1900; - //g.rc_camera_pitch.set_reverse(1); - - // ch 6 high right is down. - - g.rc_camera_roll.set_angle(4500); - g.rc_camera_roll.radio_min = 1000; - g.rc_camera_roll.radio_trim = 1500; - g.rc_camera_roll.radio_max = 2000; g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 261b591986..9dd7ee4a32 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -51,6 +51,7 @@ GCS_MAVLINK::update(void) // receive new packets mavlink_message_t msg; mavlink_status_t status; + status.packet_rx_drop_count = 0; // process received bytes while(comm_get_available(chan)) diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde index 8f9a748e74..9ecf47ab84 100644 --- a/ArduCopter/HIL_Xplane.pde +++ b/ArduCopter/HIL_Xplane.pde @@ -12,7 +12,7 @@ void HIL_XPLANE::output_HIL(void) output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)wp_distance); // 4 bytes 8,9 - output_int((int)bearing_error); // 5 bytes 10,11 + //output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)altitude_error); // 6 bytes 12, 13 output_int((int)energy_error); // 7 bytes 14,15 output_byte((int)g.waypoint_index); // 8 bytes 16 @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d67fabf2a8..98dbf41998 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv) int dump_log_start; int dump_log_end; - //byte last_log_num = get_num_logs(); - //Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); - // check that the requested log number can be read dump_log = argv[1].i; @@ -365,16 +362,15 @@ static void Log_Write_GPS() DataFlash.WriteByte(LOG_GPS_MSG); DataFlash.WriteLong(g_gps->time); // 1 - DataFlash.WriteByte(g_gps->fix); // 2 - DataFlash.WriteByte(g_gps->num_sats); // 3 + DataFlash.WriteByte(g_gps->num_sats); // 2 - DataFlash.WriteLong(current_loc.lat); // 4 - DataFlash.WriteLong(current_loc.lng); // 5 - DataFlash.WriteLong(current_loc.alt); // 7 + DataFlash.WriteLong(current_loc.lat); // 3 + DataFlash.WriteLong(current_loc.lng); // 4 + DataFlash.WriteLong(current_loc.alt); // 5 DataFlash.WriteLong(g_gps->altitude); // 6 - DataFlash.WriteInt(g_gps->ground_speed); // 8 - DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9 + DataFlash.WriteInt(g_gps->ground_speed); // 7 + DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8 DataFlash.WriteByte(END_BYTE); } @@ -382,21 +378,20 @@ static void Log_Write_GPS() // Read a GPS packet static void Log_Read_GPS() { - Serial.printf_P(PSTR("GPS, %ld, %d, %d, " + Serial.printf_P(PSTR("GPS, %ld, %d, " "%4.7f, %4.7f, %4.4f, %4.4f, " "%d, %u\n"), DataFlash.ReadLong(), // 1 time - (int)DataFlash.ReadByte(), // 2 fix - (int)DataFlash.ReadByte(), // 3 sats + (int)DataFlash.ReadByte(), // 2 sats - (float)DataFlash.ReadLong() / t7, // 4 lat - (float)DataFlash.ReadLong() / t7, // 5 lon - (float)DataFlash.ReadLong() / 100.0, // 6 gps alt - (float)DataFlash.ReadLong() / 100.0, // 7 sensor alt + (float)DataFlash.ReadLong() / t7, // 3 lat + (float)DataFlash.ReadLong() / t7, // 4 lon + (float)DataFlash.ReadLong() / 100.0, // 5 gps alt + (float)DataFlash.ReadLong() / 100.0, // 6 sensor alt - DataFlash.ReadInt(), // 8 ground speed - (uint16_t)DataFlash.ReadInt()); // 9 ground course + DataFlash.ReadInt(), // 7 ground speed + (uint16_t)DataFlash.ReadInt()); // 8 ground course } @@ -508,13 +503,13 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); DataFlash.WriteByte(END_BYTE); } #endif -// Read an attitude packet + static void Log_Read_Optflow() { Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), @@ -537,9 +532,8 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteInt((int)wp_distance); // 1 DataFlash.WriteByte(wp_verify_byte); // 2 DataFlash.WriteInt((int)(target_bearing/100)); // 3 - DataFlash.WriteInt((int)(nav_bearing/100)); // 4 - DataFlash.WriteInt((int)long_error); // 5 - DataFlash.WriteInt((int)lat_error); // 6 + DataFlash.WriteInt((int)long_error); // 4 + DataFlash.WriteInt((int)lat_error); // 5 /* @@ -564,11 +558,10 @@ static void Log_Write_Nav_Tuning() static void Log_Read_Nav_Tuning() { - Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d\n"), + Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"), DataFlash.ReadInt(), // distance DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadInt(), // target_bearing - DataFlash.ReadInt(), // nav_bearing DataFlash.ReadInt(), // long_error DataFlash.ReadInt()); // lat_error @@ -598,8 +591,6 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt()); //nav bearing */ - - } @@ -611,23 +602,18 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - // Control - //DataFlash.WriteInt((int)(g.rc_4.control_in/100)); - //DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); - // yaw - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); - DataFlash.WriteInt((int)(nav_yaw/100)); - DataFlash.WriteInt((int)yaw_error/100); + DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 + DataFlash.WriteInt((int)(nav_yaw/100)); //2 + DataFlash.WriteInt((int)yaw_error/100); //3 // Alt hold - DataFlash.WriteInt(g.rc_3.servo_out); - DataFlash.WriteInt(sonar_alt); // - DataFlash.WriteInt(baro_alt); // + DataFlash.WriteInt(g.rc_3.servo_out); //4 + DataFlash.WriteInt(sonar_alt); //5 + DataFlash.WriteInt(baro_alt); //6 - DataFlash.WriteInt((int)next_WP.alt); // - //DataFlash.WriteInt((int)altitude_error); // - DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); + DataFlash.WriteInt((int)next_WP.alt); //7 + DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8 DataFlash.WriteByte(END_BYTE); } @@ -637,14 +623,13 @@ static void Log_Write_Control_Tuning() static void Log_Read_Control_Tuning() { Serial.printf_P(PSTR( "CTUN, " - //"%d, %d, " "%d, %d, %d, " "%d, %d, %d, " - "%d, %d, %d\n"), + "%d, %d\n"), // Control - DataFlash.ReadInt(), - DataFlash.ReadInt(), + //DataFlash.ReadInt(), + //DataFlash.ReadInt(), // yaw DataFlash.ReadInt(), @@ -672,21 +657,18 @@ static void Log_Write_Performance() //* - DataFlash.WriteLong( millis()- perf_mon_timer); - DataFlash.WriteInt ( mainLoop_count); - DataFlash.WriteInt ( G_Dt_max); + //DataFlash.WriteLong( millis()- perf_mon_timer); + //DataFlash.WriteInt ( mainLoop_count); + DataFlash.WriteInt ( G_Dt_max); //1 - DataFlash.WriteByte( dcm.gyro_sat_count); - DataFlash.WriteByte( imu.adc_constraints); - DataFlash.WriteByte( dcm.renorm_sqrt_count); - DataFlash.WriteByte( dcm.renorm_blowup_count); - DataFlash.WriteByte( gps_fix_count); + DataFlash.WriteByte( dcm.gyro_sat_count); //2 + DataFlash.WriteByte( imu.adc_constraints); //3 + DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 + DataFlash.WriteByte( dcm.renorm_blowup_count); //5 + DataFlash.WriteByte( gps_fix_count); //6 - DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); - DataFlash.WriteLong ( throttle_integrator); - - //*/ - //PM, 20005, 3742, 10,0,0,0,0,89,1000, + DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 + DataFlash.WriteLong ( throttle_integrator); //8 DataFlash.WriteByte(END_BYTE); } @@ -694,24 +676,23 @@ static void Log_Write_Performance() // Read a performance packet static void Log_Read_Performance() { - Serial.printf_P(PSTR( "PM, %ld, %d, %d, " - "%d, %d, %d, %d, %d, " + Serial.printf_P(PSTR( "PM, %d, %d, %d, " + "%d, %d, %d, " "%d, %ld\n"), // Control - DataFlash.ReadLong(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), + //DataFlash.ReadLong(), + //DataFlash.ReadInt(), + DataFlash.ReadInt(), //1 + DataFlash.ReadByte(), //2 + DataFlash.ReadByte(), //3 - DataFlash.ReadByte(), - DataFlash.ReadByte(), + DataFlash.ReadByte(), //4 + DataFlash.ReadByte(), //5 + DataFlash.ReadByte(), //6 - DataFlash.ReadByte(), - DataFlash.ReadByte(), - DataFlash.ReadByte(), - - DataFlash.ReadInt(), - DataFlash.ReadLong()); + DataFlash.ReadInt(), //7 + DataFlash.ReadLong()); //8 } // Write a command processing packet. @@ -762,19 +743,28 @@ static void Log_Write_Attitude() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_ATTITUDE_MSG); + DataFlash.WriteInt((int)dcm.roll_sensor); DataFlash.WriteInt((int)dcm.pitch_sensor); DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); + + DataFlash.WriteInt((int)g.rc_1.servo_out); + DataFlash.WriteInt((int)g.rc_2.servo_out); + DataFlash.WriteInt((int)g.rc_4.servo_out); + DataFlash.WriteByte(END_BYTE); } // Read an attitude packet static void Log_Read_Attitude() { - Serial.printf_P(PSTR("ATT, %d, %d, %u\n"), + Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), DataFlash.ReadInt(), DataFlash.ReadInt(), - (uint16_t)DataFlash.ReadInt()); + (uint16_t)DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt()); } // Write a mode packet. Total length : 5 bytes @@ -834,8 +824,10 @@ static void Log_Read(int start_page, int end_page) case 1: if(data == HEAD_BYTE2) // Head byte 2 log_step++; - else + else{ log_step = 0; + Serial.println("."); + } break; case 2: diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 7d54f4ef80..927289dad6 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -25,14 +25,257 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +#define NOINLINE __attribute__((noinline)) + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_HOLD; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + default: + mode = control_mode + 100; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + 0, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + target_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + 0, + 0); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + motor_out[0], + motor_out[1], + motor_out[2], + motor_out[3], + motor_out[4], + motor_out[5], + motor_out[6], + motor_out[7]); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + g.rc_3.servo_out/10, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.RawPress, + barometer.RawTemp, + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + // try to send a message, return false if it won't fit in the serial tx buffer static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) { - uint64_t timeStamp = micros(); int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { // defer any messages on the telemetry port for 1 second after // bootup, to try to prevent bricking of Xbees @@ -40,298 +283,90 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } switch(id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + break; - case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - } + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; - case MSG_EXTENDED_STATUS1: - { - CHECK_PAYLOAD_SIZE(SYS_STATUS); + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; - switch(control_mode) { - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_HOLD; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - default: - mode = control_mode + 100; + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; - } + case MSG_NAV_CONTROLLER_OUTPUT: + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + break; - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - 0, - battery_voltage * 1000, - battery_remaining, - packet_drops); - break; - } + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); - break; - } + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; - case MSG_ATTITUDE: - { - //Vector3f omega = dcm.get_gyro(); - CHECK_PAYLOAD_SIZE(ATTITUDE); - mavlink_msg_attitude_send( - chan, - timeStamp, - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); - break; - } + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; - case MSG_LOCATION: - { - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - /*current_loc.alt * 10,*/ // changed to absolute altitude - g_gps->altitude, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); - break; - } + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; - case MSG_NAV_CONTROLLER_OUTPUT: - { - //if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - mavlink_msg_nav_controller_output_send( - chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - 0, - 0); - //} - break; - } +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; - case MSG_LOCAL_LOCATION: - { - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_local_position_send( - chan, - timeStamp, - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; - case MSG_GPS_RAW: - { - CHECK_PAYLOAD_SIZE(GPS_RAW); - mavlink_msg_gps_raw_send( - chan, - timeStamp, - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); - break; - } + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE - case MSG_SERVO_OUT: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - 0, - 0, - 0, - 0, - rssi); - break; - } + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; - case MSG_RADIO_IN: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - break; - } + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; - case MSG_RADIO_OUT: - { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - mavlink_msg_servo_output_raw_send( - chan, - motor_out[0], - motor_out[1], - motor_out[2], - motor_out[3], - motor_out[4], - motor_out[5], - motor_out[6], - motor_out[7]); - break; - } - - case MSG_VFR_HUD: - { - CHECK_PAYLOAD_SIZE(VFR_HUD); - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - g.rc_3.servo_out/10, - /*current_loc.alt / 100.0,*/ // changed to absolute altitude - g_gps->altitude/100.0, - climb_rate); - break; - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: - { - CHECK_PAYLOAD_SIZE(RAW_IMU); - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); - //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); - mavlink_msg_raw_imu_send( - chan, - timeStamp, - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); - break; - } - - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100.0, - (float)(barometer.Press-ground_pressure)/100.0, - (int)(barometer.Temp*10)); - break; - } - - case MSG_RAW_IMU3: - { - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); - break; - } - #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE - - case MSG_GPS_STATUS: - { - CHECK_PAYLOAD_SIZE(GPS_STATUS); - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); - break; - } - - default: - break; + default: + break; } return true; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c2680f856e..d9a5bca530 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 107; + static const uint16_t k_format_version = 108; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -78,12 +78,12 @@ public: k_param_top_bottom_ratio, k_param_optflow_enabled, k_param_input_voltage, + k_param_low_voltage, // // 160: Navigation parameters // k_param_crosstrack_entry_angle = 160, - k_param_pitch_max, k_param_RTL_altitude, // @@ -139,7 +139,7 @@ public: k_param_flight_mode4, k_param_flight_mode5, k_param_flight_mode6, - + k_param_simple_modes, // // 220: Waypoint data @@ -179,7 +179,7 @@ public: // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; - AP_Int8 serial3_baud; + AP_Int8 serial3_baud; // Crosstrack navigation @@ -213,6 +213,7 @@ public: AP_Int8 flight_mode4; AP_Int8 flight_mode5; AP_Int8 flight_mode6; + AP_Int8 simple_modes; // Radio settings // @@ -221,8 +222,6 @@ public: //AP_Var_group pwm_throttle; //AP_Var_group pwm_yaw; - AP_Int16 pitch_max; - // Misc // AP_Int16 log_bitmask; @@ -239,6 +238,7 @@ public: AP_Float top_bottom_ratio; AP_Int8 optflow_enabled; AP_Float input_voltage; + AP_Float low_voltage; #if FRAME_CONFIG == HELI_FRAME // Heli @@ -302,13 +302,14 @@ public: compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")), input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")), + low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), + loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), @@ -324,8 +325,7 @@ public: flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")), flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")), flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")), - - pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), + simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")), log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 10d5fb31f3..ff7e6d6bb7 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -20,18 +20,6 @@ static void clear_command_queue(){ next_command.id = NO_COMMAND; } -static void init_auto() -{ - //if (g.waypoint_index == g.waypoint_total) { - // Serial.println("ia_f"); - // do_RTL(); - //} - - // initialize commands - // ------------------- - init_commands(); -} - // Getters // ------- static struct Location get_command_with_index(int i) @@ -41,8 +29,6 @@ static struct Location get_command_with_index(int i) // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- if (i > g.waypoint_total) { - Serial.println("XCD"); - // we do not have a valid command to load // return a WP with a "Blank" id temp.id = CMD_BLANK; @@ -51,7 +37,6 @@ static struct Location get_command_with_index(int i) return temp; }else{ - //Serial.println("LD"); // we can load a command, we don't process it yet // read WP position long mem = (WP_START_BYTE) + (i * WP_SIZE); @@ -75,10 +60,9 @@ static struct Location get_command_with_index(int i) } // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ + //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ //temp.alt += home.alt; - } - //Serial.println("ADD ALT"); + //} if(temp.options & WP_OPTION_RELATIVE){ // If were relative, just offset from home @@ -190,7 +174,6 @@ static void set_next_WP(struct Location *wp) wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; // to check if we have missed the WP // ---------------------------- @@ -198,7 +181,7 @@ static void set_next_WP(struct Location *wp) // set a new crosstrack bearing // ---------------------------- - crosstrack_bearing = target_bearing; // Used for track following + //crosstrack_bearing = target_bearing; // Used for track following gcs.print_current_waypoints(); } @@ -218,14 +201,12 @@ static void init_home() home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid - home.alt = 0; // this is a test + home.alt = 0; // Home is always 0 home_is_set = true; // to point yaw towards home until we set it with Mavlink target_WP = home; - //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt); - // Save Home to EEPROM // ------------------- // no need to save this to EPROM @@ -234,8 +215,14 @@ static void init_home() // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; + // this is dangerous since we can get GPS lock at any time. //next_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index f544719c45..5b3d88fe2e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -105,17 +105,14 @@ static void handle_process_now() static void handle_no_commands() { - // we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous. - // use landing commands /* switch (control_mode){ default: - //set_mode(RTL); + set_mode(RTL); break; - } - return; - */ - Serial.println("Handle No CMDs"); + }*/ + //return; + //Serial.println("Handle No CMDs"); } /********************************************************************************/ @@ -314,13 +311,10 @@ static void do_loiter_turns() static void do_loiter_time() { - ///* - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; set_next_WP(¤t_loc); loiter_time = millis(); loiter_time_max = next_command.p1 * 1000; // units are (seconds) - //Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max); - //*/ } /********************************************************************************/ @@ -467,12 +461,8 @@ static void do_change_alt() { Location temp = next_WP; condition_start = current_loc.alt; - if (next_command.options & WP_OPTION_ALT_RELATIVE) { - condition_value = next_command.alt + home.alt; - } else { - condition_value = next_command.alt; - } - temp.alt = condition_value; + condition_value = next_command.alt; + temp.alt = next_command.alt; set_next_WP(&temp); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 29514f99c1..d2e40adec0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -210,6 +210,8 @@ //#define OPTFLOW_ENABLED #endif +//#define OPTFLOW_ENABLED + #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) # define OPTFLOW DISABLED #endif @@ -310,7 +312,7 @@ #endif #ifndef SIMPLE_RP -# define SIMPLE_RP ROLL_PITCH_SIMPLE +# define SIMPLE_RP ROLL_PITCH_STABLE #endif #ifndef SIMPLE_THR @@ -390,7 +392,7 @@ // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 3.6 +# define STABILIZE_ROLL_P 4.0 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.02 @@ -400,7 +402,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 3.6 +# define STABILIZE_PITCH_P 4.0 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.02 @@ -413,7 +415,7 @@ // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P .13 +# define RATE_ROLL_P 0.13 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 @@ -472,7 +474,7 @@ # define LOITER_P .4 // #endif #ifndef LOITER_I -# define LOITER_I 0.01 // +# define LOITER_I 0.04 // #endif #ifndef LOITER_IMAX # define LOITER_IMAX 12 // degrees° @@ -482,28 +484,12 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.1 // this +# define NAV_I 0.15 // this #endif #ifndef NAV_IMAX -# define NAV_IMAX 16 // degrees +# define NAV_IMAX 20 // degrees #endif -/* -#ifndef NAV_LOITER_P -# define NAV_LOITER_P .4 //1.4 // -#endif -#ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.05 // -#endif -#ifndef NAV_LOITER_D -# define NAV_LOITER_D 2 // -#endif -#ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 8 // degrees° -#endif -*/ - - #ifndef WAYPOINT_SPEED_MAX # define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph #endif @@ -518,9 +504,6 @@ #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif -//#ifndef THROTTLE_D -//# define THROTTLE_D 0.6 // upped with filter -//#endif #ifndef THROTTLE_IMAX # define THROTTLE_IMAX 300 #endif @@ -579,7 +562,7 @@ # define LOG_ATTITUDE_FAST DISABLED #endif #ifndef LOG_ATTITUDE_MED -# define LOG_ATTITUDE_MED DISABLED +# define LOG_ATTITUDE_MED ENABLED #endif #ifndef LOG_GPS # define LOG_GPS ENABLED @@ -671,8 +654,8 @@ # define WP_RADIUS_DEFAULT 3 #endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 10 +#ifndef LOITER_RADIUS +# define LOITER_RADIUS 10 #endif #ifndef ALT_HOLD_HOME diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 088f5e3288..b31f0b98d9 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -13,6 +13,15 @@ static void read_control_switch() switch_debouncer = false; set_mode(flight_modes[switchPosition]); + + #if CH7_OPTION != SIMPLE_MODE_CONTROL + // setup Simple mode + // do we enable simple mode? + do_simple = (g.simple_modes & (1 << switchPosition)); + //Serial.printf("do simple: %d \n", (int)do_simple); + #endif + + }else{ switch_debouncer = true; } @@ -47,6 +56,11 @@ static void read_trim_switch() do_flip = true; } +#elif CH7_OPTION == SIMPLE_MODE_CONTROL + + do_simple = (g.rc_7.control_in > 800); + //Serial.println(g.rc_7.control_in, DEC); + #elif CH7_OPTION == DO_SET_HOVER // switch is engaged if (g.rc_7.control_in > 800){ diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5a0dcbfd91..eca5b9ef6e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -14,8 +14,7 @@ #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -30,6 +29,7 @@ // CH 7 control #define DO_SET_HOVER 0 #define DO_FLIP 1 +#define SIMPLE_MODE_CONTROL 2 // Frame types #define QUAD_FRAME 0 @@ -52,7 +52,7 @@ #define FR_LED AN12 // Mega PE4 pin, OUT7 #define RE_LED AN14 // Mega PE5 pin, OUT6 #define RI_LED AN10 // Mega PH4 pin, OUT5 -#define LE_LED AN8 // Mega PH5 pin, OUT4 +#define LE_LED AN8 // Mega PH5 pin, OUT4 // Internal defines, don't edit and expect things to work // ------------------------------------------------------- @@ -83,21 +83,6 @@ #define MAX_SONAR_UNKNOWN 0 #define MAX_SONAR_XL 1 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 -#define CH_10 9 //PB5 -#define CH_11 10 //PE3 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 @@ -129,14 +114,20 @@ // ---------------- #define STABILIZE 0 // hold level position #define ACRO 1 // rate control -#define SIMPLE 2 // -#define ALT_HOLD 3 // AUTO control -#define AUTO 4 // AUTO control -#define GUIDED 5 // AUTO control -#define LOITER 6 // Hold a single location -#define RTL 7 // AUTO control -#define CIRCLE 8 // AUTO control -#define NUM_MODES 9 +#define ALT_HOLD 2 // AUTO control +#define AUTO 3 // AUTO control +#define GUIDED 4 // AUTO control +#define LOITER 5 // Hold a single location +#define RTL 6 // AUTO control +#define CIRCLE 7 // AUTO control +#define NUM_MODES 8 + +#define SIMPLE_1 1 +#define SIMPLE_2 2 +#define SIMPLE_3 4 +#define SIMPLE_4 8 +#define SIMPLE_5 16 +#define SIMPLE_6 32 // CH_6 Tuning // ----------- @@ -252,7 +243,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 546dfeeb88..dd7a584bcc 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -96,26 +96,35 @@ static void clear_leds() #if MOTOR_LEDS == 1 static void update_motor_leds(void) { - // blink rear - static bool blink = false; + if (motor_armed == true){ + if (low_batt == true){ + // blink rear + static bool blink = false; - if (blink){ - digitalWrite(RE_LED, HIGH); - digitalWrite(FR_LED, HIGH); - digitalWrite(RI_LED, LOW); - digitalWrite(LE_LED, LOW); - }else{ + if (blink){ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); + }else{ + digitalWrite(RE_LED, LOW); + digitalWrite(FR_LED, LOW); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + blink = !blink; + }else{ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + }else { digitalWrite(RE_LED, LOW); digitalWrite(FR_LED, LOW); - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); } - - blink = !blink; - - // the variable low_batt is here to let people know the voltage is low or the pack capacity is finished - // I don't know what folks want here. - // low_batt } #endif diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 5a4d0e3e9a..1a2b4689f2 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define ARM_DELAY 10 // one secon -#define DISARM_DELAY 10 // one secon +#define ARM_DELAY 10 // one second +#define DISARM_DELAY 10 // one second #define LEVEL_DELAY 120 // twelve seconds #define AUTO_LEVEL_DELAY 150 // twentyfive seconds diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index d1dde00eee..066e7c2d17 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -138,21 +138,21 @@ static void output_motor_test() // 31 // 24 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 50; - motor_out[CH_4] += 50; + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_7] += 50; - motor_out[CH_3] += 50; + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; } }else{ @@ -160,21 +160,21 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_4] += 50; - motor_out[CH_8] += 50; + motor_out[CH_4] += 100; + motor_out[CH_8] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_7] += 50; - motor_out[CH_3] += 50; + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } } @@ -189,27 +189,27 @@ static void output_motor_test() /* APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); delay(1000); } */ diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index c798014c16..a89669ba06 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -182,37 +182,75 @@ static void output_motors_disarmed() static void output_motor_test() { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); - delay(1000); + if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) + { + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50); - delay(1000); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + } + + if( g.frame_orientation == V_FRAME ) + { + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + } } #endif diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 37398ed21f..7866c7d3ab 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -149,8 +149,37 @@ static void output_motors_disarmed() static void output_motor_test() { + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); } #endif - diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index dc6b36ee06..5344a08f63 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -4,6 +4,7 @@ static void output_motors_armed() { + int roll_out, pitch_out; int out_min = g.rc_3.radio_min; int out_max = g.rc_3.radio_max; @@ -139,23 +140,23 @@ static void output_motor_test() // 31 // 24 if(g.rc_1.control_in > 3000){ - motor_out[CH_1] += 50; - motor_out[CH_4] += 50; + motor_out[CH_1] += 100; + motor_out[CH_4] += 100; } if(g.rc_1.control_in < -3000){ - motor_out[CH_2] += 50; - motor_out[CH_3] += 50; + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ - motor_out[CH_2] += 50; - motor_out[CH_4] += 50; + motor_out[CH_2] += 100; + motor_out[CH_4] += 100; } if(g.rc_2.control_in < -3000){ - motor_out[CH_1] += 50; - motor_out[CH_3] += 50; + motor_out[CH_1] += 100; + motor_out[CH_3] += 100; } }else{ @@ -163,16 +164,16 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000) - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; if(g.rc_1.control_in < -3000) - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; if(g.rc_2.control_in > 3000) - motor_out[CH_4] += 50; + motor_out[CH_4] += 100; if(g.rc_2.control_in < -3000) - motor_out[CH_3] += 50; + motor_out[CH_3] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 0fbff12ab7..ec9c747b41 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -96,15 +96,15 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_4] += 50; + motor_out[CH_4] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index ede41a0fba..23f51218a2 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -144,18 +144,18 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; - motor_out[CH_7] += 50; + motor_out[CH_1] += 100; + motor_out[CH_7] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; - motor_out[CH_3] += 50; + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 50; - motor_out[CH_4] += 50; + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 6cddf96010..855761192f 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -30,15 +30,6 @@ static void navigate() // target_bearing is where we should be heading // -------------------------------------------- target_bearing = get_bearing(¤t_loc, &next_WP); - - // nav_bearing will include xtrac correction - // ----------------------------------------- - //xtrack_enabled = false; - if(xtrack_enabled){ - nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); - }else{ - nav_bearing = target_bearing; - } } static bool check_missed_wp() @@ -70,6 +61,9 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH } + +// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0); + #define NAV_ERR_MAX 400 static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed) { @@ -101,18 +95,30 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed } // find the rates: - // calc the cos of the error to tell how fast we are moving towards the target in cm - y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); + float temp = radians((float)g_gps->ground_course/100.0); + + #ifdef OPTFLOW_ENABLED + // calc the cos of the error to tell how fast we are moving towards the target in cm + if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ + x_actual_speed = optflow.vlon * 10; + y_actual_speed = optflow.vlat * 10; + }else{ + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + } + #else + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + #endif + y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); - // calc the sin of the error to tell how fast we are moving laterally to the target in cm - x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -250, 250); + x_rate_error = constrain(x_rate_error, -600, 600); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } @@ -127,13 +133,6 @@ static void calc_nav_pitch_roll() nav_pitch = -nav_pitch; } -// ------------------------------ -static void calc_bearing_error() -{ - bearing_error = nav_bearing - dcm.yaw_sensor; - bearing_error = wrap_180(bearing_error); -} - static long get_altitude_error() { return next_WP.alt - current_loc.alt; @@ -189,6 +188,7 @@ static long wrap_180(long error) return error; } +/* static long get_crosstrack_correction(void) { // Crosstrack Error @@ -206,19 +206,20 @@ static long get_crosstrack_correction(void) } return 0; } - - +*/ +/* static long cross_track_test() { long temp = wrap_180(target_bearing - crosstrack_bearing); return abs(temp); } - +*/ +/* static void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } - +*/ static long get_altitude_above_home(void) { // This is the altitude above the home location diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 4025730e8c..86b30c9de7 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -101,7 +101,7 @@ void output_min() static void read_radio() { if (APM_RC.GetState() == 1){ - + new_radio_frame = true; g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 7e928276f0..844044aa97 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -113,17 +113,20 @@ static void read_battery(void) if(g.battery_monitoring == 1) battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream + if(g.battery_monitoring == 2) battery_voltage = battery_voltage4; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) battery_voltage = battery_voltage1; + if(g.battery_monitoring == 4) { current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; } #if BATTERY_EVENT == 1 - if(battery_voltage < LOW_VOLTAGE) + if(battery_voltage < g.low_voltage) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 529297aa2d..e9c25cf4c2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_flight_modes(); report_imu(); report_compass(); + #ifdef OPTFLOW_ENABLED report_optflow(); #endif + #if FRAME_CONFIG == HELI_FRAME report_heli(); report_gyro(); @@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) byte _oldSwitchPosition = 0; byte mode = 0; - Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); + Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n")); print_hit_enter(); while(1){ @@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode = constrain(mode, 0, NUM_MODES-1); // update the user - print_switch(_switchPosition, mode); + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); // Remember switch position _oldSwitchPosition = _switchPosition; } // look for stick input - if (radio_input_switch() == true){ + if (abs(g.rc_1.control_in) > 3000){ mode++; if(mode >= NUM_MODES) mode = 0; @@ -334,13 +336,32 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) flight_modes[_switchPosition] = mode; // print new mode - print_switch(_switchPosition, mode); + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (g.rc_4.control_in > 3000){ + g.simple_modes |= (1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (g.rc_4.control_in < -3000){ + g.simple_modes &= ~(1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); } // escape hatch if(Serial.available() > 0){ - for (mode=0; mode<6; mode++) + for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); + + g.simple_modes.save(); print_done(); report_flight_modes(); return (0); @@ -740,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.optflow_enabled = false; - //} else if(argv[1].i > 10){ - // g.optflow_fov.set_and_save(argv[1].i); - // optflow.set_field_of_view(g.optflow_fov.get()); - }else{ Serial.printf_P(PSTR("\nOptions:[on, off]\n")); report_optflow(); @@ -872,7 +889,7 @@ static void report_flight_modes() print_divider(); for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i]); + print_switch(i, flight_modes[i], (g.simple_modes & (1< 100) { - bouncer = 10; - } - if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) { - bouncer = -10; - } - if (bouncer >0) { - bouncer --; - } - if (bouncer <0) { - bouncer ++; - } - - if (bouncer == 1 || bouncer == -1) { - return bouncer; - }else{ - return 0; - } -} - static void zero_eeprom(void) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 70daa8e178..1161b58d7e 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands); +MENU(main_menu, THISFIRMWARE, main_menu_commands); #endif // CLI_ENABLED @@ -73,8 +73,8 @@ static void init_ardupilot() Serial1.begin(38400, 128, 16); #endif - Serial.printf_P(PSTR("\n\nInit ACM" - "\n\nRAM: %lu\n"), + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE + "\n\nFree RAM: %lu\n"), freeRAM()); @@ -204,12 +204,12 @@ static void init_ardupilot() if(g.compass_enabled) init_compass(); -#ifdef OPTFLOW_ENABLED + #ifdef OPTFLOW_ENABLED // init the optical flow sensor if(g.optflow_enabled) { init_optflow(); } -#endif + #endif // Logging: // -------- @@ -249,11 +249,6 @@ static void init_ardupilot() start_new_log(); } - //#if(GROUND_START_DELAY > 0) - //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); - // delay(GROUND_START_DELAY * 1000); - //#endif - GPS_enabled = false; // Read in the GPS @@ -292,10 +287,8 @@ static void init_ardupilot() // --------------------------- reset_control_switch(); - //delay(100); startup_ground(); - //Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max); Log_Write_Startup(); SendDebug("\nReady to FLY "); @@ -312,7 +305,9 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- imu.init_gyro(mavlink_delay); - report_imu(); + #if CLI_ENABLED == ENABLED + report_imu(); + #endif #endif // reset the leds @@ -328,8 +323,7 @@ static void startup_ground(void) #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -361,9 +355,7 @@ static void set_mode(byte mode) // report the GPS and Motor arming status led_mode = NORMAL_LEDS; - // most modes do not calculate crosstrack correction - xtrack_enabled = false; - reset_nav_I(); + reset_nav(); switch(control_mode) { @@ -381,13 +373,6 @@ static void set_mode(byte mode) reset_hold_I(); break; - case SIMPLE: - yaw_mode = SIMPLE_YAW; - roll_pitch_mode = SIMPLE_RP; - throttle_mode = SIMPLE_THR; - reset_hold_I(); - break; - case ALT_HOLD: yaw_mode = ALT_HOLD_YAW; roll_pitch_mode = ALT_HOLD_RP; @@ -395,7 +380,7 @@ static void set_mode(byte mode) reset_hold_I(); init_throttle_cruise(); - next_WP.alt = current_loc.alt; + next_WP = current_loc; break; case AUTO: @@ -407,11 +392,7 @@ static void set_mode(byte mode) init_throttle_cruise(); // loads the commands from where we left off - init_auto(); - - // do crosstrack correction - // XXX move to flight commands - xtrack_enabled = true; + init_commands(); break; case CIRCLE: @@ -437,9 +418,10 @@ static void set_mode(byte mode) roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_AUTO; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); next_WP = current_loc; + set_next_WP(&guided_WP); break; case RTL: @@ -447,7 +429,7 @@ static void set_mode(byte mode) roll_pitch_mode = RTL_RP; throttle_mode = RTL_THR; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); do_RTL(); break; @@ -489,7 +471,7 @@ static void set_failsafe(boolean mode) static void resetPerfData(void) { - mainLoop_count = 0; + //mainLoop_count = 0; G_Dt_max = 0; gps_fix_count = 0; perf_mon_timer = millis(); @@ -508,7 +490,10 @@ init_compass() static void init_optflow() { - optflow.init(); + if( optflow.init() == false ) { + g.optflow_enabled = false; + //SendDebug("\nFailed to Init OptFlow "); + } optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view } @@ -539,7 +524,7 @@ static void init_throttle_cruise() { // are we moving from manual throttle to auto_throttle? - if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ + if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 895b41d55b..94144ff68c 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -362,7 +362,7 @@ test_adc(uint8_t argc, const Menu::arg *argv) while(1){ for(int i = 0; i < 9; i++){ - Serial.printf_P(PSTR("%d,"),adc.Ch(i)); + Serial.printf_P(PSTR("%u,"),adc.Ch(i)); } Serial.println(); delay(20); @@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { + ///* if(g.optflow_enabled) { Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); print_hit_enter(); diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..40fcc180a9 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,5 +1,4 @@ - - // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6d8685b3e3..4c560ef062 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -304,19 +304,6 @@ //#define FLIGHT_MODE_6 MANUAL // -////////////////////////////////////////////////////////////////////////////// -// RC_5_FUNCT OPTIONAL -// RC_6_FUNCT OPTIONAL -// RC_7_FUNCT OPTIONAL -// RC_8_FUNCT OPTIONAL -// -// The channel 5 through 8 function assignments allow configuration of those -// channels for use with differential ailerons, flaps, flaperons, or camera -// or intrument mounts -// -//#define RC_5_FUNCT RC_5_FUNCT_NONE -//etc. - ////////////////////////////////////////////////////////////////////////////// // For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed // then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed @@ -411,9 +398,9 @@ // also means that you should avoid switching out of MANUAL while you have // any control stick deflection. // -// The default is to enable AUTO_TRIM. +// The default is to disable AUTO_TRIM. // -//#define AUTO_TRIM ENABLED +//#define AUTO_TRIM DISABLED // ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9a0da7d2e8..569708c354 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -415,6 +415,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static float load; // % MCU cycles used +RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -501,7 +502,7 @@ static void fast_loop() hil.update(); #endif - dcm.update_DCM(G_Dt); + dcm.update_DCM(); // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); @@ -710,6 +711,8 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + break; case 2: diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3305927fa4..4847f86e76 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -61,7 +61,7 @@ static void stabilize() // Mix Stick input to allow users to override control surfaces // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { + if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) { // TODO: use RC_Channel control_mix function? @@ -92,7 +92,7 @@ static void stabilize() // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes // important for steering on the ground during landing // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { + if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) { ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = fabs(ch4_inf); ch4_inf = min(ch4_inf, 400.0); @@ -274,21 +274,16 @@ static void set_servos(void) } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in; + G_RC_AUX(k_aileron)->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; } else { if (g.mix_mode == 0) { g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.servo_out = g.channel_roll.servo_out; - g.rc_5.calc_pwm(); - } - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.servo_out = g.channel_roll.servo_out; - g.rc_6.calc_pwm(); + if (g_rc_function[RC_Channel_aux::k_aileron]) { + g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; + g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); } }else{ @@ -320,8 +315,7 @@ static void set_servos(void) } if(control_mode <= FLY_BY_WIRE_B) { - if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in; + G_RC_AUX(k_flap_auto)->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; } else if (control_mode >= FLY_BY_WIRE_C) { if (g.airspeed_enabled == true) { flapSpeedSource = g.airspeed_cruise; @@ -329,14 +323,11 @@ static void set_servos(void) flapSpeedSource = g.throttle_cruise; } if ( flapSpeedSource > g.flap_1_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0; + G_RC_AUX(k_flap_auto)->servo_out = 0; } else if (flapSpeedSource > g.flap_2_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent; + G_RC_AUX(k_flap_auto)->servo_out = g.flap_1_percent; } else { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent; + G_RC_AUX(k_flap_auto)->servo_out = g.flap_2_percent; } } @@ -347,8 +338,11 @@ static void set_servos(void) APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos - APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos - APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos + // Route configurable aux. functions to their respective servos + g.rc_5.output_ch(CH_5); + g.rc_6.output_ch(CH_6); + g.rc_7.output_ch(CH_7); + g.rc_8.output_ch(CH_8); #endif } diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt new file mode 100644 index 0000000000..d2c7506d88 --- /dev/null +++ b/ArduPlane/CMakeLists.txt @@ -0,0 +1,164 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + + +message(STATUS "DIR: ${CMAKE_SOURCE_DIR}") + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduPlane C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ArduPlane) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduPlane.pde + Attitude.pde + climb_rate.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + #flip.pde + #GCS.pde + #GCS_Ardupilot.pde + #GCS_IMU_output.pde + #GCS_Jason_text.pde + GCS_Mavlink.pde + #GCS_Standard.pde + #GCS_Xplane.pde + #heli.pde + HIL_Xplane.pde + #leds.pde + Log.pde + #motors_hexa.pde + #motors_octa.pde + #motors_octa_quad.pde + #motors_quad.pde + #motors_tri.pde + #motors_y6.pde + navigation.pde + planner.pde + radio.pde + #read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +#new +#${CMAKE_SOURCE_DIR}/libraries/Wire +#${CMAKE_SOURCE_DIR}/libraries/SPI +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..bc0268ab66 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -51,6 +51,7 @@ GCS_MAVLINK::update(void) // receive new packets mavlink_message_t msg; mavlink_status_t status; + status.packet_rx_drop_count = 0; // process received bytes while(comm_get_available(chan)) @@ -1084,15 +1085,6 @@ GCS_MAVLINK::_queued_send() #endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK -static void send_rate(uint16_t low, uint16_t high) { -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(low, high); -#endif -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(low,high); -#endif -} - /* a delay() callback that processes MAVLink packets. We set this as the callback in long running library initialisation routines to allow @@ -1102,7 +1094,7 @@ static void send_rate(uint16_t low, uint16_t high) { static void mavlink_delay(unsigned long t) { unsigned long tstart; - static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; + static unsigned long last_1hz, last_50hz; if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by @@ -1119,18 +1111,11 @@ static void mavlink_delay(unsigned long t) if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs.send_message(MSG_HEARTBEAT); + gcs.send_message(MSG_EXTENDED_STATUS1); #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); + hil.send_message(MSG_EXTENDED_STATUS1); #endif - send_rate(1, 3); - } - if (tnow - last_3hz > 333) { - last_3hz = tnow; - send_rate(3, 5); - } - if (tnow - last_10hz > 100) { - last_10hz = tnow; - send_rate(5, 45); } if (tnow - last_50hz > 20) { last_50hz = tnow; @@ -1138,7 +1123,6 @@ static void mavlink_delay(unsigned long t) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.update(); #endif - send_rate(45, 1000); } delay(1); } while (millis() - tstart < t); diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 4f01c19028..d87b01c344 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..32ee10ec50 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,327 +25,369 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +#define NOINLINE __attribute__((noinline)) + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + Vector3f omega = dcm.get_gyro(); + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + break; + case FLY_BY_WIRE_A: + mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case FLY_BY_WIRE_B: + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_LOITER; + break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + (int)g.channel_throttle.servo_out, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.RawPress, + barometer.RawTemp, + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + + // try to send a message, return false if it won't fit in the serial tx buffer static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) { - uint64_t timeStamp = micros(); int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { // defer any messages on the telemetry port for 1 second after // bootup, to try to prevent bricking of Xbees return false; } - switch(id) { + switch (id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; - case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); } + break; + + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; - case MSG_EXTENDED_STATUS1: - { - CHECK_PAYLOAD_SIZE(SYS_STATUS); - - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case MANUAL: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_TEST1; - break; - case FLY_BY_WIRE_A: - mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LOITER; - break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; - } - - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - load * 1000, - battery_voltage * 1000, - battery_remaining, - packet_drops); - break; - } - - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); - break; - } - - case MSG_ATTITUDE: - { - CHECK_PAYLOAD_SIZE(ATTITUDE); - Vector3f omega = dcm.get_gyro(); - mavlink_msg_attitude_send( - chan, - timeStamp, - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); - break; - } - - case MSG_LOCATION: - { - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); - break; - } - - case MSG_NAV_CONTROLLER_OUTPUT: - { - if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - mavlink_msg_nav_controller_output_send( - chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - airspeed_error, - crosstrack_error); - } - break; - } - - case MSG_LOCAL_LOCATION: - { - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_local_position_send( - chan, - timeStamp, - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } - - case MSG_GPS_RAW: - { - CHECK_PAYLOAD_SIZE(GPS_RAW); - mavlink_msg_gps_raw_send( - chan, - timeStamp, - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); - break; - } - - case MSG_SERVO_OUT: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); - break; - } - - case MSG_RADIO_IN: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - break; - } - - case MSG_RADIO_OUT: - { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - mavlink_msg_servo_output_raw_send( - chan, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); - break; - } - - case MSG_VFR_HUD: - { - CHECK_PAYLOAD_SIZE(VFR_HUD); - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - (int)g.channel_throttle.servo_out, - current_loc.alt / 100.0, - climb_rate); - break; - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: - { - CHECK_PAYLOAD_SIZE(RAW_IMU); - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); - //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); - mavlink_msg_raw_imu_send( - chan, - timeStamp, - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); - break; - } - - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100.0, - (float)(barometer.Press-g.ground_pressure)/100.0, - (int)(barometer.Temp*10)); - break; - } - - case MSG_RAW_IMU3: - { - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); - break; - } - #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE - - case MSG_GPS_STATUS: - { - CHECK_PAYLOAD_SIZE(GPS_STATUS); - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); - break; - } - - default: - break; + default: + break; } return true; } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..a4c402ead6 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,11 +17,11 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 11; - + static const uint16_t k_format_version = 12; + // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use + // GCS will interpret values 0-9 as ArduPilotMega. Developers may use // values within that range to identify different branches. // static const uint16_t k_software_type = 0; // 0 for APM trunk @@ -70,7 +70,7 @@ public: k_param_flap_2_speed, k_param_num_resets, - + // 110: Telemetry control // k_param_streamrates_port0 = 110, @@ -95,10 +95,13 @@ public: k_param_compass_enabled, k_param_compass, k_param_battery_monitoring, - k_param_pack_capacity, - k_param_airspeed_offset, - k_param_sonar_enabled, - k_param_airspeed_enabled, + k_param_volt_div_ratio, + k_param_curr_amp_per_volt, + k_param_input_voltage, + k_param_pack_capacity, + k_param_airspeed_offset, + k_param_sonar_enabled, + k_param_airspeed_enabled, // // 150: Navigation parameters @@ -129,16 +132,11 @@ public: k_param_throttle_fs_enabled, k_param_throttle_fs_value, k_param_throttle_cruise, - + k_param_short_fs_action, k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, - - k_param_rc_5_funct, - k_param_rc_6_funct, - k_param_rc_7_funct, - k_param_rc_8_funct, // // 200: Feed-forward gains @@ -227,13 +225,13 @@ public: AP_Int16 format_version; AP_Int8 software_type; - + // Telemetry control // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; AP_Int8 serial3_baud; - + // Feed-forward gains // AP_Float kff_pitch_compensation; @@ -273,7 +271,7 @@ public: AP_Int8 throttle_fs_enabled; AP_Int16 throttle_fs_value; AP_Int8 throttle_cruise; - + // Failsafe AP_Int8 short_fs_action; AP_Int8 long_fs_action; @@ -313,6 +311,9 @@ public: AP_Int8 compass_enabled; AP_Int16 angle_of_attack; AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current + AP_Float volt_div_ratio; + AP_Float curr_amp_per_volt; + AP_Float input_voltage; AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger AP_Int8 sonar_enabled; @@ -327,14 +328,10 @@ public: RC_Channel channel_pitch; RC_Channel channel_throttle; RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - AP_Int8 rc_5_funct; - AP_Int8 rc_6_funct; - AP_Int8 rc_7_funct; - AP_Int8 rc_8_funct; + RC_Channel_aux rc_5; + RC_Channel_aux rc_6; + RC_Channel_aux rc_7; + RC_Channel_aux rc_8; // PID controllers // @@ -386,7 +383,7 @@ public: throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - + short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), @@ -421,17 +418,16 @@ public: flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")), flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")), flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")), - - + + battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")), + curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")), + input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")), pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), - rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), - rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), - rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), - rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! diff --git a/ArduPlane/config.h b/ArduPlane/config.h index bc1056e55f..d48b4cb73d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -226,19 +226,6 @@ # define CH8_MAX 2000 #endif -////////////////////////////////////////////////////////////////////////////// -#ifndef RC_5_FUNCT -# define RC_5_FUNCT RC_5_FUNCT_NONE -#endif -#ifndef RC_6_FUNCT -# define RC_6_FUNCT RC_6_FUNCT_NONE -#endif -#ifndef RC_7_FUNCT -# define RC_7_FUNCT RC_7_FUNCT_NONE -#endif -#ifndef RC_8_FUNCT -# define RC_8_FUNCT RC_8_FUNCT_NONE -#endif #ifndef FLAP_1_PERCENT # define FLAP_1_PERCENT 0 @@ -313,7 +300,7 @@ // AUTO_TRIM // #ifndef AUTO_TRIM -# define AUTO_TRIM ENABLED +# define AUTO_TRIM DISABLED #endif diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..7d0aac6c2b 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,39 +41,12 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 #define CH_RUDDER CH_4 #define CH_YAW CH_4 -#define RC_5_FUNCT_NONE 0 -#define RC_5_FUNCT_AILERON 1 -#define RC_5_FUNCT_FLAP_AUTO 2 -#define RC_5_FUNCT_FLAPERON 3 - -#define RC_6_FUNCT_NONE 0 -#define RC_6_FUNCT_AILERON 1 -#define RC_6_FUNCT_FLAP_AUTO 2 -#define RC_6_FUNCT_FLAPERON 3 - -#define RC_7_FUNCT_NONE 0 - -#define RC_8_FUNCT_NONE 0 - // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 @@ -99,7 +72,7 @@ #define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle #define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed #define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed - // Fly By Wire B and Fly By Wire C require airspeed sensor + // Fly By Wire B and Fly By Wire C require airspeed sensor #define AUTO 10 #define RTL 11 #define LOITER 12 @@ -132,7 +105,7 @@ /// NOTE: to ensure we never block on sending MAVLink messages /// please keep each MSG_ to a single MAVLink message. If need be /// create new MSG_ IDs for additional messages on the same -/// stream +/// stream #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 @@ -190,7 +163,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 @@ -247,9 +219,9 @@ #define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from -#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO +#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio -#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT +#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt #define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor #define BATTERY_PIN1 0 // These are the pins for the voltage dividers diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index acd60e9cfc..faa0e0dc69 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -43,12 +43,12 @@ static void failsafe_long_on_event() case STABILIZE: case FLY_BY_WIRE_A: // middle position case FLY_BY_WIRE_B: // middle position + case CIRCLE: set_mode(RTL); break; case AUTO: case LOITER: - case CIRCLE: if(g.long_fs_action == 1) { set_mode(RTL); } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 748b7a9228..674c096e25 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,24 +23,7 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.set_angle(SERVO_MAX); - } else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) { - g.rc_5.set_range(0,100); - } else { - g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.set_angle(SERVO_MAX); - } else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) { - g.rc_6.set_range(0,100); - } else { - g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here - g.rc_8.set_range(0,1000); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); } static void init_rc_out() @@ -53,7 +36,7 @@ static void init_rc_out() APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); APM_RC.Init(); // APM Radio initialization @@ -173,8 +156,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -191,8 +173,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } static void trim_radio() @@ -208,8 +189,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -225,7 +205,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } - diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index cbe7ddadd2..4c994cdb3e 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -41,7 +41,7 @@ static const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "APM trunk", main_menu_commands); +MENU(main_menu, THISFIRMWARE, main_menu_commands); #endif // CLI_ENABLED diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 926e95ca1a..3b95d9c6c0 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -418,7 +418,7 @@ test_adc(uint8_t argc, const Menu::arg *argv) delay(1000); while(1){ - for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i)); + for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i)); Serial.println(); delay(100); if(Serial.available() > 0){ @@ -476,7 +476,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(G_Dt); + dcm.update_DCM(); if(g.compass_enabled) { medium_loopCounter++; @@ -563,7 +563,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(G_Dt); + dcm.update_DCM(); medium_loopCounter++; if(medium_loopCounter == 5){ diff --git a/Tools/ArduTracker/APM_Config.h.reference b/Tools/ArduTracker/APM_Config.h.reference index 0964e16769..3eb4c9c3c4 100644 --- a/Tools/ArduTracker/APM_Config.h.reference +++ b/Tools/ArduTracker/APM_Config.h.reference @@ -2,7 +2,7 @@ // Example and reference ArduPilot Mega configuration file // ======================================================= // -// This file contains documentation and examples for configuration options +// This file contains documentation and examples of configuration options // supported by the ArduPilot Mega software. // // Most of these options are just that - optional. You should create diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 6ac38ed914..b75096d41e 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -188,6 +188,7 @@ Component + @@ -232,7 +233,6 @@ UserControl - Component @@ -302,7 +302,6 @@ ElevationProfile.cs - Component @@ -338,6 +337,7 @@ Splash.cs + Form @@ -374,6 +374,7 @@ Firmware.cs + Designer FlightData.cs @@ -424,6 +425,7 @@ Firmware.cs Always + Designer FlightData.cs diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index a9c7314de0..ccd82ba138 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -240,13 +240,12 @@ namespace ArdupilotMega { STABILIZE = 0, // hold level position ACRO = 1, // rate control - SIMPLE = 2, // - ALT_HOLD = 3, // AUTO control - AUTO = 4, // AUTO control - GUIDED = 5, // AUTO control - LOITER = 6, // Hold a single location - RTL = 7, // AUTO control - CIRCLE = 8 + ALT_HOLD = 2, // AUTO control + AUTO = 3, // AUTO control + GUIDED = 4, // AUTO control + LOITER = 5, // Hold a single location + RTL = 6, // AUTO control + CIRCLE = 7 } public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode) diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index 99660f265f..94e3281f84 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -6,7 +6,6 @@ using System.IO.Ports; using System.Threading; using System.Net; // dns, ip address using System.Net.Sockets; // tcplistner -using SerialProxy; namespace System.IO.Ports { diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 71db65033b..1584a56b18 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -65,7 +65,7 @@ namespace ArdupilotMega public float gz { get; set; } // calced turn rate - public float turnrate { get { if (groundspeed == 0) return 0; return (roll * 9.8f) / groundspeed; } } + public float turnrate { get { if (groundspeed <= 1) return 0; return (roll * 9.8f) / groundspeed; } } //radio public float ch1in { get; set; } @@ -101,17 +101,17 @@ namespace ArdupilotMega //nav state public float nav_roll { get; set; } public float nav_pitch { get; set; } - public short nav_bearing { get; set; } - public short target_bearing { get; set; } - public ushort wp_dist { get { return (ushort)(_wpdist * multiplierdist); } set { _wpdist = value; } } + public float nav_bearing { get; set; } + public float target_bearing { get; set; } + public float wp_dist { get { return (_wpdist * multiplierdist); } set { _wpdist = value; } } public float alt_error { get { return _alt_error * multiplierdist; } set { _alt_error = value; } } public float ber_error { get { return (target_bearing - yaw); } set { } } public float aspd_error { get { return _aspd_error * multiplierspeed; } set { _aspd_error = value; } } public float xtrack_error { get; set; } - public int wpno { get; set; } + public float wpno { get; set; } public string mode { get; set; } public float climbrate { get; set; } - ushort _wpdist; + float _wpdist; float _aspd_error; float _alt_error; @@ -150,7 +150,8 @@ namespace ArdupilotMega public float brklevel { get; set; } // stats - public ushort packetdrop { get; set; } + public ushort packetdropremote { get; set; } + public ushort linkqualitygcs { get; set; } // requested stream rates public byte rateattitude { get; set; } @@ -284,24 +285,21 @@ namespace ArdupilotMega mode = "Acro"; break; case (byte)102: - mode = "Simple"; - break; - case (byte)103: mode = "Alt Hold"; break; - case (byte)104: + case (byte)103: mode = "Auto"; break; - case (byte)105: + case (byte)104: mode = "Guided"; break; - case (byte)106: + case (byte)105: mode = "Loiter"; break; - case (byte)107: + case (byte)106: mode = "RTL"; break; - case (byte)108: + case (byte)107: mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: @@ -361,7 +359,7 @@ namespace ArdupilotMega battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; - packetdrop = sysstatus.packet_drop; + packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { @@ -468,7 +466,7 @@ namespace ArdupilotMega wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); - int oldwp = wpno; + int oldwp = (int)wpno; wpno = wpcur.seq; @@ -545,6 +543,27 @@ namespace ArdupilotMega //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } + if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null) + { + var imu = new MAVLink.__mavlink_scaled_imu_t(); + + object temp = imu; + + MAVLink.ByteArrayToStructure(MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6); + + imu = (MAVLink.__mavlink_scaled_imu_t)(temp); + + gx = imu.xgyro; + gy = imu.ygyro; + gz = imu.zgyro; + + ax = imu.xacc; + ay = imu.yacc; + az = imu.zacc; + + //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; + } + if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null) { MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index 82d0c1de51..a93b411f51 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -30,8 +30,8 @@ { this.components = new System.ComponentModel.Container(); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration)); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle(); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); this.Params = new System.Windows.Forms.DataGridView(); this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn(); @@ -167,9 +167,6 @@ this.label21 = new System.Windows.Forms.Label(); this.THR_HOLD_P = new System.Windows.Forms.DomainUpDown(); this.label22 = new System.Windows.Forms.Label(); - this.groupBox18 = new System.Windows.Forms.GroupBox(); - this.PITCH_MAX = new System.Windows.Forms.DomainUpDown(); - this.label27 = new System.Windows.Forms.Label(); this.groupBox19 = new System.Windows.Forms.GroupBox(); this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown(); this.label28 = new System.Windows.Forms.Label(); @@ -285,7 +282,6 @@ this.groupBox4.SuspendLayout(); this.groupBox6.SuspendLayout(); this.groupBox7.SuspendLayout(); - this.groupBox18.SuspendLayout(); this.groupBox19.SuspendLayout(); this.groupBox20.SuspendLayout(); this.groupBox21.SuspendLayout(); @@ -302,14 +298,14 @@ this.Params.AllowUserToAddRows = false; this.Params.AllowUserToDeleteRows = false; resources.ApplyResources(this.Params, "Params"); - dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon; - dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White; - dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3; + dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon; + dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White; + dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1; this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize; this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] { this.Command, @@ -318,14 +314,14 @@ this.mavScale, this.RawValue}); this.Params.Name = "Params"; - dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption; - dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText; - dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4; + dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption; + dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText; + dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; this.Params.RowHeadersVisible = false; this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); // @@ -990,7 +986,6 @@ this.TabAC2.Controls.Add(this.groupBox4); this.TabAC2.Controls.Add(this.groupBox6); this.TabAC2.Controls.Add(this.groupBox7); - this.TabAC2.Controls.Add(this.groupBox18); this.TabAC2.Controls.Add(this.groupBox19); this.TabAC2.Controls.Add(this.groupBox20); this.TabAC2.Controls.Add(this.groupBox21); @@ -1157,24 +1152,6 @@ resources.ApplyResources(this.label22, "label22"); this.label22.Name = "label22"; // - // groupBox18 - // - this.groupBox18.Controls.Add(this.PITCH_MAX); - this.groupBox18.Controls.Add(this.label27); - resources.ApplyResources(this.groupBox18, "groupBox18"); - this.groupBox18.Name = "groupBox18"; - this.groupBox18.TabStop = false; - // - // PITCH_MAX - // - resources.ApplyResources(this.PITCH_MAX, "PITCH_MAX"); - this.PITCH_MAX.Name = "PITCH_MAX"; - // - // label27 - // - resources.ApplyResources(this.label27, "label27"); - this.label27.Name = "label27"; - // // groupBox19 // this.groupBox19.Controls.Add(this.HLD_LAT_IMAX); @@ -1887,7 +1864,6 @@ this.groupBox4.ResumeLayout(false); this.groupBox6.ResumeLayout(false); this.groupBox7.ResumeLayout(false); - this.groupBox18.ResumeLayout(false); this.groupBox19.ResumeLayout(false); this.groupBox20.ResumeLayout(false); this.groupBox21.ResumeLayout(false); @@ -2031,9 +2007,6 @@ private System.Windows.Forms.Label label21; private System.Windows.Forms.DomainUpDown THR_HOLD_P; private System.Windows.Forms.Label label22; - private System.Windows.Forms.GroupBox groupBox18; - private System.Windows.Forms.DomainUpDown PITCH_MAX; - private System.Windows.Forms.Label label27; private System.Windows.Forms.GroupBox groupBox19; private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX; private System.Windows.Forms.Label label28; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 9dba66f3d1..8758775aac 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -257,6 +257,8 @@ namespace ArdupilotMega.GCSViews internal void processToScreen() { + + Params.Rows.Clear(); // process hashdefines and update display @@ -541,7 +543,10 @@ namespace ArdupilotMega.GCSViews private void BUT_writePIDS_Click(object sender, EventArgs e) { - foreach (string value in changes.Keys) + + Hashtable temp = (Hashtable)changes.Clone(); + + foreach (string value in temp.Keys) { try { @@ -566,6 +571,7 @@ namespace ArdupilotMega.GCSViews if (row.Cells[0].Value.ToString() == value) { row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45); + changes.Remove(value); break; } } @@ -575,8 +581,6 @@ namespace ArdupilotMega.GCSViews } catch { MessageBox.Show("Set " + value + " Failed"); } } - - changes.Clear(); } const float rad2deg = (float)(180 / Math.PI); @@ -600,9 +604,11 @@ namespace ArdupilotMega.GCSViews MainV2.fixtheme(temp); - TabSetup.Controls.Clear(); + temp.ShowDialog(); - TabSetup.Controls.Add(temp.Controls[0]); + startup = true; + processToScreen(); + startup = false; } } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index 8c4784c6d9..1ecdd308f8 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -192,6 +192,2421 @@ Top, Bottom, Left, Right + + 111, 82 + + + 78, 20 + + + 11 + + + THR_FS_VALUE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + NoControl + + + 6, 86 + + + 50, 13 + + + 12 + + + FS Value + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + THR_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + NoControl + + + 6, 63 + + + 27, 13 + + + 13 + + + Max + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + THR_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + NoControl + + + 6, 40 + + + 24, 13 + + + 14 + + + Min + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 15 + + + Cruise + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 0 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSPD_RATIO + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + NoControl + + + 6, 87 + + + 32, 13 + + + 1 + + + Ratio + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + NoControl + + + 6, 59 + + + 53, 13 + + + 3 + + + FBW max + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + NoControl + + + 6, 40 + + + 50, 13 + + + 5 + + + FBW min + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + NoControl + + + 6, 17 + + + 64, 13 + + + 6 + + + Cruise + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + + 195, 108 + + + 1 + + + Airspeed m/s + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + LIM_PITCH_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + NoControl + + + 6, 63 + + + 51, 13 + + + 10 + + + Pitch Min + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + 111, 36 + + + 78, 20 + + + 7 + + + LIM_PITCH_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + NoControl + + + 6, 40 + + + 54, 13 + + + 11 + + + Pitch Max + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 5 + + + LIM_ROLL_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + NoControl + + + 6, 17 + + + 55, 13 + + + 12 + + + Bank Max + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + + 195, 108 + + + 2 + + + Navigation Angles + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 2 + + + 111, 36 + + + 78, 20 + + + 7 + + + XTRK_ANGLE_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 8 + + + Entry Angle + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + 111, 13 + + + 78, 20 + + + 5 + + + XTRK_GAIN_SC + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + NoControl + + + 6, 17 + + + 52, 13 + + + 9 + + + Gain (cm) + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + + 195, 108 + + + 3 + + + Xtrack Pids + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 13 + + + KFF_PTCH2THR + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 14 + + + P to T + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + KFF_RDDRMIX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + NoControl + + + 6, 63 + + + 61, 13 + + + 15 + + + Rudder Mix + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + KFF_PTCHCOMP + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 16 + + + Pitch Comp + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + + 195, 108 + + + 4 + + + Other Mix's + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 4 + + + 111, 82 + + + 78, 20 + + + 11 + + + ENRGY2THR_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + ENRGY2THR_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + ENRGY2THR_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + + 195, 108 + + + 5 + + + Energy/Alt Pid + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 5 + + + 111, 82 + + + 78, 20 + + + 0 + + + ALT2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ALT2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ALT2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ALT2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + + 195, 108 + + + 6 + + + Nav Pitch Alt Pid + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 6 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSP2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSP2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSP2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ARSP2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + + 195, 108 + + + 7 + + + Nav Pitch AS Pid + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 7 + + + 111, 82 + + + 78, 20 + + + 11 + + + HDNG2RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + HDNG2RLL_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + HDNG2RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + + 195, 108 + + + 8 + + + Nav Roll Pid + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 8 + + + 111, 82 + + + 78, 20 + + + 11 + + + YW2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + YW2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + YW2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + + 195, 108 + + + 9 + + + Servo Yaw Pid + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 9 + + + 111, 82 + + + 78, 20 + + + 11 + + + PTCH2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + PTCH2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + PTCH2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + + 195, 108 + + + 10 + + + Servo Pitch Pid + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 10 + + + 111, 82 + + + 78, 20 + + + 11 + + + RLL2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + RLL2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + RLL2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + + 195, 108 + + + 11 + + + Servo Roll Pid + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + APM 2.x + TabAPM2 @@ -204,6 +2619,1824 @@ 0 + + True + + + 3, 198 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + + + 80, 86 + + + 78, 20 + + + 16 + + + WP_SPEED_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + NoControl + + + 6, 89 + + + 54, 13 + + + 17 + + + m/s + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + NAV_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + NAV_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 1 + + + 80, 86 + + + 78, 20 + + + 18 + + + XTRK_ANGLE_CD1 + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + NoControl + + + 6, 89 + + + 82, 13 + + + 19 + + + Error Max + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + XTRACK_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + XTRACK_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + XTRACK_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 7 + + + 182, 241 + + + 170, 110 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + + + 80, 63 + + + 78, 20 + + + 11 + + + THR_HOLD_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + THR_HOLD_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + THR_HOLD_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 6, 241 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 3 + + + 80, 61 + + + 78, 20 + + + 11 + + + HLD_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + NoControl + + + 6, 64 + + + 65, 13 + + + 12 + + + IMAX + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + HLD_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + HLD_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 4 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 5 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 6 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 7 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 8 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 9 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 10 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + AC2 + TabAC2 @@ -216,6 +4449,1072 @@ 1 + + NoControl + + + 30, 277 + + + 61, 13 + + + 37 + + + Waypoints + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + NoControl + + + 139, 276 + + + 177, 17 + + + 38 + + + Load Waypoints on connect? + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + NoControl + + + 30, 252 + + + 103, 18 + + + 36 + + + Track Length + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + 139, 250 + + + 67, 20 + + + 35 + + + 17, 17 + + + On the Flight Data Tab + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + NoControl + + + 579, 67 + + + 102, 17 + + + 34 + + + Alt Warning + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + NoControl + + + 30, 228 + + + 61, 13 + + + 0 + + + APM Reset + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + NoControl + + + 139, 227 + + + 163, 17 + + + 1 + + + Reset APM on USB Connect + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + Bottom, Left + + + NoControl + + + 33, 411 + + + 144, 17 + + + 2 + + + Mavlink Message Debug + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + NoControl + + + 590, 203 + + + 22, 13 + + + 3 + + + RC + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + 0 + + + 1 + + + 3 + + + 10 + + + 621, 200 + + + 80, 21 + + + 4 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + NoControl + + + 425, 203 + + + 69, 13 + + + 5 + + + Mode/Status + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + NoControl + + + 280, 203 + + + 44, 13 + + + 6 + + + Position + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + NoControl + + + 136, 203 + + + 43, 13 + + + 7 + + + Attitude + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + NoControl + + + 27, 203 + + + 84, 13 + + + 8 + + + Telemetry Rates + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + 0 + + + 1 + + + 3 + + + 10 + + + 499, 200 + + + 80, 21 + + + 9 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + 0 + + + 1 + + + 3 + + + 10 + + + 334, 200 + + + 80, 21 + + + 10 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + 0 + + + 1 + + + 3 + + + 10 + + + 188, 200 + + + 80, 21 + + + 11 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + NoControl + + + 283, 168 + + + 402, 13 + + + 12 + + + NOTE: The Configuration Tab will NOT display these units, as those are raw values. + + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + NoControl + + + 30, 176 + + + 65, 13 + + + 13 + + + Speed Units + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + NoControl + + + 30, 149 + + + 52, 13 + + + 14 + + + Dist Units + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + 139, 173 + + + 138, 21 + + + 15 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + 139, 146 + + + 138, 21 + + + 16 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + NoControl + + + 30, 122 + + + 45, 13 + + + 17 + + + Joystick + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + NoControl + + + 30, 71 + + + 44, 13 + + + 18 + + + Speech + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + NoControl + + + 471, 67 + + + 102, 17 + + + 19 + + + Battery Warning + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + NoControl + + + 378, 67 + + + 87, 17 + + + 20 + + + Time Interval + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + NoControl + + + 322, 67 + + + 56, 17 + + + 21 + + + Mode + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + NoControl + + + 245, 67 + + + 71, 17 + + + 22 + + + Waypoint + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + NoControl + + + 30, 47 + + + 57, 13 + + + 23 + + + OSD Color + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + 139, 40 + + + 138, 21 + + + 24 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + 139, 90 + + + 138, 21 + + + 25 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + NoControl + + + 30, 94 + + + 69, 13 + + + 26 + + + UI Language + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + NoControl + + + 139, 67 + + + 99, 17 + + + 27 + + + Enable Speech + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + NoControl + + + 552, 15 + + + 125, 17 + + + 28 + + + Enable HUD Overlay + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + NoControl + + + 30, 16 + + + 100, 23 + + + 29 + + + Video Device + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + 139, 13 + + + 245, 21 + + + 30 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + NoControl + + + 139, 117 + + + 99, 23 + + + 31 + + + Joystick Setup + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 36 + + + NoControl + + + 471, 11 + + + 75, 23 + + + 32 + + + Stop + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 37 + + + NoControl + + + 390, 11 + + + 75, 23 + + + 33 + + + Start + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 38 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + TabPlanner @@ -228,6 +5527,18 @@ 2 + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + TabSetup @@ -270,5389 +5581,6 @@ 2 - - groupBox3 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 0 - - - groupBox1 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 1 - - - groupBox2 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 2 - - - groupBox15 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 3 - - - groupBox16 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 4 - - - groupBox14 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 5 - - - groupBox13 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 6 - - - groupBox12 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 7 - - - groupBox11 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 8 - - - groupBox10 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 9 - - - groupBox9 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 10 - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - APM 2.x - - - THR_FS_VALUE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 0 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 1 - - - THR_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 2 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 3 - - - THR_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 4 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 5 - - - TRIM_THROTTLE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 6 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 7 - - - 405, 217 - - - 195, 108 - - - 0 - - - Throttle 0-100% - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 50, 13 - - - 12 - - - FS Value - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 27, 13 - - - 13 - - - Max - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 24, 13 - - - 14 - - - Min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 15 - - - Cruise - - - ARSPD_RATIO - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 0 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 1 - - - ARSPD_FBW_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 3 - - - ARSPD_FBW_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 4 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 5 - - - TRIM_ARSPD_CM - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 6 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 7 - - - 406, 325 - - - 195, 108 - - - 1 - - - Airspeed m/s - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 87 - - - 32, 13 - - - 1 - - - Ratio - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 59 - - - 53, 13 - - - 3 - - - FBW max - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 50, 13 - - - 5 - - - FBW min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 64, 13 - - - 6 - - - Cruise * 100 - - - LIM_PITCH_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 0 - - - label39 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 1 - - - LIM_PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 2 - - - label38 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 3 - - - LIM_ROLL_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 4 - - - label37 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 5 - - - 205, 325 - - - 195, 108 - - - 2 - - - Navigation Angles *100 - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 51, 13 - - - 10 - - - Pitch Min - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 54, 13 - - - 11 - - - Pitch Max - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 55, 13 - - - 12 - - - Bank Max - - - XTRK_ANGLE_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 0 - - - label79 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 1 - - - XTRK_GAIN_SC - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 2 - - - label80 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 3 - - - 4, 325 - - - 195, 108 - - - 3 - - - Xtrack Pids - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 8 - - - Entry Angle - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 52, 13 - - - 9 - - - Gain (cm) - - - KFF_PTCH2THR - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 0 - - - label83 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 1 - - - KFF_RDDRMIX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 2 - - - label78 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 3 - - - KFF_PTCHCOMP - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 4 - - - label81 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 5 - - - 205, 217 - - - 195, 108 - - - 4 - - - Other Mix's - - - 111, 13 - - - 78, 20 - - - 13 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 14 - - - P to T - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 61, 13 - - - 15 - - - Rudder Mix - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 16 - - - Pitch Comp - - - ENRGY2THR_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 0 - - - label73 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 1 - - - ENRGY2THR_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 2 - - - label74 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 3 - - - ENRGY2THR_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 4 - - - label75 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 5 - - - ENRGY2THR_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 6 - - - label76 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 7 - - - 4, 217 - - - 195, 108 - - - 5 - - - Energy/Alt Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - ALT2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 0 - - - label69 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 1 - - - ALT2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 2 - - - label70 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 3 - - - ALT2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 4 - - - label71 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 5 - - - ALT2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 6 - - - label72 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 7 - - - 406, 109 - - - 195, 108 - - - 6 - - - Nav Pitch Alt Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - ARSP2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 0 - - - label65 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 1 - - - ARSP2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 2 - - - label66 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 3 - - - ARSP2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 4 - - - label67 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 5 - - - ARSP2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 6 - - - label68 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 7 - - - 205, 109 - - - 195, 108 - - - 7 - - - Nav Pitch AS Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - HDNG2RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 0 - - - label61 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 1 - - - HDNG2RLL_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 2 - - - label62 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 3 - - - HDNG2RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 4 - - - label63 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 5 - - - HDNG2RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 6 - - - label64 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 7 - - - 4, 109 - - - 195, 108 - - - 8 - - - Nav Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - YW2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 0 - - - label57 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 1 - - - YW2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 2 - - - label58 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 3 - - - YW2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 4 - - - label59 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 5 - - - YW2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 6 - - - label60 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 7 - - - 406, 1 - - - 195, 108 - - - 9 - - - Servo Yaw Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - PTCH2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 0 - - - label53 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 1 - - - PTCH2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 2 - - - label54 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 3 - - - PTCH2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 4 - - - label55 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 5 - - - PTCH2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 6 - - - label56 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 7 - - - 205, 1 - - - 195, 108 - - - 10 - - - Servo Pitch Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - RLL2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 0 - - - label49 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 1 - - - RLL2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 2 - - - label50 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 3 - - - RLL2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 4 - - - label51 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 5 - - - RLL2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 6 - - - label52 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 7 - - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - CHK_lockrollpitch - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 0 - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 1 - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 2 - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 3 - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 4 - - - groupBox19 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 5 - - - groupBox20 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 6 - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 7 - - - groupBox22 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 8 - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 9 - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 10 - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 11 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - AC2 - - - True - - - 3, 198 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - - - WP_SPEED_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 0 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 1 - - - NAV_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 2 - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 3 - - - NAV_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 4 - - - label15 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 5 - - - NAV_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 6 - - - label16 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 7 - - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - 80, 86 - - - 78, 20 - - - 16 - - - NoControl - - - 6, 89 - - - 54, 13 - - - 17 - - - cm/s - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - XTRK_ANGLE_CD1 - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 0 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 1 - - - XTRACK_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 2 - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 3 - - - XTRACK_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 4 - - - label17 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 5 - - - XTRACK_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 6 - - - label18 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 7 - - - 182, 241 - - - 170, 110 - - - 2 - - - Crosstrack Correction - - - 80, 86 - - - 78, 20 - - - 18 - - - NoControl - - - 6, 89 - - - 82, 13 - - - 19 - - - Error Max * 100 - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - THR_HOLD_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 0 - - - label19 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 1 - - - THR_HOLD_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 2 - - - label21 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 3 - - - THR_HOLD_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 4 - - - label22 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 5 - - - 6, 241 - - - 170, 110 - - - 3 - - - Altitude Hold - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 0 - - - label27 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 1 - - - 525, 241 - - - 176, 110 - - - 5 - - - Other Mix's - - - 94, 17 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 20 - - - 82, 13 - - - 10 - - - Pitch Max * 100 - - - HLD_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 0 - - - label28 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 1 - - - HLD_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 2 - - - label30 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 3 - - - HLD_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 4 - - - label31 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 5 - - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - - - 80, 61 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 64 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 0 - - - label32 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 1 - - - STB_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 2 - - - label34 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 3 - - - STB_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 4 - - - label35 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 5 - - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 0 - - - label36 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 1 - - - STB_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 2 - - - label41 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 3 - - - STB_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 4 - - - label42 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 5 - - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 0 - - - label43 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 1 - - - STB_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 2 - - - label45 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 3 - - - STB_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 4 - - - label46 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 5 - - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - RATE_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 0 - - - label47 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 1 - - - RATE_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 2 - - - label77 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 3 - - - RATE_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 4 - - - label82 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 5 - - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 0 - - - label84 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 1 - - - RATE_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 2 - - - label86 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 3 - - - RATE_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 4 - - - label87 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 5 - - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 0 - - - label88 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 1 - - - RATE_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 2 - - - label90 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 3 - - - RATE_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 4 - - - label91 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 5 - - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 68, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - label24 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 0 - - - CHK_loadwponconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 1 - - - label23 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 2 - - - NUM_tracklength - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 3 - - - CHK_speechaltwarning - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 4 - - - label108 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 5 - - - CHK_resetapmonconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 6 - - - CHK_mavdebug - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 7 - - - label107 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 8 - - - CMB_raterc - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 9 - - - label104 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 10 - - - label103 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 11 - - - label102 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 12 - - - label101 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 13 - - - CMB_ratestatus - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 14 - - - CMB_rateposition - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 15 - - - CMB_rateattitude - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 16 - - - label99 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 17 - - - label98 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 18 - - - label97 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 19 - - - CMB_speedunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 20 - - - CMB_distunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 21 - - - label96 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 22 - - - label95 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 23 - - - CHK_speechbattery - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 24 - - - CHK_speechcustom - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 25 - - - CHK_speechmode - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 26 - - - CHK_speechwaypoint - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 27 - - - label94 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 28 - - - CMB_osdcolor - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 29 - - - CMB_language - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 30 - - - label93 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 31 - - - CHK_enablespeech - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 32 - - - CHK_hudshow - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 33 - - - label92 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 34 - - - CMB_videosources - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 35 - - - BUT_Joystick - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 36 - - - BUT_videostop - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 37 - - - BUT_videostart - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 38 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - NoControl - - - 30, 277 - - - 61, 13 - - - 37 - - - Waypoints - - - NoControl - - - 139, 276 - - - 177, 17 - - - 38 - - - Load Waypoints on connect? - - - NoControl - - - 30, 252 - - - 103, 18 - - - 36 - - - Track Length - - - 17, 17 - - - 139, 250 - - - 67, 20 - - - 35 - - - On the Flight Data Tab - - - NoControl - - - 579, 67 - - - 102, 17 - - - 34 - - - Alt Warning - - - NoControl - - - 30, 228 - - - 61, 13 - - - 0 - - - APM Reset - - - NoControl - - - 139, 227 - - - 163, 17 - - - 1 - - - Reset APM on USB Connect - - - Bottom, Left - - - NoControl - - - 33, 411 - - - 144, 17 - - - 2 - - - Mavlink Message Debug - - - NoControl - - - 590, 203 - - - 22, 13 - - - 3 - - - RC - - - 0 - - - 1 - - - 3 - - - 10 - - - 621, 200 - - - 80, 21 - - - 4 - - - NoControl - - - 425, 203 - - - 69, 13 - - - 5 - - - Mode/Status - - - NoControl - - - 280, 203 - - - 44, 13 - - - 6 - - - Position - - - NoControl - - - 136, 203 - - - 43, 13 - - - 7 - - - Attitude - - - NoControl - - - 27, 203 - - - 84, 13 - - - 8 - - - Telemetry Rates - - - 0 - - - 1 - - - 3 - - - 10 - - - 499, 200 - - - 80, 21 - - - 9 - - - 0 - - - 1 - - - 3 - - - 10 - - - 334, 200 - - - 80, 21 - - - 10 - - - 0 - - - 1 - - - 3 - - - 10 - - - 188, 200 - - - 80, 21 - - - 11 - - - NoControl - - - 283, 168 - - - 402, 13 - - - 12 - - - NOTE: The Configuration Tab will NOT display these units, as those are raw values. - - - - NoControl - - - 30, 176 - - - 65, 13 - - - 13 - - - Speed Units - - - NoControl - - - 30, 149 - - - 52, 13 - - - 14 - - - Dist Units - - - 139, 173 - - - 138, 21 - - - 15 - - - 139, 146 - - - 138, 21 - - - 16 - - - NoControl - - - 30, 122 - - - 45, 13 - - - 17 - - - Joystick - - - NoControl - - - 30, 71 - - - 44, 13 - - - 18 - - - Speech - - - NoControl - - - 471, 67 - - - 102, 17 - - - 19 - - - Battery Warning - - - NoControl - - - 378, 67 - - - 87, 17 - - - 20 - - - Time Interval - - - NoControl - - - 322, 67 - - - 56, 17 - - - 21 - - - Mode - - - NoControl - - - 245, 67 - - - 71, 17 - - - 22 - - - Waypoint - - - NoControl - - - 30, 47 - - - 57, 13 - - - 23 - - - OSD Color - - - 139, 40 - - - 138, 21 - - - 24 - - - 139, 90 - - - 138, 21 - - - 25 - - - NoControl - - - 30, 94 - - - 69, 13 - - - 26 - - - UI Language - - - NoControl - - - 139, 67 - - - 99, 17 - - - 27 - - - Enable Speech - - - NoControl - - - 552, 15 - - - 125, 17 - - - 28 - - - Enable HUD Overlay - - - NoControl - - - 30, 16 - - - 100, 23 - - - 29 - - - Video Device - - - 139, 13 - - - 245, 21 - - - 30 - - - NoControl - - - 139, 117 - - - 99, 23 - - - 31 - - - Joystick Setup - - - NoControl - - - 471, 11 - - - 75, 23 - - - 32 - - - Stop - - - NoControl - - - 390, 11 - - - 75, 23 - - - 33 - - - Start - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 1e7c2544c7..15f3a849f7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -519,13 +519,18 @@ namespace ArdupilotMega.GCSViews } catch (Exception ex) { lbl_status.Text = "Failed download"; MessageBox.Show("Failed to download new firmware : " + ex.Message); return; } + UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board); + } + + void UploadFlash(string filename, string board) + { byte[] FLASH = new byte[1]; StreamReader sr = null; try { lbl_status.Text = "Reading Hex File"; this.Refresh(); - sr = new StreamReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex"); + sr = new StreamReader(filename); FLASH = readIntelHEXv2(sr); sr.Close(); Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index 5e8c541c78..77bae958ef 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -436,10 +436,13 @@ namespace ArdupilotMega.GCSViews { this.BeginInvoke((MethodInvoker)delegate() { - tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100); - - lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%"); + try + { + tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100); + lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%"); + } + catch { } }); } @@ -861,6 +864,8 @@ namespace ArdupilotMega.GCSViews if (MainV2.comPort.logplaybackfile != null) MainV2.comPort.logplaybackfile.BaseStream.Position = (long)(MainV2.comPort.logplaybackfile.BaseStream.Length * (tracklog.Value / 100.0)); + + updateLogPlayPosition(); } bool loaded = false; @@ -1223,7 +1228,7 @@ namespace ArdupilotMega.GCSViews // Get the TypeCode enumeration. Multiple types get mapped to a common typecode. TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType()); - if (!(typeCode == TypeCode.Single || typeCode == TypeCode.Double || typeCode == TypeCode.Int32)) + if (!(typeCode == TypeCode.Single)) continue; CheckBox chk_box = new CheckBox(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 21732c373e..496882201c 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -347,7 +347,7 @@ namespace ArdupilotMega.GCSViews { TXT_mouselat.Text = lat.ToString(); TXT_mouselong.Text = lng.ToString(); - TXT_mousealt.Text = alt.ToString(); + TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0"); try { @@ -1228,7 +1228,7 @@ namespace ArdupilotMega.GCSViews if (CHK_holdalt.Checked) { - port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist * 100); + port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist); } else { @@ -1360,7 +1360,7 @@ namespace ArdupilotMega.GCSViews } } - TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist / 100).ToString("0"); + TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist).ToString("0"); TXT_WPRad.Text = ((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist).ToString("0"); try { diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index b4e8e178f4..a8746a1e1b 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -23,7 +23,7 @@ using OpenTK.Graphics; namespace hud { - public partial class HUD : GLControl + public class HUD : GLControl// : Graphics { object paintlock = new object(); object streamlock = new object(); @@ -38,7 +38,9 @@ namespace hud int[] charbitmaptexid = new int[6000]; int[] charwidth = new int[6000]; - int huddrawtime = 0; + public int huddrawtime = 0; + + public bool opengl = true; public HUD() { @@ -46,7 +48,7 @@ namespace hud return; InitializeComponent(); - graphicsObject = this; + //graphicsObject = this; //graphicsObject = Graphics.FromImage(objBitmap); } @@ -150,7 +152,6 @@ System.ComponentModel.Category("Values")] Bitmap objBitmap = new Bitmap(1024, 1024); int count = 0; DateTime countdate = DateTime.Now; - HUD graphicsObject; // Graphics DateTime starttime = DateTime.MinValue; @@ -225,51 +226,56 @@ System.ComponentModel.Category("Values")] { //GL.Enable(EnableCap.AlphaTest); - if (this.DesignMode) - { - e.Graphics.Clear(this.BackColor); - e.Graphics.Flush(); - return; + if (this.DesignMode) + { + e.Graphics.Clear(this.BackColor); + e.Graphics.Flush(); + return; } - - if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) + + if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) { //Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds); //e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); - return; + //return; } starttime = DateTime.Now; - //Console.WriteLine(DateTime.Now.Millisecond); + if (Console.CursorLeft > 0) + { + //Console.WriteLine(" "+ Console.CursorLeft +" "); + } + + //Console.Write("HUD a "+(DateTime.Now - starttime).TotalMilliseconds); MakeCurrent(); - //GL.LoadIdentity(); - - //GL.ClearColor(Color.Red); + //Console.Write(" b " + (DateTime.Now - starttime).TotalMilliseconds); GL.Clear(ClearBufferMask.ColorBufferBit); - //GL.LoadIdentity(); + //Console.Write(" c " + (DateTime.Now - starttime).TotalMilliseconds); - //GL.Viewport(0, 0, Width, Height); + doPaint(e); - doPaint(); + //Console.Write(" d " + (DateTime.Now - starttime).TotalMilliseconds); this.SwapBuffers(); + //Console.Write(" e " + (DateTime.Now - starttime).TotalMilliseconds); + + count++; + + huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds; + if (DateTime.Now.Second != countdate.Second) { countdate = DateTime.Now; - Console.WriteLine("HUD " + count + " hz drawtime " + huddrawtime); + Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count)); count = 0; + huddrawtime = 0; } - huddrawtime = (int)(DateTime.Now - starttime).TotalMilliseconds; -#if DEBUG - //Console.WriteLine("HUD e " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); -#endif - } void Clear(Color color) @@ -282,7 +288,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); - void DrawArc(Pen penn,RectangleF rect, float start,float degrees) + public void DrawArc(Pen penn,RectangleF rect, float start,float degrees) { //GL.Disable(EnableCap.Texture2D); @@ -309,7 +315,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawEllipse(Pen penn, Rectangle rect) + public void DrawEllipse(Pen penn, Rectangle rect) { //GL.Disable(EnableCap.Texture2D); @@ -340,7 +346,7 @@ System.ComponentModel.Category("Values")] int texture; Bitmap bitmap = new Bitmap(512,512); - void DrawImage(Image img, int x, int y, int width, int height) + public void DrawImage(Image img, int x, int y, int width, int height) { if (img == null) return; @@ -392,7 +398,7 @@ System.ComponentModel.Category("Values")] GL.Disable(EnableCap.Texture2D); } - void DrawPath(Pen penn,GraphicsPath gp) + public void DrawPath(Pen penn, GraphicsPath gp) { try { @@ -401,7 +407,7 @@ System.ComponentModel.Category("Values")] catch { } } - void FillPath(Brush brushh,GraphicsPath gp) + public void FillPath(Brush brushh, GraphicsPath gp) { try { @@ -410,32 +416,32 @@ System.ComponentModel.Category("Values")] catch { } } - void SetClip(Rectangle rect) + public void SetClip(Rectangle rect) { } - void ResetClip() + public void ResetClip() { } - void ResetTransform() + public void ResetTransform() { GL.LoadIdentity(); } - void RotateTransform(float angle) + public void RotateTransform(float angle) { GL.Rotate(angle,0,0,1); } - void TranslateTransform(float x, float y) + public void TranslateTransform(float x, float y) { GL.Translate(x, y, 0f); } - void FillPolygon(Brush brushh, Point[] list) + public void FillPolygon(Brush brushh, Point[] list) { //GL.Disable(EnableCap.Texture2D); @@ -456,7 +462,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void FillPolygon(Brush brushh, PointF[] list) + public void FillPolygon(Brush brushh, PointF[] list) { //GL.Disable(EnableCap.Texture2D); @@ -478,7 +484,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawPolygon(redPen, pointlist); - void DrawPolygon(Pen penn, Point[] list) + public void DrawPolygon(Pen penn, Point[] list) { //GL.Disable(EnableCap.Texture2D); @@ -499,7 +505,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawPolygon(Pen penn, PointF[] list) + public void DrawPolygon(Pen penn, PointF[] list) { //GL.Disable(EnableCap.Texture2D); @@ -523,7 +529,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.FillRectangle(linearBrush, bg); - void FillRectangle(Brush brushh,RectangleF rectf) + public void FillRectangle(Brush brushh, RectangleF rectf) { float x1 = rectf.X; float y1 = rectf.Y; @@ -571,12 +577,12 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); - void DrawRectangle(Pen penn, RectangleF rect) + public void DrawRectangle(Pen penn, RectangleF rect) { DrawRectangle(penn, rect.X, rect.Y, rect.Width, rect.Height); } - void DrawRectangle(Pen penn,double x1,double y1, double width,double height) + public void DrawRectangle(Pen penn, double x1, double y1, double width, double height) { //GL.Disable(EnableCap.Texture2D); @@ -597,7 +603,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawLine(Pen penn,double x1,double y1, double x2,double y2) + public void DrawLine(Pen penn, double x1, double y1, double x2, double y2) { //GL.Disable(EnableCap.Texture2D); @@ -616,8 +622,13 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void doPaint() + void doPaint(object e) { + HUD graphicsObject = this; + //Graphics graphicsObject = ((PaintEventArgs)e).Graphics; + //graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + + try { graphicsObject.Clear(Color.Gray); @@ -1055,7 +1066,7 @@ System.ComponentModel.Category("Values")] } } - + greenPen.Width = 4; // vsi @@ -1131,6 +1142,20 @@ System.ComponentModel.Category("Values")] drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5); drawstring(graphicsObject, (int)disttowp + ">" + wpno, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + fontsize + 2 + 10); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize) - 2 - 20); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 15, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize) - 2 - 20); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 10, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize ) - 2 - 20); + + drawstring(graphicsObject, ArdupilotMega.MainV2.cs.linkqualitygcs.ToString("0") + "%", font, fontsize, whiteBrush, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20); + if (ArdupilotMega.MainV2.cs.linkqualitygcs == 0) + { + graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2); + + graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20); + } + drawstring(graphicsObject, ArdupilotMega.MainV2.cs.datetime.ToString("HH:mm:ss"), font, fontsize, whiteBrush, scrollbg.Left - 20, scrollbg.Top - fontsize - 2 - 20); + + // battery graphicsObject.ResetTransform(); @@ -1191,8 +1216,6 @@ System.ComponentModel.Category("Values")] Console.WriteLine("hud error "+ex.ToString()); //MessageBox.Show(ex.ToString()); } - - count++; } protected override void OnPaintBackground(PaintEventArgs e) @@ -1355,6 +1378,31 @@ System.ComponentModel.Category("Values")] x += charwidth[charid] * scale; } } + + void drawstring(Graphics e, string text, Font font, float fontsize, Brush brush, float x, float y) + { + if (text == null || text == "") + return; + + pth.Reset(); + + + if (text != null) + pth.AddString(text, font.FontFamily, 0, fontsize + 5, new Point((int)x, (int)y), StringFormat.GenericTypographic); + + //Draw the edge + // this uses lots of cpu time + + //e.SmoothingMode = SmoothingMode.HighSpeed; + + e.DrawPath(P, pth); + + //Draw the face + + e.FillPath(brush, pth); + + //pth.Dispose(); + } protected override void OnResize(EventArgs e) { @@ -1365,6 +1413,18 @@ System.ComponentModel.Category("Values")] charbitmaps = new Bitmap[charbitmaps.Length]; + try + { + + foreach (int texid in charbitmaptexid) + { + if (texid != 0) + GL.DeleteTexture(texid); + } + + } + catch { } + GC.Collect(); try diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index e47cb2c35d..950c676b73 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -366,45 +366,6 @@ namespace ArdupilotMega } } - private byte[] readline(NetSerial comport) - { - byte[] temp = new byte[1024]; - int count = 0; - int timeout = 0; - - while (timeout <= 100) - { - if (!comport.IsOpen) { break; } - if (comport.BytesToRead > 0) - { - byte letter = (byte)comport.ReadByte(); - - temp[count] = letter; - - if (letter == '\n') // normal line - { - break; - } - - - count++; - if (count == temp.Length) - break; - timeout = 0; - } else { - timeout++; - System.Threading.Thread.Sleep(5); - } - } - if (timeout >= 100) { - Console.WriteLine("readline timeout"); - } - - Array.Resize(ref temp, count + 1); - - return temp; - } - List modelist = new List(); List[] position = new List[200]; int positionindex = 0; @@ -414,6 +375,13 @@ namespace ArdupilotMega { Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; + AltitudeMode altmode = AltitudeMode.absolute; + + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + { + altmode = AltitudeMode.relativeToGround; // because of sonar, this is both right and wrong. right for sonar, wrong in terms of gps as the land slopes off. + } + KMLRoot kml = new KMLRoot(); Folder fldr = new Folder("Log"); @@ -436,7 +404,7 @@ namespace ArdupilotMega continue; LineString ls = new LineString(); - ls.AltitudeMode = AltitudeMode.absolute; + ls.AltitudeMode = altmode; ls.Extrude = true; //ls.Tessellate = true; @@ -523,7 +491,7 @@ namespace ArdupilotMega pmplane.visibility = false; Model model = mod.model; - model.AltitudeMode = AltitudeMode.absolute; + model.AltitudeMode = altmode; model.Scale.x = 2; model.Scale.y = 2; model.Scale.z = 2; @@ -545,7 +513,7 @@ namespace ArdupilotMega catch { } pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude); - pmplane.Point.AltitudeMode = AltitudeMode.absolute; + pmplane.Point.AltitudeMode = altmode; Link link = new Link(); link.href = "block_plane_0.dae"; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 0c26ebb6cd..dc503e48e2 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -16,6 +16,9 @@ namespace ArdupilotMega { public ICommsSerial BaseStream = new SerialPort(); + /// + /// used for outbound packet sending + /// byte packetcount = 0; public byte sysid = 0; public byte compid = 0; @@ -29,6 +32,8 @@ namespace ArdupilotMega public DateTime lastvalidpacket = DateTime.Now; bool oldlogformat = false; + byte mavlinkversion = 0; + public PointLatLngAlt[] wps = new PointLatLngAlt[200]; public bool debugmavlink = false; @@ -45,7 +50,12 @@ namespace ArdupilotMega public int bps = 0; public DateTime bpstime = DateTime.Now; int recvpacketcount = 0; - int packetslost = 0; + + float synclost; + float packetslost = 0; + float packetsnotlost = 0; + DateTime packetlosttimer = DateTime.Now; + //Stopwatch stopwatch = new Stopwatch(); @@ -169,15 +179,25 @@ namespace ArdupilotMega if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4]) { + __mavlink_heartbeat_t hb = new __mavlink_heartbeat_t(); + + object temp = hb; + + MAVLink.ByteArrayToStructure(buffer, ref temp, 6); + + hb = (MAVLink.__mavlink_heartbeat_t)(temp); + + mavlinkversion = hb.mavlink_version; + sysid = buffer[3]; compid = buffer[4]; recvpacketcount = buffer[2]; - Console.WriteLine("ID " + sysid + " " + compid); + Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion); break; } } - frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + ") "; + frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") "; frm.Refresh(); if (getparams == true) getParamList(); @@ -190,6 +210,7 @@ namespace ArdupilotMega Console.WriteLine("Done open " + sysid + " " + compid); + packetslost = 0; } public static byte[] StructureToByteArrayEndian(params object[] list) @@ -331,12 +352,28 @@ namespace ArdupilotMega /// struct of data public void generatePacket(byte messageType, object indata) { - byte[] data = StructureToByteArrayEndian(indata); + byte[] data; + + if (mavlinkversion == 3) + { + data = StructureToByteArray(indata); + } + else + { + data = StructureToByteArrayEndian(indata); + } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; - packet[0] = (byte)'U'; + if (mavlinkversion == 3) + { + packet[0] = 254; + } + else if (mavlinkversion == 2) + { + packet[0] = (byte)'U'; + } packet[1] = (byte)data.Length; packet[2] = packetcount; packet[3] = 255; // this is always 255 - MYGCS @@ -351,6 +388,12 @@ namespace ArdupilotMega } ushort checksum = crc_calculate(packet, packet[1] + 6); + + if (mavlinkversion == 3) + { + checksum = crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum); + } + byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte @@ -425,6 +468,8 @@ namespace ArdupilotMega byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname); + modifyParamForDisplay(false, paramname, ref value); + Array.Resize(ref temp, 15); req.param_id = temp; @@ -458,7 +503,31 @@ namespace ArdupilotMega { if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { - param[paramname] = req.param_value; + __mavlink_param_value_t par = new __mavlink_param_value_t(); + + object tempobj = par; + + ByteArrayToStructure(buffer, ref tempobj, 6); + + par = (__mavlink_param_value_t)tempobj; + + string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); + + int pos = st.IndexOf('\0'); + + if (pos != -1) + { + st = st.Substring(0, pos); + } + + if (st != paramname) + { + Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st); + continue; + } + + param[st] = (par.param_value); + MainV2.givecomport = false; //System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte return true; @@ -559,6 +628,8 @@ namespace ArdupilotMega st = st.Substring(0, pos); } + modifyParamForDisplay(true, st, ref par.param_value); + param[st] = (par.param_value); a++; @@ -575,17 +646,21 @@ namespace ArdupilotMega return param; } - public _param getParam() + void modifyParamForDisplay(bool fromapm, string paramname, ref float value) { - throw new Exception("getParam Not implemented"); - /* - _param temp = new _param(); - - temp.name = "none"; - temp.value = 0; - - return temp; - */ + if (paramname.ToUpper().EndsWith("_IMAX") || paramname.ToUpper().EndsWith("ALT_HOLD_RTL") || paramname.ToUpper().EndsWith("TRIM_ARSPD_CM") + || paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN") + || paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX")) + { + if (fromapm) + { + value /= 100.0f; + } + else + { + value *= 100.0f; + } + } } /// @@ -951,6 +1026,8 @@ namespace ArdupilotMega req.seq = index; + //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); + // request generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); @@ -972,11 +1049,14 @@ namespace ArdupilotMega MainV2.givecomport = false; throw new Exception("Timeout on read - getWP"); } + //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); byte[] buffer = readPacket(); + //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT) { + //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); __mavlink_waypoint_t wp = new __mavlink_waypoint_t(); object temp = (object)wp; @@ -1070,7 +1150,7 @@ namespace ArdupilotMega } else { - //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); + Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } @@ -1412,12 +1492,13 @@ namespace ArdupilotMega } else { + MainV2.cs.datetime = DateTime.Now; temp[count] = (byte)BaseStream.ReadByte(); } } catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; } - if (temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync + if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync { if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n') { @@ -1432,7 +1513,7 @@ namespace ArdupilotMega // reset count on valid packet readcount = 0; - if (temp[0] == 'U') + if (temp[0] == 'U' || temp[0] == 254) { length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length if (count >= 5 || logreadmode) @@ -1455,16 +1536,18 @@ namespace ArdupilotMega else { int to = 0; - while (BaseStream.BytesToRead < length) + while (BaseStream.BytesToRead < (length - 4)) { if (to > 1000) { Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } - System.Threading.Thread.Sleep(2); + System.Threading.Thread.Sleep(1); System.Windows.Forms.Application.DoEvents(); to++; + + //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); } int read = BaseStream.Read(temp, 6, length - 4); } @@ -1484,9 +1567,18 @@ namespace ArdupilotMega Array.Resize(ref temp, count); + if (packetlosttimer.AddSeconds(10) < DateTime.Now) + { + packetlosttimer = DateTime.Now; + packetslost = (int)(packetslost * 0.8f); + packetsnotlost = (int)(packetsnotlost * 0.8f); + } + + MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100); + if (bpstime.Second != DateTime.Now.Second && !logreadmode) { - Console.WriteLine("bps {0} loss {1} left {2}", bps1, packetslost, BaseStream.BytesToRead); + Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; @@ -1498,11 +1590,16 @@ namespace ArdupilotMega if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet { - return new byte[0]; + return temp;// new byte[0]; } ushort crc = crc_calculate(temp, temp.Length - 2); + if (temp.Length > 5 && temp[0] == 254) + { + crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc); + } + if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff)) { int packetno = 0; @@ -1516,13 +1613,27 @@ namespace ArdupilotMega try { - if (temp[0] == 'U' && temp.Length >= temp[1]) + + if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1]) { if (temp[2] != ((recvpacketcount + 1) % 0x100)) { - Console.WriteLine("lost {0}", temp[2]); - packetslost++; // actualy sync loss's + synclost++; // actualy sync loss's + + if (temp[2] < ((recvpacketcount + 1) % 0x100)) + { + packetslost += 0x100 - recvpacketcount + temp[2]; + } + else + { + packetslost += temp[2] - recvpacketcount; + } + + Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost); } + + packetsnotlost++; + recvpacketcount = temp[2]; //MAVLINK_MSG_ID_GPS_STATUS @@ -1672,6 +1783,8 @@ namespace ArdupilotMega lastlogread = date1; + MainV2.cs.datetime = lastlogread; + int length = 5; int a = 0; while (a < length) @@ -1751,6 +1864,34 @@ namespace ArdupilotMega } public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset) + { + if (bytearray[0] == 'U') + { + ByteArrayToStructureEndian(bytearray, ref obj, startoffset); + } + else + { + int len = Marshal.SizeOf(obj); + + IntPtr i = Marshal.AllocHGlobal(len); + + // create structure from ptr + obj = Marshal.PtrToStructure(i, obj.GetType()); + + try + { + // copy byte array to ptr + Marshal.Copy(bytearray, startoffset, i, len); + } + catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); } + + obj = Marshal.PtrToStructure(i, obj.GetType()); + + Marshal.FreeHGlobal(i); + } + } + + public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset) { int len = Marshal.SizeOf(obj); @@ -1779,51 +1920,12 @@ namespace ArdupilotMega TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType()); if (typeCode != TypeCode.Object) - {/* - switch (Marshal.SizeOf(fieldValue)) - { - case 1: - Marshal.WriteByte(i, reversestartoffset - 6, bytearray[reversestartoffset]); - break; - case 2: - byte[] temp = new byte[2]; - temp[0] = bytearray[reversestartoffset + 1]; - temp[1] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt16(i, reversestartoffset - 6, BitConverter.ToInt16(temp, 0)); - break; - case 4: - byte[] temp2 = new byte[4]; - temp2[0] = bytearray[reversestartoffset + 3]; - temp2[1] = bytearray[reversestartoffset + 2]; - temp2[2] = bytearray[reversestartoffset + 1]; - temp2[3] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt32(i, reversestartoffset - 6, BitConverter.ToInt32(temp2, 0)); - break; - case 8: - byte[] temp3 = new byte[8]; - temp3[0] = bytearray[reversestartoffset + 7]; - temp3[1] = bytearray[reversestartoffset + 6]; - temp3[2] = bytearray[reversestartoffset + 5]; - temp3[3] = bytearray[reversestartoffset + 4]; - temp3[4] = bytearray[reversestartoffset + 3]; - temp3[5] = bytearray[reversestartoffset + 2]; - temp3[6] = bytearray[reversestartoffset + 1]; - temp3[7] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt64(i, reversestartoffset - 6, BitConverter.ToInt64(temp3, 0)); - break; - default: - Console.WriteLine("bytearraytostruct Bad value"); - break; - } */ + { Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue)); reversestartoffset += Marshal.SizeOf(fieldValue); } else { - /* - for (int c = 0; c < ((byte[])fieldValue).Length;c++) - Marshal.WriteByte(i, c, bytearray[reversestartoffset + c]); - */ reversestartoffset += ((byte[])fieldValue).Length; } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs index 66c29af6a3..53ac881f15 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs @@ -7,864 +7,1346 @@ namespace ArdupilotMega { partial class MAVLink { - public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_sensor_offsets_t - { - public short mag_ofs_x; ///< magnetometer X offset - public short mag_ofs_y; ///< magnetometer Y offset - public short mag_ofs_z; ///< magnetometer Z offset - public float mag_declination; ///< magnetic declination (radians) - public int raw_press; ///< raw pressure from barometer - public int raw_temp; ///< raw temperature from barometer - public float gyro_cal_x; ///< gyro X calibration - public float gyro_cal_y; ///< gyro Y calibration - public float gyro_cal_z; ///< gyro Z calibration - public float accel_cal_x; ///< accel X calibration - public float accel_cal_y; ///< accel Y calibration - public float accel_cal_z; ///< accel Z calibration - }; - - public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_mag_offsets_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public short mag_ofs_x; ///< magnetometer X offset - public short mag_ofs_y; ///< magnetometer Y offset - public short mag_ofs_z; ///< magnetometer Z offset - }; - + public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}; + public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}; public const byte MAVLINK_MSG_ID_MEMINFO = 152; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_meminfo_t { - public ushort brkval; ///< heap top - public ushort freemem; ///< free memory + public ushort brkval; ///< heap top + public ushort freemem; ///< free memory }; - public enum MAV_CMD - { - WAYPOINT = 16, - LOITER_UNLIM = 17, - LOITER_TURNS = 18, - LOITER_TIME = 19, - RETURN_TO_LAUNCH = 20, - LAND = 21, - TAKEOFF = 22, - ROI = 80, - PATHPLANNING = 81, - LAST = 95, - CONDITION_DELAY = 112, - CONDITION_CHANGE_ALT = 113, - CONDITION_DISTANCE = 114, - CONDITION_YAW = 115, - CONDITION_LAST = 159, - DO_SET_MODE = 176, - DO_JUMP = 177, - DO_CHANGE_SPEED = 178, - DO_SET_HOME = 179, - DO_SET_PARAMETER = 180, - DO_SET_RELAY = 181, - DO_REPEAT_RELAY = 182, - DO_SET_SERVO = 183, - DO_REPEAT_SERVO = 184, - DO_CONTROL_VIDEO = 200, - DO_SET_ROI = 201, - DO_LAST = 240, - PREFLIGHT_CALIBRATION = 241, - PREFLIGHT_STORAGE = 245, - }; - - public enum MAV_DATA_STREAM - { - MAV_DATA_STREAM_ALL = 0, - MAV_DATA_STREAM_RAW_SENSORS = 1, - MAV_DATA_STREAM_EXTENDED_STATUS = 2, - MAV_DATA_STREAM_RC_CHANNELS = 3, - MAV_DATA_STREAM_RAW_CONTROLLER = 4, - MAV_DATA_STREAM_POSITION = 6, - MAV_DATA_STREAM_EXTRA1 = 10, - MAV_DATA_STREAM_EXTRA2 = 11, - MAV_DATA_STREAM_EXTRA3 = 12, - }; - - public enum MAV_ROI - { - MAV_ROI_NONE = 0, - MAV_ROI_WPNEXT = 1, - MAV_ROI_WPINDEX = 2, - MAV_ROI_LOCATION = 3, - MAV_ROI_TARGET = 4, - }; - - public const byte MAVLINK_MSG_ID_HEARTBEAT = 0; + public const byte MAVLINK_MSG_ID_MEMINFO_LEN = 4; + public const byte MAVLINK_MSG_ID_152_LEN = 4; + public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_heartbeat_t + public struct __mavlink_sensor_offsets_t { - public byte type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - public byte autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - public byte mavlink_version; ///< MAVLink version + public short mag_ofs_x; ///< magnetometer X offset + public short mag_ofs_y; ///< magnetometer Y offset + public short mag_ofs_z; ///< magnetometer Z offset + public float mag_declination; ///< magnetic declination (radians) + public int raw_press; ///< raw pressure from barometer + public int raw_temp; ///< raw temperature from barometer + public float gyro_cal_x; ///< gyro X calibration + public float gyro_cal_y; ///< gyro Y calibration + public float gyro_cal_z; ///< gyro Z calibration + public float accel_cal_x; ///< accel X calibration + public float accel_cal_y; ///< accel Y calibration + public float accel_cal_z; ///< accel Z calibration }; - public const byte MAVLINK_MSG_ID_BOOT = 1; + public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN = 42; + public const byte MAVLINK_MSG_ID_150_LEN = 42; + public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_boot_t + public struct __mavlink_set_mag_offsets_t { - public uint version; ///< The onboard software version + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public short mag_ofs_x; ///< magnetometer X offset + public short mag_ofs_y; ///< magnetometer Y offset + public short mag_ofs_z; ///< magnetometer Z offset }; - public const byte MAVLINK_MSG_ID_SYSTEM_TIME = 2; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_system_time_t + public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN = 8; + public const byte MAVLINK_MSG_ID_151_LEN = 8; + public enum MAV_CMD { - public ulong time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + WAYPOINT=16, /* Navigate to waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing)| Latitude| Longitude| Altitude| */ + LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + ROI=80, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + devices such as cameras. + |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty| */ + PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + ENUM_END=246, /* | */ }; - public const byte MAVLINK_MSG_ID_PING = 3; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_ping_t + public enum MAV_DATA_STREAM { - public uint seq; ///< PING sequence - public byte target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - public byte target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - public ulong time; ///< Unix timestamp in microseconds + MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END=13, /* | */ }; - public const byte MAVLINK_MSG_ID_SYSTEM_TIME_UTC = 4; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_system_time_utc_t + public enum MAV_ROI { - public uint utc_date; ///< GPS UTC date ddmmyy - public uint utc_time; ///< GPS UTC time hhmmss - }; - - public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_change_operator_control_t - { - public byte target_system; ///< System the GCS requests control for - public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - public byte version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - [MarshalAs(UnmanagedType.ByValArray, SizeConst=25)] - char passkey; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - }; - - public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_change_operator_control_ack_t - { - public byte gcs_system_id; ///< ID of the GCS this message - public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - public byte ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - }; - - public const byte MAVLINK_MSG_ID_AUTH_KEY = 7; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_auth_key_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=32)] - char key; ///< key - }; - - public const byte MAVLINK_MSG_ID_ACTION_ACK = 9; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_action_ack_t - { - public byte action; ///< The action id - public byte result; ///< 0: Action DENIED, 1: Action executed + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ }; public const byte MAVLINK_MSG_ID_ACTION = 10; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_action_t { - public byte target; ///< The system executing the action - public byte target_component; ///< The component executing the action - public byte action; ///< The action id + public byte target; ///< The system executing the action + public byte target_component; ///< The component executing the action + public byte action; ///< The action id }; - public const byte MAVLINK_MSG_ID_SET_MODE = 11; + public const byte MAVLINK_MSG_ID_ACTION_LEN = 3; + public const byte MAVLINK_MSG_ID_10_LEN = 3; + public const byte MAVLINK_MSG_ID_ACTION_ACK = 9; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_mode_t + public struct __mavlink_action_ack_t { - public byte target; ///< The system setting the mode - public byte mode; ///< The new mode - }; - - public const byte MAVLINK_MSG_ID_SET_NAV_MODE = 12; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_nav_mode_t - { - public byte target; ///< The system setting the mode - public byte nav_mode; ///< The new navigation mode - }; - - public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_request_read_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public short param_index; ///< Parameter index. Send -1 to use the param ID field as identifier - }; - - public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_request_list_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_PARAM_VALUE = 22; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_value_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public float param_value; ///< Onboard parameter value - public ushort param_count; ///< Total number of onboard parameters - public ushort param_index; ///< Index of this onboard parameter - }; - - public const byte MAVLINK_MSG_ID_PARAM_SET = 23; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_set_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public float param_value; ///< Onboard parameter value - }; - - public const byte MAVLINK_MSG_ID_GPS_RAW_INT = 25; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_raw_int_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - public int lat; ///< Latitude in 1E7 degrees - public int lon; ///< Longitude in 1E7 degrees - public int alt; ///< Altitude in 1E3 meters (millimeters) - public float eph; ///< GPS HDOP - public float epv; ///< GPS VDOP - public float v; ///< GPS ground speed (m/s) - public float hdg; ///< Compass heading in degrees, 0..360 degrees - }; - - public const byte MAVLINK_MSG_ID_SCALED_IMU = 26; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_scaled_imu_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short xacc; ///< X acceleration (mg) - public short yacc; ///< Y acceleration (mg) - public short zacc; ///< Z acceleration (mg) - public short xgyro; ///< Angular speed around X axis (millirad /sec) - public short ygyro; ///< Angular speed around Y axis (millirad /sec) - public short zgyro; ///< Angular speed around Z axis (millirad /sec) - public short xmag; ///< X Magnetic field (milli tesla) - public short ymag; ///< Y Magnetic field (milli tesla) - public short zmag; ///< Z Magnetic field (milli tesla) - }; - - public const byte MAVLINK_MSG_ID_GPS_STATUS = 27; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_status_t - { - public byte satellites_visible; ///< Number of satellites visible - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_prn; ///< Global satellite ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_used; ///< 0: Satellite not used, 1: used for localization - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_elevation; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_azimuth; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_snr; ///< Signal to noise ratio of satellite - }; - - public const byte MAVLINK_MSG_ID_RAW_IMU = 28; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_raw_imu_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short xacc; ///< X acceleration (raw) - public short yacc; ///< Y acceleration (raw) - public short zacc; ///< Z acceleration (raw) - public short xgyro; ///< Angular speed around X axis (raw) - public short ygyro; ///< Angular speed around Y axis (raw) - public short zgyro; ///< Angular speed around Z axis (raw) - public short xmag; ///< X Magnetic field (raw) - public short ymag; ///< Y Magnetic field (raw) - public short zmag; ///< Z Magnetic field (raw) - }; - - public const byte MAVLINK_MSG_ID_RAW_PRESSURE = 29; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_raw_pressure_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short press_abs; ///< Absolute pressure (raw) - public short press_diff1; ///< Differential pressure 1 (raw) - public short press_diff2; ///< Differential pressure 2 (raw) - public short temperature; ///< Raw Temperature measurement (raw) - }; - - public const byte MAVLINK_MSG_ID_SCALED_PRESSURE = 38; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_scaled_pressure_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float press_abs; ///< Absolute pressure (hectopascal) - public float press_diff; ///< Differential pressure 1 (hectopascal) - public short temperature; ///< Temperature measurement (0.01 degrees celsius) + public byte action; ///< The action id + public byte result; ///< 0: Action DENIED, 1: Action executed }; + public const byte MAVLINK_MSG_ID_ACTION_ACK_LEN = 2; + public const byte MAVLINK_MSG_ID_9_LEN = 2; public const byte MAVLINK_MSG_ID_ATTITUDE = 30; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_attitude_t { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float roll; ///< Roll angle (rad) - public float pitch; ///< Pitch angle (rad) - public float yaw; ///< Yaw angle (rad) - public float rollspeed; ///< Roll angular speed (rad/s) - public float pitchspeed; ///< Pitch angular speed (rad/s) - public float yawspeed; ///< Yaw angular speed (rad/s) + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) }; - public const byte MAVLINK_MSG_ID_LOCAL_POSITION = 31; + public const byte MAVLINK_MSG_ID_ATTITUDE_LEN = 32; + public const byte MAVLINK_MSG_ID_30_LEN = 32; + public const byte MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT = 60; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_local_position_t + public struct __mavlink_attitude_controller_output_t + { + public byte enabled; ///< 1: enabled, 0: disabled + public byte roll; ///< Attitude roll: -128: -100%, 127: +100% + public byte pitch; ///< Attitude pitch: -128: -100%, 127: +100% + public byte yaw; ///< Attitude yaw: -128: -100%, 127: +100% + public byte thrust; ///< Attitude thrust: -128: -100%, 127: +100% + + }; + + public const byte MAVLINK_MSG_ID_ATTITUDE_NEW = 30; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_attitude_new_t { public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float x; ///< X Position - public float y; ///< Y Position - public float z; ///< Z Position - public float vx; ///< X Speed - public float vy; ///< Y Speed - public float vz; ///< Z Speed + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + + }; + + public const byte MAVLINK_MSG_ID_AUTH_KEY = 7; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_auth_key_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=32)] + char key; ///< key + }; + + public const byte MAVLINK_MSG_ID_AUTH_KEY_LEN = 32; + public const byte MAVLINK_MSG_ID_7_LEN = 32; + public const byte MAVLINK_MSG_ID_BOOT = 1; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_boot_t + { + public uint version; ///< The onboard software version + }; + + public const byte MAVLINK_MSG_ID_BOOT_LEN = 4; + public const byte MAVLINK_MSG_ID_1_LEN = 4; + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_change_operator_control_t + { + public byte target_system; ///< System the GCS requests control for + public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + public byte version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=25)] + char passkey; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + }; + + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN = 28; + public const byte MAVLINK_MSG_ID_5_LEN = 28; + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_change_operator_control_ack_t + { + public byte gcs_system_id; ///< ID of the GCS this message + public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + public byte ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + }; + + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN = 3; + public const byte MAVLINK_MSG_ID_6_LEN = 3; + public const byte MAVLINK_MSG_ID_COMMAND = 75; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_command_t + { + public byte target_system; ///< System which should execute the command + public byte target_component; ///< Component which should execute the command, 0 for all components + public byte command; ///< Command ID, as defined by MAV_CMD enum. + public byte confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + public float param1; ///< Parameter 1, as defined by MAV_CMD enum. + public float param2; ///< Parameter 2, as defined by MAV_CMD enum. + public float param3; ///< Parameter 3, as defined by MAV_CMD enum. + public float param4; ///< Parameter 4, as defined by MAV_CMD enum. + }; + + public const byte MAVLINK_MSG_ID_COMMAND_LEN = 20; + public const byte MAVLINK_MSG_ID_75_LEN = 20; + public const byte MAVLINK_MSG_ID_COMMAND_ACK = 76; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_command_ack_t + { + public float command; ///< Current airspeed in m/s + public float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + }; + + public const byte MAVLINK_MSG_ID_COMMAND_ACK_LEN = 8; + public const byte MAVLINK_MSG_ID_76_LEN = 8; + public const byte MAVLINK_MSG_ID_CONTROL_STATUS = 52; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_control_status_t + { + public byte position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + public byte vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + public byte gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + public byte ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent + public byte control_att; ///< 0: Attitude control disabled, 1: enabled + public byte control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled + public byte control_pos_z; ///< 0: Z position control disabled, 1: enabled + public byte control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + }; + + public const byte MAVLINK_MSG_ID_CONTROL_STATUS_LEN = 8; + public const byte MAVLINK_MSG_ID_52_LEN = 8; + public const byte MAVLINK_MSG_ID_DEBUG = 255; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_debug_t + { + public byte ind; ///< index of debug variable + public float value; ///< DEBUG value + }; + + public const byte MAVLINK_MSG_ID_DEBUG_LEN = 5; + public const byte MAVLINK_MSG_ID_255_LEN = 5; + public const byte MAVLINK_MSG_ID_DEBUG_VECT = 251; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_debug_vect_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name + public ulong usec; ///< Timestamp + public float x; ///< x + public float y; ///< y + public float z; ///< z + }; + + public const byte MAVLINK_MSG_ID_DEBUG_VECT_LEN = 30; + public const byte MAVLINK_MSG_ID_251_LEN = 30; + public const byte MAVLINK_MSG_ID_FULL_STATE = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_full_state_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + }; public const byte MAVLINK_MSG_ID_GLOBAL_POSITION = 33; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_global_position_t { - public ulong usec; ///< Timestamp (microseconds since unix epoch) - public float lat; ///< Latitude, in degrees - public float lon; ///< Longitude, in degrees - public float alt; ///< Absolute altitude, in meters - public float vx; ///< X Speed (in Latitude direction, positive: going north) - public float vy; ///< Y Speed (in Longitude direction, positive: going east) - public float vz; ///< Z Speed (in Altitude direction, positive: going up) + public ulong usec; ///< Timestamp (microseconds since unix epoch) + public float lat; ///< Latitude, in degrees + public float lon; ///< Longitude, in degrees + public float alt; ///< Absolute altitude, in meters + public float vx; ///< X Speed (in Latitude direction, positive: going north) + public float vy; ///< Y Speed (in Longitude direction, positive: going east) + public float vz; ///< Z Speed (in Altitude direction, positive: going up) }; - public const byte MAVLINK_MSG_ID_GPS_RAW = 32; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_LEN = 32; + public const byte MAVLINK_MSG_ID_33_LEN = 32; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 73; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_raw_t + public struct __mavlink_global_position_int_t { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - public float lat; ///< Latitude in degrees - public float lon; ///< Longitude in degrees - public float alt; ///< Altitude in meters - public float eph; ///< GPS HDOP - public float epv; ///< GPS VDOP - public float v; ///< GPS ground speed - public float hdg; ///< Compass heading in degrees, 0..360 degrees - }; - - public const byte MAVLINK_MSG_ID_SYS_STATUS = 34; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_sys_status_t - { - public byte mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - public byte nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - public byte status; ///< System status flag, see MAV_STATUS ENUM - public ushort load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - public ushort vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) - public ushort battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) - public ushort packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) - }; - - public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_rc_channels_raw_t - { - public ushort chan1_raw; ///< RC channel 1 value, in microseconds - public ushort chan2_raw; ///< RC channel 2 value, in microseconds - public ushort chan3_raw; ///< RC channel 3 value, in microseconds - public ushort chan4_raw; ///< RC channel 4 value, in microseconds - public ushort chan5_raw; ///< RC channel 5 value, in microseconds - public ushort chan6_raw; ///< RC channel 6 value, in microseconds - public ushort chan7_raw; ///< RC channel 7 value, in microseconds - public ushort chan8_raw; ///< RC channel 8 value, in microseconds - public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - }; - - public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 36; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_rc_channels_scaled_t - { - public short chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - }; - - public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 37; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_servo_output_raw_t - { - public ushort servo1_raw; ///< Servo output 1 value, in microseconds - public ushort servo2_raw; ///< Servo output 2 value, in microseconds - public ushort servo3_raw; ///< Servo output 3 value, in microseconds - public ushort servo4_raw; ///< Servo output 4 value, in microseconds - public ushort servo5_raw; ///< Servo output 5 value, in microseconds - public ushort servo6_raw; ///< Servo output 6 value, in microseconds - public ushort servo7_raw; ///< Servo output 7 value, in microseconds - public ushort servo8_raw; ///< Servo output 8 value, in microseconds - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT = 39; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - public byte frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - public byte command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - public byte current; ///< false:0, true:1 - public byte autocontinue; ///< autocontinue to next wp - public float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - public float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - public float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - public float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - public float x; ///< PARAM5 / local: x position, global: latitude - public float y; ///< PARAM6 / y position: global: longitude - public float z; ///< PARAM7 / z position: global: altitude - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST = 40; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_request_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT = 41; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_set_current_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT = 42; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_current_t - { - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST = 43; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_request_list_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT = 44; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_count_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort count; ///< Number of Waypoints in the Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL = 45; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_clear_all_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED = 46; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_reached_t - { - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_ACK = 47; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_ack_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public byte type; ///< 0: OK, 1: Error - }; - - public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN = 48; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_set_global_origin_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public int latitude; ///< global position * 1E7 - public int longitude; ///< global position * 1E7 - public int altitude; ///< global position * 1000 + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 }; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN = 18; + public const byte MAVLINK_MSG_ID_73_LEN = 18; public const byte MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET = 49; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_gps_local_origin_set_t { - public int latitude; ///< Latitude (WGS84), expressed as * 1E7 - public int longitude; ///< Longitude (WGS84), expressed as * 1E7 - public int altitude; ///< Altitude(WGS84), expressed as * 1000 + public int latitude; ///< Latitude (WGS84), expressed as * 1E7 + public int longitude; ///< Longitude (WGS84), expressed as * 1E7 + public int altitude; ///< Altitude(WGS84), expressed as * 1000 }; - public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET = 50; + public const byte MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN = 12; + public const byte MAVLINK_MSG_ID_49_LEN = 12; + public const byte MAVLINK_MSG_ID_GPS_RAW = 32; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_local_position_setpoint_set_t + public struct __mavlink_gps_raw_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< Desired yaw angle + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + public float lat; ///< Latitude in degrees + public float lon; ///< Longitude in degrees + public float alt; ///< Altitude in meters + public float eph; ///< GPS HDOP + public float epv; ///< GPS VDOP + public float v; ///< GPS ground speed + public float hdg; ///< Compass heading in degrees, 0..360 degrees }; + public const byte MAVLINK_MSG_ID_GPS_RAW_LEN = 37; + public const byte MAVLINK_MSG_ID_32_LEN = 37; + public const byte MAVLINK_MSG_ID_GPS_RAW_INT = 25; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_raw_int_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + public int lat; ///< Latitude in 1E7 degrees + public int lon; ///< Longitude in 1E7 degrees + public int alt; ///< Altitude in 1E3 meters (millimeters) + public float eph; ///< GPS HDOP + public float epv; ///< GPS VDOP + public float v; ///< GPS ground speed (m/s) + public float hdg; ///< Compass heading in degrees, 0..360 degrees + }; + + public const byte MAVLINK_MSG_ID_GPS_RAW_INT_LEN = 37; + public const byte MAVLINK_MSG_ID_25_LEN = 37; + public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN = 48; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_set_global_origin_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public int latitude; ///< global position * 1E7 + public int longitude; ///< global position * 1E7 + public int altitude; ///< global position * 1000 + }; + + public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN = 14; + public const byte MAVLINK_MSG_ID_48_LEN = 14; + public const byte MAVLINK_MSG_ID_GPS_STATUS = 27; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_status_t + { + public byte satellites_visible; ///< Number of satellites visible + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_prn; ///< Global satellite ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_used; ///< 0: Satellite not used, 1: used for localization + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_elevation; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_azimuth; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_snr; ///< Signal to noise ratio of satellite + }; + + public const byte MAVLINK_MSG_ID_GPS_STATUS_LEN = 101; + public const byte MAVLINK_MSG_ID_27_LEN = 101; + public const byte MAVLINK_MSG_ID_HEARTBEAT = 0; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_heartbeat_t + { + public byte type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + public byte autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + public byte mavlink_version; ///< MAVLink version + }; + + public const byte MAVLINK_MSG_ID_HEARTBEAT_LEN = 3; + public const byte MAVLINK_MSG_ID_0_LEN = 3; + public const byte MAVLINK_MSG_ID_HIL_CONTROLS = 68; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_hil_controls_t + { + public ulong time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll_ailerons; ///< Control output -3 .. 1 + public float pitch_elevator; ///< Control output -1 .. 1 + public float yaw_rudder; ///< Control output -1 .. 1 + public float throttle; ///< Throttle 0 .. 1 + public byte mode; ///< System mode (MAV_MODE) + public byte nav_mode; ///< Navigation mode (MAV_NAV_MODE) + }; + + public const byte MAVLINK_MSG_ID_HIL_CONTROLS_LEN = 26; + public const byte MAVLINK_MSG_ID_68_LEN = 26; + public const byte MAVLINK_MSG_ID_HIL_STATE = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_hil_state_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + }; + + public const byte MAVLINK_MSG_ID_HIL_STATE_LEN = 56; + public const byte MAVLINK_MSG_ID_67_LEN = 56; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION = 31; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_local_position_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float x; ///< X Position + public float y; ///< Y Position + public float z; ///< Z Position + public float vx; ///< X Speed + public float vy; ///< Y Speed + public float vz; ///< Z Speed + }; + + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_LEN = 32; + public const byte MAVLINK_MSG_ID_31_LEN = 32; public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT = 51; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_local_position_setpoint_t { - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< Desired yaw angle + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< Desired yaw angle }; - public const byte MAVLINK_MSG_ID_CONTROL_STATUS = 52; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN = 16; + public const byte MAVLINK_MSG_ID_51_LEN = 16; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET = 50; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_control_status_t + public struct __mavlink_local_position_setpoint_set_t { - public byte position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - public byte vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - public byte gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - public byte ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent - public byte control_att; ///< 0: Attitude control disabled, 1: enabled - public byte control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled - public byte control_pos_z; ///< 0: Z position control disabled, 1: enabled - public byte control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< Desired yaw angle }; - public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 53; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN = 18; + public const byte MAVLINK_MSG_ID_50_LEN = 18; + public const byte MAVLINK_MSG_ID_MANUAL_CONTROL = 69; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_safety_set_allowed_area_t + public struct __mavlink_manual_control_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - public float p1x; ///< x position 1 / Latitude 1 - public float p1y; ///< y position 1 / Longitude 1 - public float p1z; ///< z position 1 / Altitude 1 - public float p2x; ///< x position 2 / Latitude 2 - public float p2y; ///< y position 2 / Longitude 2 - public float p2z; ///< z position 2 / Altitude 2 + public byte target; ///< The system to be controlled + public float roll; ///< roll + public float pitch; ///< pitch + public float yaw; ///< yaw + public float thrust; ///< thrust + public byte roll_manual; ///< roll control enabled auto:0, manual:1 + public byte pitch_manual; ///< pitch auto:0, manual:1 + public byte yaw_manual; ///< yaw auto:0, manual:1 + public byte thrust_manual; ///< thrust auto:0, manual:1 }; - public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 54; + public const byte MAVLINK_MSG_ID_MANUAL_CONTROL_LEN = 21; + public const byte MAVLINK_MSG_ID_69_LEN = 21; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 252; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_safety_allowed_area_t + public struct __mavlink_named_value_float_t { - public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - public float p1x; ///< x position 1 / Latitude 1 - public float p1y; ///< y position 1 / Longitude 1 - public float p1z; ///< z position 1 / Altitude 1 - public float p2x; ///< x position 2 / Latitude 2 - public float p2y; ///< y position 2 / Longitude 2 - public float p2z; ///< z position 2 / Altitude 2 + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name of the debug variable + public float value; ///< Floating point value }; - public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 55; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN = 14; + public const byte MAVLINK_MSG_ID_252_LEN = 14; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT = 253; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_roll_pitch_yaw_thrust_t + public struct __mavlink_named_value_int_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float roll; ///< Desired roll angle in radians - public float pitch; ///< Desired pitch angle in radians - public float yaw; ///< Desired yaw angle in radians - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 56; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_roll_pitch_yaw_speed_thrust_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float roll_speed; ///< Desired roll angular speed in rad/s - public float pitch_speed; ///< Desired pitch angular speed in rad/s - public float yaw_speed; ///< Desired yaw angular speed in rad/s - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 57; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_roll_pitch_yaw_thrust_setpoint_t - { - public uint time_ms; ///< Timestamp in milliseconds since system boot - public float roll; ///< Desired roll angle in radians - public float pitch; ///< Desired pitch angle in radians - public float yaw; ///< Desired yaw angle in radians - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 58; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t - { - public uint time_ms; ///< Timestamp in milliseconds since system boot - public float roll_speed; ///< Desired roll angular speed in rad/s - public float pitch_speed; ///< Desired pitch angular speed in rad/s - public float yaw_speed; ///< Desired yaw angular speed in rad/s - public float thrust; ///< Collective thrust, normalized to 0 .. 1 + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name of the debug variable + public int value; ///< Signed integer value }; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN = 14; + public const byte MAVLINK_MSG_ID_253_LEN = 14; public const byte MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_nav_controller_output_t { - public float nav_roll; ///< Current desired roll in degrees - public float nav_pitch; ///< Current desired pitch in degrees - public short nav_bearing; ///< Current desired heading in degrees - public short target_bearing; ///< Bearing to current waypoint/target in degrees - public ushort wp_dist; ///< Distance to active waypoint in meters - public float alt_error; ///< Current altitude error in meters - public float aspd_error; ///< Current airspeed error in meters/second - public float xtrack_error; ///< Current crosstrack error on x-y plane in meters + public float nav_roll; ///< Current desired roll in degrees + public float nav_pitch; ///< Current desired pitch in degrees + public short nav_bearing; ///< Current desired heading in degrees + public short target_bearing; ///< Bearing to current waypoint/target in degrees + public ushort wp_dist; ///< Distance to active waypoint in meters + public float alt_error; ///< Current altitude error in meters + public float aspd_error; ///< Current airspeed error in meters/second + public float xtrack_error; ///< Current crosstrack error on x-y plane in meters + }; + + public const byte MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN = 26; + public const byte MAVLINK_MSG_ID_62_LEN = 26; + public const byte MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT = 140; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_object_detection_event_t + { + public uint time; ///< Timestamp in milliseconds since system boot + public ushort object_id; ///< Object ID + public byte type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + char name; ///< Name of the object as defined by the detector + public byte quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence + public float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + public float distance; ///< Ground distance in meters + }; + + public const byte MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT_LEN = 36; + public const byte MAVLINK_MSG_ID_140_LEN = 36; + public const byte MAVLINK_MSG_ID_OPTICAL_FLOW = 100; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_optical_flow_t + { + public ulong time; ///< Timestamp (UNIX) + public byte sensor_id; ///< Sensor ID + public short flow_x; ///< Flow in pixels in x-sensor direction + public short flow_y; ///< Flow in pixels in y-sensor direction + public byte quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + public float ground_distance; ///< Ground distance in meters + }; + + public const byte MAVLINK_MSG_ID_OPTICAL_FLOW_LEN = 18; + public const byte MAVLINK_MSG_ID_100_LEN = 18; + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_request_list_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN = 2; + public const byte MAVLINK_MSG_ID_21_LEN = 2; + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_request_read_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public short param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + }; + + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN = 19; + public const byte MAVLINK_MSG_ID_20_LEN = 19; + public const byte MAVLINK_MSG_ID_PARAM_SET = 23; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_set_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public float param_value; ///< Onboard parameter value + }; + + public const byte MAVLINK_MSG_ID_PARAM_SET_LEN = 21; + public const byte MAVLINK_MSG_ID_23_LEN = 21; + public const byte MAVLINK_MSG_ID_PARAM_VALUE = 22; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_value_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public float param_value; ///< Onboard parameter value + public ushort param_count; ///< Total number of onboard parameters + public ushort param_index; ///< Index of this onboard parameter + }; + + public const byte MAVLINK_MSG_ID_PARAM_VALUE_LEN = 23; + public const byte MAVLINK_MSG_ID_22_LEN = 23; + public const byte MAVLINK_MSG_ID_PING = 3; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_ping_t + { + public uint seq; ///< PING sequence + public byte target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + public byte target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + public ulong time; ///< Unix timestamp in microseconds + }; + + public const byte MAVLINK_MSG_ID_PING_LEN = 14; + public const byte MAVLINK_MSG_ID_3_LEN = 14; + public const byte MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT = 61; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_position_controller_output_t + { + public byte enabled; ///< 1: enabled, 0: disabled + public byte x; ///< Position x: -128: -100%, 127: +100% + public byte y; ///< Position y: -128: -100%, 127: +100% + public byte z; ///< Position z: -128: -100%, 127: +100% + public byte yaw; ///< Position yaw: -128: -100%, 127: +100% + }; public const byte MAVLINK_MSG_ID_POSITION_TARGET = 63; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_position_target_t { - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< yaw orientation in radians, 0 = NORTH + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< yaw orientation in radians, 0 = NORTH }; - public const byte MAVLINK_MSG_ID_STATE_CORRECTION = 64; + public const byte MAVLINK_MSG_ID_POSITION_TARGET_LEN = 16; + public const byte MAVLINK_MSG_ID_63_LEN = 16; + public const byte MAVLINK_MSG_ID_RAW_IMU = 28; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_state_correction_t + public struct __mavlink_raw_imu_t { - public float xErr; ///< x position error - public float yErr; ///< y position error - public float zErr; ///< z position error - public float rollErr; ///< roll error (radians) - public float pitchErr; ///< pitch error (radians) - public float yawErr; ///< yaw error (radians) - public float vxErr; ///< x velocity - public float vyErr; ///< y velocity - public float vzErr; ///< z velocity + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short xacc; ///< X acceleration (raw) + public short yacc; ///< Y acceleration (raw) + public short zacc; ///< Z acceleration (raw) + public short xgyro; ///< Angular speed around X axis (raw) + public short ygyro; ///< Angular speed around Y axis (raw) + public short zgyro; ///< Angular speed around Z axis (raw) + public short xmag; ///< X Magnetic field (raw) + public short ymag; ///< Y Magnetic field (raw) + public short zmag; ///< Z Magnetic field (raw) }; - public const byte MAVLINK_MSG_ID_SET_ALTITUDE = 65; + public const byte MAVLINK_MSG_ID_RAW_IMU_LEN = 26; + public const byte MAVLINK_MSG_ID_28_LEN = 26; + public const byte MAVLINK_MSG_ID_RAW_PRESSURE = 29; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_altitude_t + public struct __mavlink_raw_pressure_t { - public byte target; ///< The system setting the altitude - public uint mode; ///< The new altitude in meters - }; - - public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_request_data_stream_t - { - public byte target_system; ///< The target requested to send the message stream. - public byte target_component; ///< The target requested to send the message stream. - public byte req_stream_id; ///< The ID of the requested message type - public ushort req_message_rate; ///< Update rate in Hertz - public byte start_stop; ///< 1 to start sending, 0 to stop sending. - }; - - public const byte MAVLINK_MSG_ID_HIL_STATE = 67; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_hil_state_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float roll; ///< Roll angle (rad) - public float pitch; ///< Pitch angle (rad) - public float yaw; ///< Yaw angle (rad) - public float rollspeed; ///< Roll angular speed (rad/s) - public float pitchspeed; ///< Pitch angular speed (rad/s) - public float yawspeed; ///< Yaw angular speed (rad/s) - public int lat; ///< Latitude, expressed as * 1E7 - public int lon; ///< Longitude, expressed as * 1E7 - public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - public short xacc; ///< X acceleration (mg) - public short yacc; ///< Y acceleration (mg) - public short zacc; ///< Z acceleration (mg) - }; - - public const byte MAVLINK_MSG_ID_HIL_CONTROLS = 68; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_hil_controls_t - { - }; - - public const byte MAVLINK_MSG_ID_MANUAL_CONTROL = 69; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_manual_control_t - { - public byte target; ///< The system to be controlled - public float roll; ///< roll - public float pitch; ///< pitch - public float yaw; ///< yaw - public float thrust; ///< thrust - public byte roll_manual; ///< roll control enabled auto:0, manual:1 - public byte pitch_manual; ///< pitch auto:0, manual:1 - public byte yaw_manual; ///< yaw auto:0, manual:1 - public byte thrust_manual; ///< thrust auto:0, manual:1 + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short press_abs; ///< Absolute pressure (raw) + public short press_diff1; ///< Differential pressure 1 (raw) + public short press_diff2; ///< Differential pressure 2 (raw) + public short temperature; ///< Raw Temperature measurement (raw) }; + public const byte MAVLINK_MSG_ID_RAW_PRESSURE_LEN = 16; + public const byte MAVLINK_MSG_ID_29_LEN = 16; public const byte MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE = 70; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_rc_channels_override_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort chan1_raw; ///< RC channel 1 value, in microseconds + public ushort chan2_raw; ///< RC channel 2 value, in microseconds + public ushort chan3_raw; ///< RC channel 3 value, in microseconds + public ushort chan4_raw; ///< RC channel 4 value, in microseconds + public ushort chan5_raw; ///< RC channel 5 value, in microseconds + public ushort chan6_raw; ///< RC channel 6 value, in microseconds + public ushort chan7_raw; ///< RC channel 7 value, in microseconds + public ushort chan8_raw; ///< RC channel 8 value, in microseconds + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN = 18; + public const byte MAVLINK_MSG_ID_70_LEN = 18; + public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_rc_channels_raw_t + { + public ushort chan1_raw; ///< RC channel 1 value, in microseconds + public ushort chan2_raw; ///< RC channel 2 value, in microseconds + public ushort chan3_raw; ///< RC channel 3 value, in microseconds + public ushort chan4_raw; ///< RC channel 4 value, in microseconds + public ushort chan5_raw; ///< RC channel 5 value, in microseconds + public ushort chan6_raw; ///< RC channel 6 value, in microseconds + public ushort chan7_raw; ///< RC channel 7 value, in microseconds + public ushort chan8_raw; ///< RC channel 8 value, in microseconds + public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN = 17; + public const byte MAVLINK_MSG_ID_35_LEN = 17; + public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 36; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_rc_channels_scaled_t + { + public short chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN = 17; + public const byte MAVLINK_MSG_ID_36_LEN = 17; + public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_data_stream_t + { + public byte target_system; ///< The target requested to send the message stream. + public byte target_component; ///< The target requested to send the message stream. + public byte req_stream_id; ///< The ID of the requested message type + public ushort req_message_rate; ///< Update rate in Hertz + public byte start_stop; ///< 1 to start sending, 0 to stop sending. + }; + + public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN = 6; + public const byte MAVLINK_MSG_ID_66_LEN = 6; + public const byte MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_dynamic_gyro_calibration_t + { + public byte target_system; ///< The system which should auto-calibrate + public byte target_component; ///< The system component which should auto-calibrate + public float mode; ///< The current ground-truth rpm + public byte axis; ///< The axis to calibrate: 0 roll, 1 pitch, 2 yaw + public ushort time; ///< The time to average over in ms + + }; + + public const byte MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION = 68; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_static_calibration_t + { + public byte target_system; ///< The system which should auto-calibrate + public byte target_component; ///< The system component which should auto-calibrate + public ushort time; ///< The time to average over in ms + + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 58; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t + { + public ulong time_us; ///< Timestamp in micro seconds since unix epoch + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + public float thrust; ///< Collective thrust, normalized to 0 .. 1 + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN = 24; + public const byte MAVLINK_MSG_ID_58_LEN = 24; + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 57; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_roll_pitch_yaw_thrust_setpoint_t + { + public ulong time_us; ///< Timestamp in micro seconds since unix epoch + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + public float thrust; ///< Collective thrust, normalized to 0 .. 1 + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN = 24; + public const byte MAVLINK_MSG_ID_57_LEN = 24; + public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 54; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_safety_allowed_area_t + { + public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + public float p1x; ///< x position 1 / Latitude 1 + public float p1y; ///< y position 1 / Longitude 1 + public float p1z; ///< z position 1 / Altitude 1 + public float p2x; ///< x position 2 / Latitude 2 + public float p2y; ///< y position 2 / Longitude 2 + public float p2z; ///< z position 2 / Altitude 2 + }; + + public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN = 25; + public const byte MAVLINK_MSG_ID_54_LEN = 25; + public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 53; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_safety_set_allowed_area_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + public float p1x; ///< x position 1 / Latitude 1 + public float p1y; ///< y position 1 / Longitude 1 + public float p1z; ///< z position 1 / Altitude 1 + public float p2x; ///< x position 2 / Latitude 2 + public float p2y; ///< y position 2 / Longitude 2 + public float p2z; ///< z position 2 / Altitude 2 + }; + + public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN = 27; + public const byte MAVLINK_MSG_ID_53_LEN = 27; + public const byte MAVLINK_MSG_ID_SCALED_IMU = 26; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_scaled_imu_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + public short xgyro; ///< Angular speed around X axis (millirad /sec) + public short ygyro; ///< Angular speed around Y axis (millirad /sec) + public short zgyro; ///< Angular speed around Z axis (millirad /sec) + public short xmag; ///< X Magnetic field (milli tesla) + public short ymag; ///< Y Magnetic field (milli tesla) + public short zmag; ///< Z Magnetic field (milli tesla) + }; + + public const byte MAVLINK_MSG_ID_SCALED_IMU_LEN = 26; + public const byte MAVLINK_MSG_ID_26_LEN = 26; + public const byte MAVLINK_MSG_ID_SCALED_PRESSURE = 38; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_scaled_pressure_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float press_abs; ///< Absolute pressure (hectopascal) + public float press_diff; ///< Differential pressure 1 (hectopascal) + public short temperature; ///< Temperature measurement (0.01 degrees celsius) + }; + + public const byte MAVLINK_MSG_ID_SCALED_PRESSURE_LEN = 18; + public const byte MAVLINK_MSG_ID_38_LEN = 18; + public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 37; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_servo_output_raw_t + { + public ushort servo1_raw; ///< Servo output 1 value, in microseconds + public ushort servo2_raw; ///< Servo output 2 value, in microseconds + public ushort servo3_raw; ///< Servo output 3 value, in microseconds + public ushort servo4_raw; ///< Servo output 4 value, in microseconds + public ushort servo5_raw; ///< Servo output 5 value, in microseconds + public ushort servo6_raw; ///< Servo output 6 value, in microseconds + public ushort servo7_raw; ///< Servo output 7 value, in microseconds + public ushort servo8_raw; ///< Servo output 8 value, in microseconds + }; + + public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN = 16; + public const byte MAVLINK_MSG_ID_37_LEN = 16; + public const byte MAVLINK_MSG_ID_SET_ALTITUDE = 65; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_altitude_t + { + public byte target; ///< The system setting the altitude + public uint mode; ///< The new altitude in meters + }; + + public const byte MAVLINK_MSG_ID_SET_ALTITUDE_LEN = 5; + public const byte MAVLINK_MSG_ID_65_LEN = 5; + public const byte MAVLINK_MSG_ID_SET_MODE = 11; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_mode_t + { + public byte target; ///< The system setting the mode + public byte mode; ///< The new mode + }; + + public const byte MAVLINK_MSG_ID_SET_MODE_LEN = 2; + public const byte MAVLINK_MSG_ID_11_LEN = 2; + public const byte MAVLINK_MSG_ID_SET_NAV_MODE = 12; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_nav_mode_t + { + public byte target; ///< The system setting the mode + public byte nav_mode; ///< The new navigation mode + }; + + public const byte MAVLINK_MSG_ID_SET_NAV_MODE_LEN = 2; + public const byte MAVLINK_MSG_ID_12_LEN = 2; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW = 55; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_roll_pitch_yaw_t { public byte target_system; ///< System ID public byte target_component; ///< Component ID - public ushort chan1_raw; ///< RC channel 1 value, in microseconds - public ushort chan2_raw; ///< RC channel 2 value, in microseconds - public ushort chan3_raw; ///< RC channel 3 value, in microseconds - public ushort chan4_raw; ///< RC channel 4 value, in microseconds - public ushort chan5_raw; ///< RC channel 5 value, in microseconds - public ushort chan6_raw; ///< RC channel 6 value, in microseconds - public ushort chan7_raw; ///< RC channel 7 value, in microseconds - public ushort chan8_raw; ///< RC channel 8 value, in microseconds + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + }; - public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 73; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED = 56; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_global_position_int_t + public struct __mavlink_set_roll_pitch_yaw_speed_t { - public int lat; ///< Latitude, expressed as * 1E7 - public int lon; ///< Longitude, expressed as * 1E7 - public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + }; - public const byte MAVLINK_MSG_ID_VFR_HUD = 74; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 56; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_vfr_hud_t + public struct __mavlink_set_roll_pitch_yaw_speed_thrust_t { - public float airspeed; ///< Current airspeed in m/s - public float groundspeed; ///< Current ground speed in m/s - public short heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - public ushort throttle; ///< Current throttle setting in integer percent, 0 to 100 - public float alt; ///< Current altitude (MSL), in meters - public float climb; ///< Current climb rate in meters/second + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + public float thrust; ///< Collective thrust, normalized to 0 .. 1 }; - public const byte MAVLINK_MSG_ID_COMMAND = 75; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN = 18; + public const byte MAVLINK_MSG_ID_56_LEN = 18; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 55; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_command_t + public struct __mavlink_set_roll_pitch_yaw_thrust_t { - public byte target_system; ///< System which should execute the command - public byte target_component; ///< Component which should execute the command, 0 for all components - public byte command; ///< Command ID, as defined by MAV_CMD enum. - public byte confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - public float param1; ///< Parameter 1, as defined by MAV_CMD enum. - public float param2; ///< Parameter 2, as defined by MAV_CMD enum. - public float param3; ///< Parameter 3, as defined by MAV_CMD enum. - public float param4; ///< Parameter 4, as defined by MAV_CMD enum. + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + public float thrust; ///< Collective thrust, normalized to 0 .. 1 }; - public const byte MAVLINK_MSG_ID_COMMAND_ACK = 76; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN = 18; + public const byte MAVLINK_MSG_ID_55_LEN = 18; + public const byte MAVLINK_MSG_ID_STATE_CORRECTION = 64; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_command_ack_t + public struct __mavlink_state_correction_t { - public float command; ///< Current airspeed in m/s - public float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - }; - - public const byte MAVLINK_MSG_ID_DEBUG_VECT = 251; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_debug_vect_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name - public ulong usec; ///< Timestamp - public float x; ///< x - public float y; ///< y - public float z; ///< z - }; - - public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 252; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_named_value_float_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name of the debug variable - public float value; ///< Floating point value - }; - - public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT = 253; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_named_value_int_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name of the debug variable - public int value; ///< Signed integer value + public float xErr; ///< x position error + public float yErr; ///< y position error + public float zErr; ///< z position error + public float rollErr; ///< roll error (radians) + public float pitchErr; ///< pitch error (radians) + public float yawErr; ///< yaw error (radians) + public float vxErr; ///< x velocity + public float vyErr; ///< y velocity + public float vzErr; ///< z velocity }; + public const byte MAVLINK_MSG_ID_STATE_CORRECTION_LEN = 36; + public const byte MAVLINK_MSG_ID_64_LEN = 36; public const byte MAVLINK_MSG_ID_STATUSTEXT = 254; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_statustext_t { - public byte severity; ///< Severity of status, 0 = info message, 255 = critical fault - [MarshalAs(UnmanagedType.ByValArray, SizeConst=50)] - public byte[] text; ///< Status text message, without null termination character + public byte severity; ///< Severity of status, 0 = info message, 255 = critical fault + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=50)] + public byte[] text; ///< Status text message, without null termination character }; - public const byte MAVLINK_MSG_ID_DEBUG = 255; + public const byte MAVLINK_MSG_ID_STATUSTEXT_LEN = 51; + public const byte MAVLINK_MSG_ID_254_LEN = 51; + public const byte MAVLINK_MSG_ID_SYSTEM_TIME = 2; + public const byte MAVLINK_MSG_ID_SYSTEM_TIME_UTC = 4; + public const byte MAVLINK_MSG_ID_SYS_STATUS = 34; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_debug_t + public struct __mavlink_sys_status_t { - public byte ind; ///< index of debug variable - public float value; ///< DEBUG value + public byte mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + public byte nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + public byte status; ///< System status flag, see MAV_STATUS ENUM + public ushort load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + public ushort vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) + public ushort battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) + public ushort packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) }; -Type[] mavstructs = new Type[] {typeof(__mavlink_heartbeat_t) ,typeof(__mavlink_boot_t) ,typeof(__mavlink_system_time_t) ,typeof(__mavlink_ping_t) ,typeof(__mavlink_system_time_utc_t) ,typeof(__mavlink_change_operator_control_t) ,typeof(__mavlink_change_operator_control_ack_t) ,typeof(__mavlink_auth_key_t) ,null ,typeof(__mavlink_action_ack_t) ,typeof(__mavlink_action_t) ,typeof(__mavlink_set_mode_t) ,typeof(__mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_param_request_read_t) ,typeof(__mavlink_param_request_list_t) ,typeof(__mavlink_param_value_t) ,typeof(__mavlink_param_set_t) ,null ,typeof(__mavlink_gps_raw_int_t) ,typeof(__mavlink_scaled_imu_t) ,typeof(__mavlink_gps_status_t) ,typeof(__mavlink_raw_imu_t) ,typeof(__mavlink_raw_pressure_t) ,typeof(__mavlink_attitude_t) ,typeof(__mavlink_local_position_t) ,typeof(__mavlink_gps_raw_t) ,typeof(__mavlink_global_position_t) ,typeof(__mavlink_sys_status_t) ,typeof(__mavlink_rc_channels_raw_t) ,typeof(__mavlink_rc_channels_scaled_t) ,typeof(__mavlink_servo_output_raw_t) ,typeof(__mavlink_scaled_pressure_t) ,typeof(__mavlink_waypoint_t) ,typeof(__mavlink_waypoint_request_t) ,typeof(__mavlink_waypoint_set_current_t) ,typeof(__mavlink_waypoint_current_t) ,typeof(__mavlink_waypoint_request_list_t) ,typeof(__mavlink_waypoint_count_t) ,typeof(__mavlink_waypoint_clear_all_t) ,typeof(__mavlink_waypoint_reached_t) ,typeof(__mavlink_waypoint_ack_t) ,typeof(__mavlink_gps_set_global_origin_t) ,typeof(__mavlink_gps_local_origin_set_t) ,typeof(__mavlink_local_position_setpoint_set_t) ,typeof(__mavlink_local_position_setpoint_t) ,typeof(__mavlink_control_status_t) ,typeof(__mavlink_safety_set_allowed_area_t) ,typeof(__mavlink_safety_allowed_area_t) ,typeof(__mavlink_set_roll_pitch_yaw_thrust_t) ,typeof(__mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof(__mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof(__mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,null ,null ,typeof(__mavlink_nav_controller_output_t) ,typeof(__mavlink_position_target_t) ,typeof(__mavlink_state_correction_t) ,typeof(__mavlink_set_altitude_t) ,typeof(__mavlink_request_data_stream_t) ,typeof(__mavlink_hil_state_t) ,typeof(__mavlink_hil_controls_t) ,typeof(__mavlink_manual_control_t) ,typeof(__mavlink_rc_channels_override_t) ,null ,null ,typeof(__mavlink_global_position_int_t) ,typeof(__mavlink_vfr_hud_t) ,typeof(__mavlink_command_t) ,typeof(__mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_sensor_offsets_t) ,typeof(__mavlink_set_mag_offsets_t) ,typeof(__mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_debug_vect_t) ,typeof(__mavlink_named_value_float_t) ,typeof(__mavlink_named_value_int_t) ,typeof(__mavlink_statustext_t) ,typeof(__mavlink_debug_t) ,null ,}; + public const byte MAVLINK_MSG_ID_SYS_STATUS_LEN = 11; + public const byte MAVLINK_MSG_ID_34_LEN = 11; + public const byte MAVLINK_MSG_ID_VFR_HUD = 74; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_vfr_hud_t + { + public float airspeed; ///< Current airspeed in m/s + public float groundspeed; ///< Current ground speed in m/s + public short heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + public ushort throttle; ///< Current throttle setting in integer percent, 0 to 100 + public float alt; ///< Current altitude (MSL), in meters + public float climb; ///< Current climb rate in meters/second + }; + + public const byte MAVLINK_MSG_ID_VFR_HUD_LEN = 20; + public const byte MAVLINK_MSG_ID_74_LEN = 20; + public const byte MAVLINK_MSG_ID_WAYPOINT = 39; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + public byte frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + public byte command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + public byte current; ///< false:0, true:1 + public byte autocontinue; ///< autocontinue to next wp + public float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + public float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + public float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + public float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + public float x; ///< PARAM5 / local: x position, global: latitude + public float y; ///< PARAM6 / y position: global: longitude + public float z; ///< PARAM7 / z position: global: altitude + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_LEN = 36; + public const byte MAVLINK_MSG_ID_39_LEN = 36; + public const byte MAVLINK_MSG_ID_WAYPOINT_ACK = 47; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_ack_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public byte type; ///< 0: OK, 1: Error + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_ACK_LEN = 3; + public const byte MAVLINK_MSG_ID_47_LEN = 3; + public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL = 45; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_clear_all_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN = 2; + public const byte MAVLINK_MSG_ID_45_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT = 44; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_count_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort count; ///< Number of Waypoints in the Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN = 4; + public const byte MAVLINK_MSG_ID_44_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT = 42; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_current_t + { + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN = 2; + public const byte MAVLINK_MSG_ID_42_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED = 46; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_reached_t + { + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN = 2; + public const byte MAVLINK_MSG_ID_46_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST = 40; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_request_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN = 4; + public const byte MAVLINK_MSG_ID_40_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST = 43; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_request_list_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN = 2; + public const byte MAVLINK_MSG_ID_43_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT = 41; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_set_current_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN = 4; + public const byte MAVLINK_MSG_ID_41_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_GLOBAL_REFERENCE = 48; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_set_global_reference_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float global_x; ///< global x position + public float global_y; ///< global y position + public float global_z; ///< global z position + public float global_yaw; ///< global yaw orientation in radians, 0 = NORTH + public float local_x; ///< local x position that matches the global x position + public float local_y; ///< local y position that matches the global y position + public float local_z; ///< local z position that matches the global z position + public float local_yaw; ///< local yaw that matches the global yaw orientation + + }; + + public enum MAV_CLASS + { + MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything + MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch + MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu + MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com + MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands + MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set + MAV_CLASS_NONE = 8, ///< No valid autopilot + MAV_CLASS_NB ///< Number of autopilot classes + }; + + public enum MAV_ACTION + { + MAV_ACTION_HOLD = 0, + MAV_ACTION_MOTORS_START = 1, + MAV_ACTION_LAUNCH = 2, + MAV_ACTION_RETURN = 3, + MAV_ACTION_EMCY_LAND = 4, + MAV_ACTION_EMCY_KILL = 5, + MAV_ACTION_CONFIRM_KILL = 6, + MAV_ACTION_CONTINUE = 7, + MAV_ACTION_MOTORS_STOP = 8, + MAV_ACTION_HALT = 9, + MAV_ACTION_SHUTDOWN = 10, + MAV_ACTION_REBOOT = 11, + MAV_ACTION_SET_MANUAL = 12, + MAV_ACTION_SET_AUTO = 13, + MAV_ACTION_STORAGE_READ = 14, + MAV_ACTION_STORAGE_WRITE = 15, + MAV_ACTION_CALIBRATE_RC = 16, + MAV_ACTION_CALIBRATE_GYRO = 17, + MAV_ACTION_CALIBRATE_MAG = 18, + MAV_ACTION_CALIBRATE_ACC = 19, + MAV_ACTION_CALIBRATE_PRESSURE = 20, + MAV_ACTION_REC_START = 21, + MAV_ACTION_REC_PAUSE = 22, + MAV_ACTION_REC_STOP = 23, + MAV_ACTION_TAKEOFF = 24, + MAV_ACTION_NAVIGATE = 25, + MAV_ACTION_LAND = 26, + MAV_ACTION_LOITER = 27, + MAV_ACTION_SET_ORIGIN = 28, + MAV_ACTION_RELAY_ON = 29, + MAV_ACTION_RELAY_OFF = 30, + MAV_ACTION_GET_IMAGE = 31, + MAV_ACTION_VIDEO_START = 32, + MAV_ACTION_VIDEO_STOP = 33, + MAV_ACTION_RESET_MAP = 34, + MAV_ACTION_RESET_PLAN = 35, + MAV_ACTION_DELAY_BEFORE_COMMAND = 36, + MAV_ACTION_ASCEND_AT_RATE = 37, + MAV_ACTION_CHANGE_MODE = 38, + MAV_ACTION_LOITER_MAX_TURNS = 39, + MAV_ACTION_LOITER_MAX_TIME = 40, + MAV_ACTION_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, + MAV_ACTION_NB ///< Number of MAV actions + }; + + public enum MAV_MODE + { + MAV_MODE_UNINIT = 0, ///< System is in undefined state + MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe + MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control + MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint + MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation + MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use + MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use + MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use + MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive + MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back + }; + + public enum MAV_STATE + { + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT, + MAV_STATE_CALIBRATING, + MAV_STATE_STANDBY, + MAV_STATE_ACTIVE, + MAV_STATE_CRITICAL, + MAV_STATE_EMERGENCY, + MAV_STATE_HILSIM, + MAV_STATE_POWEROFF + }; + + public enum MAV_NAV + { + MAV_NAV_GROUNDED = 0, + MAV_NAV_LIFTOFF, + MAV_NAV_HOLD, + MAV_NAV_WAYPOINT, + MAV_NAV_VECTOR, + MAV_NAV_RETURNING, + MAV_NAV_LANDING, + MAV_NAV_LOST, + MAV_NAV_LOITER, + MAV_NAV_FREE_DRIFT + }; + + public enum MAV_TYPE + { + MAV_GENERIC = 0, + MAV_FIXED_WING = 1, + MAV_QUADROTOR = 2, + MAV_COAXIAL = 3, + MAV_HELICOPTER = 4, + MAV_GROUND = 5, + OCU = 6, + MAV_AIRSHIP = 7, + MAV_FREE_BALLOON = 8, + MAV_ROCKET = 9, + UGV_GROUND_ROVER = 10, + UGV_SURFACE_SHIP = 11 + }; + + public enum MAV_AUTOPILOT_TYPE + { + MAV_AUTOPILOT_GENERIC = 0, + MAV_AUTOPILOT_PIXHAWK = 1, + MAV_AUTOPILOT_SLUGS = 2, + MAV_AUTOPILOT_ARDUPILOTMEGA = 3, + MAV_AUTOPILOT_NONE = 4 + }; + + public enum MAV_COMPONENT + { + MAV_COMP_ID_GPS, + MAV_COMP_ID_WAYPOINTPLANNER, + MAV_COMP_ID_BLOBTRACKER, + MAV_COMP_ID_PATHPLANNER, + MAV_COMP_ID_AIRSLAM, + MAV_COMP_ID_MAPPER, + MAV_COMP_ID_CAMERA, + MAV_COMP_ID_IMU = 200, + MAV_COMP_ID_IMU_2 = 201, + MAV_COMP_ID_IMU_3 = 202, + MAV_COMP_ID_UDP_BRIDGE = 240, + MAV_COMP_ID_UART_BRIDGE = 241, + MAV_COMP_ID_SYSTEM_CONTROL = 250 + }; + + public enum MAV_FRAME + { + MAV_FRAME_GLOBAL = 0, + MAV_FRAME_LOCAL = 1, + MAV_FRAME_MISSION = 2, + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, + MAV_FRAME_LOCAL_ENU = 4 + }; + +Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs index dce000b17e..864b322f5d 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs @@ -7,159 +7,7 @@ namespace ArdupilotMega { partial class MAVLink { - public enum MAV_CLASS - { - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes - }; - - public enum MAV_ACTION - { - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions - }; - - public enum MAV_MODE - { - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back - }; - - public enum MAV_STATE - { - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF - }; - - public enum MAV_NAV - { - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT - }; - - public enum MAV_TYPE - { - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 - }; - - public enum MAV_AUTOPILOT_TYPE - { - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 - }; - - public enum MAV_COMPONENT - { - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 - }; - - public enum MAV_FRAME - { - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 - }; + } } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 48dac31e32..00e90ad6e8 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -76,6 +76,14 @@ namespace ArdupilotMega //System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US"); //System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US"); + srtm.datadirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "srtm"; + + georefimage temp = new georefimage(); + + //temp.dowork(141); + + //return; + var t = Type.GetType("Mono.Runtime"); MAC = (t != null); @@ -153,6 +161,15 @@ namespace ArdupilotMega } catch (Exception e) { MessageBox.Show("A Major error has occured : " + e.ToString()); this.Close(); } + GCSViews.FlightData.myhud.Refresh(); + GCSViews.FlightData.myhud.Refresh(); + GCSViews.FlightData.myhud.Refresh(); + + if (GCSViews.FlightData.myhud.huddrawtime > 1000) + { + MessageBox.Show("The HUD draw time is above 1 seconds. Please update your graphics card driver."); + } + changeunits(); try @@ -867,6 +884,8 @@ namespace ArdupilotMega DateTime speechcustomtime = DateTime.Now; + DateTime linkqualitytime = DateTime.Now; + while (serialthread) { try @@ -919,6 +938,21 @@ namespace ArdupilotMega speechcustomtime = DateTime.Now; } + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) + { + MainV2.cs.linkqualitygcs = 0; + } + + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds >= 1) + { + GCSViews.FlightData.myhud.Invalidate(); + if (linkqualitytime.Second != DateTime.Now.Second) + { + MainV2.cs.linkqualitygcs = (ushort)(MainV2.cs.linkqualitygcs * 0.8f); + linkqualitytime = DateTime.Now; + } + } + if (speechenable && talk != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; @@ -939,7 +973,7 @@ namespace ArdupilotMega if (heatbeatsend.Second != DateTime.Now.Second) { - Console.WriteLine("remote lost {0}", cs.packetdrop); + Console.WriteLine("remote lost {0}", cs.packetdropremote); MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t(); @@ -952,10 +986,12 @@ namespace ArdupilotMega } // data loss warning - if (speechenable && talk != null && (DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) { - if (MainV2.talk.State == SynthesizerState.Ready) - MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); + if (speechenable && talk != null) { + if (MainV2.talk.State == SynthesizerState.Ready) + MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); + } } //Console.WriteLine(comPort.BaseStream.BytesToRead); @@ -1459,6 +1495,12 @@ namespace ArdupilotMega MainV2.comPort.Open(false); return true; } + if (keyData == (Keys.Control | Keys.Y)) // for ryan beall + { + MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE); + MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE"); + return true; + } if (keyData == (Keys.Control | Keys.J)) // for jani { string data = "!!"; diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 4ab6aec1c7..8f357122d7 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -431,13 +431,14 @@ namespace ArdupilotMega // bar moves to 100 % in this step progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f); - Application.DoEvents(); + progressBar1.Refresh(); + //Application.DoEvents(); byte[] packet = mine.readPacket(); string text = ""; mine.DebugPacket(packet, ref text); - sw.Write(text); + sw.Write(mine.lastlogread +" "+text); } sw.Close(); diff --git a/Tools/ArdupilotMegaPlanner/MyButton.cs b/Tools/ArdupilotMegaPlanner/MyButton.cs index 12508be1f7..11bf6cd531 100644 --- a/Tools/ArdupilotMegaPlanner/MyButton.cs +++ b/Tools/ArdupilotMegaPlanner/MyButton.cs @@ -50,7 +50,7 @@ namespace ArdupilotMega if (!this.Enabled) { - SolidBrush brush = new SolidBrush(Color.FromArgb(200, 0x2b, 0x3a, 0x03)); + SolidBrush brush = new SolidBrush(Color.FromArgb(150, 0x2b, 0x3a, 0x03)); gr.FillRectangle(brush, 0, 0, this.Width, this.Height); } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 25abe98b99..622fe43525 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.67")] +[assembly: AssemblyFileVersion("1.0.70")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt index 2bba4ab6e7..011946774a 100644 --- a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt +++ b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt @@ -1,4 +1,4 @@ -== MAVLink Parameters == +== MAVLink Parameters == (this is a copy fo the wiki page FYI) This is a list of all the user-modifiable MAVLink parameters and what they do. You can modify them via the MAVLink parameters window in any compatible GCS, such as the Mission Planner, HK GCS or !QGroundControl. @@ -213,9 +213,9 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some ||SYSID_SW_MREV|| || ||0|| || ||Description coming soon|| ||SYSID_SW_TYPE|| || ||0|| || ||Description coming soon|| ||THR_SLEWRATE||0||100||0||1||1||THROTTLE_SLEW_RATE - 0 = Disabled, otherwise it limits throttle movement rate. Units are % per second. This is a test feature and may go away.|| -||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| \ No newline at end of file +||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs index d8cf085621..243c6d9816 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs @@ -32,22 +32,11 @@ System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup)); this.tabControl1 = new System.Windows.Forms.TabControl(); this.tabReset = new System.Windows.Forms.TabPage(); - this.BUT_reset = new ArdupilotMega.MyButton(); this.tabRadioIn = new System.Windows.Forms.TabPage(); this.CHK_revch3 = new System.Windows.Forms.CheckBox(); this.CHK_revch4 = new System.Windows.Forms.CheckBox(); this.CHK_revch2 = new System.Windows.Forms.CheckBox(); this.CHK_revch1 = new System.Windows.Forms.CheckBox(); - this.BUT_Calibrateradio = new ArdupilotMega.MyButton(); - this.BAR8 = new ArdupilotMega.HorizontalProgressBar2(); - this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components); - this.BAR7 = new ArdupilotMega.HorizontalProgressBar2(); - this.BAR6 = new ArdupilotMega.HorizontalProgressBar2(); - this.BAR5 = new ArdupilotMega.HorizontalProgressBar2(); - this.BARpitch = new ArdupilotMega.VerticalProgressBar2(); - this.BARthrottle = new ArdupilotMega.VerticalProgressBar2(); - this.BARyaw = new ArdupilotMega.HorizontalProgressBar2(); - this.BARroll = new ArdupilotMega.HorizontalProgressBar2(); this.tabModes = new System.Windows.Forms.TabPage(); this.label14 = new System.Windows.Forms.Label(); this.LBL_flightmodepwm = new System.Windows.Forms.Label(); @@ -71,7 +60,6 @@ this.CMB_fmode2 = new System.Windows.Forms.ComboBox(); this.label1 = new System.Windows.Forms.Label(); this.CMB_fmode1 = new System.Windows.Forms.ComboBox(); - this.BUT_SaveModes = new ArdupilotMega.MyButton(); this.tabHardware = new System.Windows.Forms.TabPage(); this.linkLabelmagdec = new System.Windows.Forms.LinkLabel(); this.label106 = new System.Windows.Forms.Label(); @@ -90,7 +78,6 @@ this.pictureBox1 = new System.Windows.Forms.PictureBox(); this.tabArducopter = new System.Windows.Forms.TabPage(); this.label28 = new System.Windows.Forms.Label(); - this.BUT_levelac2 = new ArdupilotMega.MyButton(); this.label16 = new System.Windows.Forms.Label(); this.label15 = new System.Windows.Forms.Label(); this.pictureBoxQuadX = new System.Windows.Forms.PictureBox(); @@ -119,6 +106,20 @@ this.HS2_REV = new System.Windows.Forms.CheckBox(); this.HS1_REV = new System.Windows.Forms.CheckBox(); this.label17 = new System.Windows.Forms.Label(); + this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); + this.BUT_reset = new ArdupilotMega.MyButton(); + this.BUT_Calibrateradio = new ArdupilotMega.MyButton(); + this.BAR8 = new ArdupilotMega.HorizontalProgressBar2(); + this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components); + this.BAR7 = new ArdupilotMega.HorizontalProgressBar2(); + this.BAR6 = new ArdupilotMega.HorizontalProgressBar2(); + this.BAR5 = new ArdupilotMega.HorizontalProgressBar2(); + this.BARpitch = new ArdupilotMega.VerticalProgressBar2(); + this.BARthrottle = new ArdupilotMega.VerticalProgressBar2(); + this.BARyaw = new ArdupilotMega.HorizontalProgressBar2(); + this.BARroll = new ArdupilotMega.HorizontalProgressBar2(); + this.BUT_SaveModes = new ArdupilotMega.MyButton(); + this.BUT_levelac2 = new ArdupilotMega.MyButton(); this.BUT_saveheliconfig = new ArdupilotMega.MyButton(); this.BUT_0collective = new ArdupilotMega.MyButton(); this.HS4 = new ArdupilotMega.VerticalProgressBar2(); @@ -128,11 +129,15 @@ this.HS2_TRIM = new ArdupilotMega.MyTrackBar(); this.HS1_TRIM = new ArdupilotMega.MyTrackBar(); this.Gservoloc = new AGaugeApp.AGauge(); - this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); + this.CB_simple1 = new System.Windows.Forms.CheckBox(); + this.CB_simple2 = new System.Windows.Forms.CheckBox(); + this.CB_simple3 = new System.Windows.Forms.CheckBox(); + this.CB_simple4 = new System.Windows.Forms.CheckBox(); + this.CB_simple5 = new System.Windows.Forms.CheckBox(); + this.CB_simple6 = new System.Windows.Forms.CheckBox(); this.tabControl1.SuspendLayout(); this.tabReset.SuspendLayout(); this.tabRadioIn.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); this.tabModes.SuspendLayout(); this.tabHardware.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit(); @@ -143,6 +148,7 @@ ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit(); this.tabHeli.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit(); @@ -169,14 +175,6 @@ this.tabReset.Name = "tabReset"; this.tabReset.UseVisualStyleBackColor = true; // - // BUT_reset - // - resources.ApplyResources(this.BUT_reset, "BUT_reset"); - this.BUT_reset.Name = "BUT_reset"; - this.BUT_reset.Tag = ""; - this.BUT_reset.UseVisualStyleBackColor = true; - this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click); - // // tabRadioIn // this.tabRadioIn.Controls.Add(this.CHK_revch3); @@ -224,139 +222,14 @@ this.CHK_revch1.UseVisualStyleBackColor = true; this.CHK_revch1.CheckedChanged += new System.EventHandler(this.CHK_revch1_CheckedChanged); // - // BUT_Calibrateradio - // - resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio"); - this.BUT_Calibrateradio.Name = "BUT_Calibrateradio"; - this.BUT_Calibrateradio.UseVisualStyleBackColor = true; - this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click); - // - // BAR8 - // - this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true)); - this.BAR8.Label = "Radio 8"; - resources.ApplyResources(this.BAR8, "BAR8"); - this.BAR8.Maximum = 2200; - this.BAR8.maxline = 0; - this.BAR8.Minimum = 800; - this.BAR8.minline = 0; - this.BAR8.Name = "BAR8"; - this.BAR8.Value = 1500; - this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // currentStateBindingSource - // - this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState); - // - // BAR7 - // - this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true)); - this.BAR7.Label = "Radio 7"; - resources.ApplyResources(this.BAR7, "BAR7"); - this.BAR7.Maximum = 2200; - this.BAR7.maxline = 0; - this.BAR7.Minimum = 800; - this.BAR7.minline = 0; - this.BAR7.Name = "BAR7"; - this.BAR7.Value = 1500; - this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BAR6 - // - this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true)); - this.BAR6.Label = "Radio 6"; - resources.ApplyResources(this.BAR6, "BAR6"); - this.BAR6.Maximum = 2200; - this.BAR6.maxline = 0; - this.BAR6.Minimum = 800; - this.BAR6.minline = 0; - this.BAR6.Name = "BAR6"; - this.BAR6.Value = 1500; - this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BAR5 - // - this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true)); - this.BAR5.Label = "Radio 5"; - resources.ApplyResources(this.BAR5, "BAR5"); - this.BAR5.Maximum = 2200; - this.BAR5.maxline = 0; - this.BAR5.Minimum = 800; - this.BAR5.minline = 0; - this.BAR5.Name = "BAR5"; - this.BAR5.Value = 1500; - this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARpitch - // - this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true)); - this.BARpitch.Label = "Pitch"; - resources.ApplyResources(this.BARpitch, "BARpitch"); - this.BARpitch.Maximum = 2200; - this.BARpitch.maxline = 0; - this.BARpitch.Minimum = 800; - this.BARpitch.minline = 0; - this.BARpitch.Name = "BARpitch"; - this.BARpitch.Value = 1500; - this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARthrottle - // - this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69))))); - this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true)); - this.BARthrottle.Label = "Throttle"; - resources.ApplyResources(this.BARthrottle, "BARthrottle"); - this.BARthrottle.Maximum = 2200; - this.BARthrottle.maxline = 0; - this.BARthrottle.Minimum = 800; - this.BARthrottle.minline = 0; - this.BARthrottle.Name = "BARthrottle"; - this.BARthrottle.Value = 1000; - this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31))))); - // - // BARyaw - // - this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true)); - this.BARyaw.Label = "Yaw"; - resources.ApplyResources(this.BARyaw, "BARyaw"); - this.BARyaw.Maximum = 2200; - this.BARyaw.maxline = 0; - this.BARyaw.Minimum = 800; - this.BARyaw.minline = 0; - this.BARyaw.Name = "BARyaw"; - this.BARyaw.Value = 1500; - this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARroll - // - this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true)); - this.BARroll.Label = "Roll"; - resources.ApplyResources(this.BARroll, "BARroll"); - this.BARroll.Maximum = 2200; - this.BARroll.maxline = 0; - this.BARroll.Minimum = 800; - this.BARroll.minline = 0; - this.BARroll.Name = "BARroll"; - this.BARroll.Value = 1500; - this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // // tabModes // + this.tabModes.Controls.Add(this.CB_simple6); + this.tabModes.Controls.Add(this.CB_simple5); + this.tabModes.Controls.Add(this.CB_simple4); + this.tabModes.Controls.Add(this.CB_simple3); + this.tabModes.Controls.Add(this.CB_simple2); + this.tabModes.Controls.Add(this.CB_simple1); this.tabModes.Controls.Add(this.label14); this.tabModes.Controls.Add(this.LBL_flightmodepwm); this.tabModes.Controls.Add(this.label13); @@ -519,13 +392,6 @@ resources.ApplyResources(this.CMB_fmode1, "CMB_fmode1"); this.CMB_fmode1.Name = "CMB_fmode1"; // - // BUT_SaveModes - // - resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes"); - this.BUT_SaveModes.Name = "BUT_SaveModes"; - this.BUT_SaveModes.UseVisualStyleBackColor = true; - this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click); - // // tabHardware // this.tabHardware.BackColor = System.Drawing.Color.DarkRed; @@ -661,11 +527,11 @@ // tabArducopter // this.tabArducopter.Controls.Add(this.label28); - this.tabArducopter.Controls.Add(this.BUT_levelac2); this.tabArducopter.Controls.Add(this.label16); this.tabArducopter.Controls.Add(this.label15); this.tabArducopter.Controls.Add(this.pictureBoxQuadX); this.tabArducopter.Controls.Add(this.pictureBoxQuad); + this.tabArducopter.Controls.Add(this.BUT_levelac2); resources.ApplyResources(this.tabArducopter, "tabArducopter"); this.tabArducopter.Name = "tabArducopter"; this.tabArducopter.UseVisualStyleBackColor = true; @@ -675,13 +541,6 @@ resources.ApplyResources(this.label28, "label28"); this.label28.Name = "label28"; // - // BUT_levelac2 - // - resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2"); - this.BUT_levelac2.Name = "BUT_levelac2"; - this.BUT_levelac2.UseVisualStyleBackColor = true; - this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click); - // // label16 // resources.ApplyResources(this.label16, "label16"); @@ -879,6 +738,159 @@ resources.ApplyResources(this.label17, "label17"); this.label17.Name = "label17"; // + // BUT_reset + // + resources.ApplyResources(this.BUT_reset, "BUT_reset"); + this.BUT_reset.Name = "BUT_reset"; + this.BUT_reset.Tag = ""; + this.BUT_reset.UseVisualStyleBackColor = true; + this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click); + // + // BUT_Calibrateradio + // + resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio"); + this.BUT_Calibrateradio.Name = "BUT_Calibrateradio"; + this.BUT_Calibrateradio.UseVisualStyleBackColor = true; + this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click); + // + // BAR8 + // + this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true)); + this.BAR8.Label = "Radio 8"; + resources.ApplyResources(this.BAR8, "BAR8"); + this.BAR8.Maximum = 2200; + this.BAR8.maxline = 0; + this.BAR8.Minimum = 800; + this.BAR8.minline = 0; + this.BAR8.Name = "BAR8"; + this.BAR8.Value = 1500; + this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // currentStateBindingSource + // + this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState); + // + // BAR7 + // + this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true)); + this.BAR7.Label = "Radio 7"; + resources.ApplyResources(this.BAR7, "BAR7"); + this.BAR7.Maximum = 2200; + this.BAR7.maxline = 0; + this.BAR7.Minimum = 800; + this.BAR7.minline = 0; + this.BAR7.Name = "BAR7"; + this.BAR7.Value = 1500; + this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BAR6 + // + this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true)); + this.BAR6.Label = "Radio 6"; + resources.ApplyResources(this.BAR6, "BAR6"); + this.BAR6.Maximum = 2200; + this.BAR6.maxline = 0; + this.BAR6.Minimum = 800; + this.BAR6.minline = 0; + this.BAR6.Name = "BAR6"; + this.BAR6.Value = 1500; + this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BAR5 + // + this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true)); + this.BAR5.Label = "Radio 5"; + resources.ApplyResources(this.BAR5, "BAR5"); + this.BAR5.Maximum = 2200; + this.BAR5.maxline = 0; + this.BAR5.Minimum = 800; + this.BAR5.minline = 0; + this.BAR5.Name = "BAR5"; + this.BAR5.Value = 1500; + this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARpitch + // + this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true)); + this.BARpitch.Label = "Pitch"; + resources.ApplyResources(this.BARpitch, "BARpitch"); + this.BARpitch.Maximum = 2200; + this.BARpitch.maxline = 0; + this.BARpitch.Minimum = 800; + this.BARpitch.minline = 0; + this.BARpitch.Name = "BARpitch"; + this.BARpitch.Value = 1500; + this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARthrottle + // + this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69))))); + this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true)); + this.BARthrottle.Label = "Throttle"; + resources.ApplyResources(this.BARthrottle, "BARthrottle"); + this.BARthrottle.Maximum = 2200; + this.BARthrottle.maxline = 0; + this.BARthrottle.Minimum = 800; + this.BARthrottle.minline = 0; + this.BARthrottle.Name = "BARthrottle"; + this.BARthrottle.Value = 1000; + this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31))))); + // + // BARyaw + // + this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true)); + this.BARyaw.Label = "Yaw"; + resources.ApplyResources(this.BARyaw, "BARyaw"); + this.BARyaw.Maximum = 2200; + this.BARyaw.maxline = 0; + this.BARyaw.Minimum = 800; + this.BARyaw.minline = 0; + this.BARyaw.Name = "BARyaw"; + this.BARyaw.Value = 1500; + this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARroll + // + this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true)); + this.BARroll.Label = "Roll"; + resources.ApplyResources(this.BARroll, "BARroll"); + this.BARroll.Maximum = 2200; + this.BARroll.maxline = 0; + this.BARroll.Minimum = 800; + this.BARroll.minline = 0; + this.BARroll.Name = "BARroll"; + this.BARroll.Value = 1500; + this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BUT_SaveModes + // + resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes"); + this.BUT_SaveModes.Name = "BUT_SaveModes"; + this.BUT_SaveModes.UseVisualStyleBackColor = true; + this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click); + // + // BUT_levelac2 + // + resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2"); + this.BUT_levelac2.Name = "BUT_levelac2"; + this.BUT_levelac2.UseVisualStyleBackColor = true; + this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click); + // // BUT_saveheliconfig // resources.ApplyResources(this.BUT_saveheliconfig, "BUT_saveheliconfig"); @@ -925,8 +937,8 @@ // // HS4_TRIM // - this.HS4_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS4_TRIM, "HS4_TRIM"); + this.HS4_TRIM.LargeChange = 1000; this.HS4_TRIM.Maximum = 2000D; this.HS4_TRIM.Minimum = 1000D; this.HS4_TRIM.Name = "HS4_TRIM"; @@ -937,8 +949,8 @@ // // HS3_TRIM // - this.HS3_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS3_TRIM, "HS3_TRIM"); + this.HS3_TRIM.LargeChange = 1000; this.HS3_TRIM.Maximum = 2000D; this.HS3_TRIM.Minimum = 1000D; this.HS3_TRIM.Name = "HS3_TRIM"; @@ -949,8 +961,8 @@ // // HS2_TRIM // - this.HS2_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS2_TRIM, "HS2_TRIM"); + this.HS2_TRIM.LargeChange = 1000; this.HS2_TRIM.Maximum = 2000D; this.HS2_TRIM.Minimum = 1000D; this.HS2_TRIM.Name = "HS2_TRIM"; @@ -961,8 +973,8 @@ // // HS1_TRIM // - this.HS1_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS1_TRIM, "HS1_TRIM"); + this.HS1_TRIM.LargeChange = 1000; this.HS1_TRIM.Maximum = 2000D; this.HS1_TRIM.Minimum = 1000D; this.HS1_TRIM.Name = "HS1_TRIM"; @@ -1114,6 +1126,42 @@ this.Gservoloc.Value2 = 180F; this.Gservoloc.Value3 = 0F; // + // CB_simple1 + // + resources.ApplyResources(this.CB_simple1, "CB_simple1"); + this.CB_simple1.Name = "CB_simple1"; + this.CB_simple1.UseVisualStyleBackColor = true; + // + // CB_simple2 + // + resources.ApplyResources(this.CB_simple2, "CB_simple2"); + this.CB_simple2.Name = "CB_simple2"; + this.CB_simple2.UseVisualStyleBackColor = true; + // + // CB_simple3 + // + resources.ApplyResources(this.CB_simple3, "CB_simple3"); + this.CB_simple3.Name = "CB_simple3"; + this.CB_simple3.UseVisualStyleBackColor = true; + // + // CB_simple4 + // + resources.ApplyResources(this.CB_simple4, "CB_simple4"); + this.CB_simple4.Name = "CB_simple4"; + this.CB_simple4.UseVisualStyleBackColor = true; + // + // CB_simple5 + // + resources.ApplyResources(this.CB_simple5, "CB_simple5"); + this.CB_simple5.Name = "CB_simple5"; + this.CB_simple5.UseVisualStyleBackColor = true; + // + // CB_simple6 + // + resources.ApplyResources(this.CB_simple6, "CB_simple6"); + this.CB_simple6.Name = "CB_simple6"; + this.CB_simple6.UseVisualStyleBackColor = true; + // // Setup // resources.ApplyResources(this, "$this"); @@ -1126,7 +1174,6 @@ this.tabReset.ResumeLayout(false); this.tabRadioIn.ResumeLayout(false); this.tabRadioIn.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit(); this.tabModes.ResumeLayout(false); this.tabModes.PerformLayout(); this.tabHardware.ResumeLayout(false); @@ -1141,6 +1188,7 @@ ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit(); this.tabHeli.ResumeLayout(false); this.tabHeli.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).EndInit(); @@ -1250,6 +1298,12 @@ private System.Windows.Forms.CheckBox CHK_revch4; private System.Windows.Forms.CheckBox CHK_revch2; private System.Windows.Forms.CheckBox CHK_revch1; + private System.Windows.Forms.CheckBox CB_simple6; + private System.Windows.Forms.CheckBox CB_simple5; + private System.Windows.Forms.CheckBox CB_simple4; + private System.Windows.Forms.CheckBox CB_simple3; + private System.Windows.Forms.CheckBox CB_simple2; + private System.Windows.Forms.CheckBox CB_simple1; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 53279ab634..b2b9ad463c 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -231,7 +231,6 @@ namespace ArdupilotMega.Setup MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc); - MainV2.comPort.param = MainV2.comPort.getParamList(); if (Configuration != null) { Configuration.startup = true; @@ -253,6 +252,13 @@ namespace ArdupilotMega.Setup { if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) // APM { + CB_simple1.Visible = false; + CB_simple2.Visible = false; + CB_simple3.Visible = false; + CB_simple4.Visible = false; + CB_simple5.Visible = false; + CB_simple6.Visible = false; + CMB_fmode1.Items.Clear(); CMB_fmode2.Items.Clear(); CMB_fmode3.Items.Clear(); @@ -411,6 +417,10 @@ namespace ArdupilotMega.Setup MainV2.comPort.setParam("FLTMODE4", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode4.Text)); MainV2.comPort.setParam("FLTMODE5", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode5.Text)); MainV2.comPort.setParam("FLTMODE6", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode6.Text)); + + float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 <<2 : 0) + + (CB_simple4.Checked ? 1 << 3 : 0) + (CB_simple5.Checked ? 1 << 4 : 0) + (CB_simple6.Checked ? 1 << 5 : 0); + MainV2.comPort.setParam("SIMPLE", value); } } catch { MessageBox.Show("Failed to set Flight modes"); } diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx index be2a79628f..fdcafe6fac 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx @@ -117,20 +117,6 @@ System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - 214, 161 - - - 195, 23 - - - - 0 - - - Reset APM Hardware to Default - BUT_reset @@ -143,12 +129,14 @@ 0 + 4, 22 666, 393 + 4 @@ -167,10 +155,1780 @@ 0 + + CHK_revch3 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 0 + + + CHK_revch4 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 1 + + + CHK_revch2 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 2 + + + CHK_revch1 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 3 + + + BUT_Calibrateradio + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 4 + + + BAR8 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 5 + + + BAR7 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 6 + + + BAR6 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 7 + + + BAR5 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 8 + + + BARpitch + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 9 + + + BARthrottle + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 10 + + + BARyaw + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 11 + + + BARroll + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 12 + + + 4, 22 + + + + 3, 3, 3, 3 + + + 666, 393 + + + 0 + + + Radio Input + + + tabRadioIn + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 1 + + + True + + + NoControl + + + 372, 235 + + + 87, 17 + + + 119 + + + Simple Mode + + + CB_simple6 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 0 + + + True + + + NoControl + + + 373, 208 + + + 87, 17 + + + 118 + + + Simple Mode + + + CB_simple5 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 1 + + + True + + + NoControl + + + 372, 181 + + + 87, 17 + + + 117 + + + Simple Mode + + + CB_simple4 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 2 + + + True + + + NoControl + + + 373, 154 + + + 87, 17 + + + 116 + + + Simple Mode + + + CB_simple3 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 3 + + + True + + + NoControl + + + 372, 127 + + + 87, 17 + + + 115 + + + Simple Mode + + + CB_simple2 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 4 + + + True + + + 373, 101 + + + 87, 17 + + + 114 + + + Simple Mode + + + CB_simple1 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 5 + + + True + + + NoControl + + + 242, 67 + + + 74, 13 + + + 113 + + + Current PWM: + + + label14 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 6 + + + True + + + NoControl + + + 322, 67 + + + 13, 13 + + + 112 + + + 0 + + + LBL_flightmodepwm + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 7 + + + True + + + NoControl + + + 242, 50 + + + 74, 13 + + + 111 + + + Current Mode: + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 8 + + + True + + + NoControl + + + 322, 50 + + + 42, 13 + + + 110 + + + Manual + + + lbl_currentmode + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 9 + + + True + + + NoControl + + + 483, 101 + + + 76, 13 + + + 109 + + + PWM 0 - 1230 + + + False + + + label12 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 10 + + + True + + + NoControl + + + 483, 236 + + + 70, 13 + + + 108 + + + PWM 1750 + + + + False + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 11 + + + True + + + NoControl + + + 483, 209 + + + 94, 13 + + + 107 + + + PWM 1621 - 1749 + + + False + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 12 + + + True + + + NoControl + + + 483, 182 + + + 94, 13 + + + 106 + + + PWM 1491 - 1620 + + + False + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 13 + + + True + + + NoControl + + + 483, 155 + + + 94, 13 + + + 105 + + + PWM 1361 - 1490 + + + False + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 14 + + + True + + + NoControl + + + 483, 128 + + + 94, 13 + + + 104 + + + PWM 1231 - 1360 + + + False + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 15 + + + True + + + NoControl + + + 168, 236 + + + 71, 13 + + + 11 + + + Flight Mode 6 + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 16 + + + 245, 233 + + + 121, 21 + + + 10 + + + CMB_fmode6 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 17 + + + True + + + NoControl + + + 168, 209 + + + 71, 13 + + + 9 + + + Flight Mode 5 + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 18 + + + 245, 206 + + + 121, 21 + + + 8 + + + CMB_fmode5 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 19 + + + True + + + NoControl + + + 168, 182 + + + 71, 13 + + + 7 + + + Flight Mode 4 + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 20 + + + 245, 179 + + + 121, 21 + + + 6 + + + CMB_fmode4 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 21 + + + True + + + NoControl + + + 168, 155 + + + 71, 13 + + + 5 + + + Flight Mode 3 + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 22 + + + 245, 152 + + + 121, 21 + + + 4 + + + CMB_fmode3 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 23 + + + True + + + NoControl + + + 168, 128 + + + 71, 13 + + + 3 + + + Flight Mode 2 + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 24 + + + 245, 125 + + + 121, 21 + + + 2 + + + CMB_fmode2 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 25 + + + True + + + NoControl + + + 168, 101 + + + 71, 13 + + + 1 + + + Flight Mode 1 + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 26 + + + 245, 98 + + + 121, 21 + + + 0 + + + CMB_fmode1 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 27 + + + NoControl + + + 245, 260 + + + 121, 23 + + + 103 + + + Save Modes + + + BUT_SaveModes + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabModes + + + 28 + + + 4, 22 + + + 666, 393 + + + 3 + + + Modes + + + tabModes + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 2 + + + linkLabelmagdec + + + System.Windows.Forms.LinkLabel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 0 + + + label106 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 1 + + + label105 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 2 + + + TXT_battcapacity + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 3 + + + CMB_batmontype + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 4 + + + label100 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 5 + + + TXT_declination + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 6 + + + CHK_enableairspeed + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 7 + + + CHK_enablesonar + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 8 + + + CHK_enablebattmon + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 9 + + + CHK_enablecompass + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 10 + + + pictureBox4 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 11 + + + pictureBox3 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 12 + + + pictureBox2 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 13 + + + pictureBox1 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 14 + + + 4, 22 + + + 3, 3, 3, 3 + + + 666, 393 + + + 1 + + + Hardware + + + tabHardware + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 3 + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 0 + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 1 + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 2 + + + pictureBoxQuadX + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 3 + + + pictureBoxQuad + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 4 + + + BUT_levelac2 + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabArducopter + + + 5 + + + 4, 22 + + + 666, 393 + + + 2 + + + ArduCopter2 + + + tabArducopter + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 4 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 0 + + + GYR_GAIN_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 1 + + + GYR_ENABLE_ + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 2 + + + label26 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 3 + + + PIT_MAX_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 4 + + + label25 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 5 + + + ROL_MAX_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 6 + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 7 + + + COL_MID_ + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 8 + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 9 + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 10 + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 11 + + + HS4_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 12 + + + label20 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 13 + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 14 + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 15 + + + SV3_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 16 + + + SV2_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 17 + + + SV1_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 18 + + + HS3_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 19 + + + HS2_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 20 + + + HS1_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 21 + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 22 + + + BUT_saveheliconfig + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 23 + + + BUT_0collective + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 24 + + + HS4 + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 25 + + + HS3 + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 26 + + + HS4_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 27 + + + HS3_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 28 + + + HS2_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 29 + + + HS1_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 30 + + + Gservoloc + + + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 31 + + + 4, 22 + + + 666, 393 + + + 5 + + + AC2 Heli - BETA + + + tabHeli + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 5 + + + Fill + + + 0, 0 + + + 674, 419 + + + 93 + + + tabControl1 + + + System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 0 + True - NoControl @@ -288,837 +2046,12 @@ 3 - - 482, 340 - - - 134, 23 - - - 102 - - - Calibrate Radio - - - BUT_Calibrateradio - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 4 - - - 17, 17 - - - 446, 240 - - - 170, 25 - - - 101 - - - BAR8 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 5 - - - 446, 185 - - - 170, 25 - - - 100 - - - BAR7 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 6 - - - 446, 130 - - - 170, 25 - - - 99 - - - BAR6 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 7 - - - 446, 75 - - - 170, 25 - - - 98 - - - BAR5 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 8 - - - 143, 61 - - - 47, 211 - - - 96 - - - BARpitch - - - ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 9 - - - 359, 61 - - - 47, 211 - - - 95 - - - BARthrottle - - - ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 10 - - - 21, 304 - - - 288, 23 - - - 94 - - - BARyaw - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 11 - - - 21, 6 - - - 288, 23 - - - 93 - - - BARroll - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 12 - - - 4, 22 - - - 3, 3, 3, 3 - - - 666, 393 - - - 0 - - - Radio Input - - - tabRadioIn - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 1 - - - True - - - 242, 67 - - - 74, 13 - - - 113 - - - Current PWM: - - - label14 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 0 - - - True - - - 322, 67 - - - 13, 13 - - - 112 - - - 0 - - - LBL_flightmodepwm - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 1 - - - True - - - 242, 50 - - - 74, 13 - - - 111 - - - Current Mode: - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 2 - - - True - - - 322, 50 - - - 42, 13 - - - 110 - - - Manual - - - lbl_currentmode - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 3 - - - True - - - 372, 101 - - - 76, 13 - - - 109 - - - PWM 0 - 1230 - - - label12 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 4 - - - True - - - 372, 236 - - - 70, 13 - - - 108 - - - PWM 1750 + - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 5 - - - True - - - 372, 209 - - - 94, 13 - - - 107 - - - PWM 1621 - 1749 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 6 - - - True - - - 372, 182 - - - 94, 13 - - - 106 - - - PWM 1491 - 1620 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 7 - - - True - - - 372, 155 - - - 94, 13 - - - 105 - - - PWM 1361 - 1490 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 8 - - - True - - - 372, 128 - - - 94, 13 - - - 104 - - - PWM 1231 - 1360 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 9 - - - True - - - 168, 236 - - - 71, 13 - - - 11 - - - Flight Mode 6 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 10 - - - 245, 233 - - - 121, 21 - - - 10 - - - CMB_fmode6 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 11 - - - True - - - 168, 209 - - - 71, 13 - - - 9 - - - Flight Mode 5 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 12 - - - 245, 206 - - - 121, 21 - - - 8 - - - CMB_fmode5 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 13 - - - True - - - 168, 182 - - - 71, 13 - - - 7 - - - Flight Mode 4 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 14 - - - 245, 179 - - - 121, 21 - - - 6 - - - CMB_fmode4 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 15 - - - True - - - 168, 155 - - - 71, 13 - - - 5 - - - Flight Mode 3 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 16 - - - 245, 152 - - - 121, 21 - - - 4 - - - CMB_fmode3 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 17 - - - True - - - 168, 128 - - - 71, 13 - - - 3 - - - Flight Mode 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 18 - - - 245, 125 - - - 121, 21 - - - 2 - - - CMB_fmode2 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 19 - - - True - - - 168, 101 - - - 71, 13 - - - 1 - - - Flight Mode 1 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 20 - - - 245, 98 - - - 121, 21 - - - 0 - - - CMB_fmode1 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 21 - - - 245, 260 - - - 121, 23 - - - 103 - - - Save Modes - - - BUT_SaveModes - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabModes - - - 22 - - - 4, 22 - - - 666, 393 - - - 3 - - - Modes - - - tabModes - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 2 - True + + NoControl + 390, 80 @@ -1281,6 +2214,9 @@ 5 + + 214, 17 + 383, 57 @@ -1290,9 +2226,6 @@ 20 - - 214, 17 - Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3 @@ -1419,6 +2352,9 @@ Zoom + + NoControl + 78, 268 @@ -1443,6 +2379,9 @@ Zoom + + NoControl + 78, 187 @@ -1467,6 +2406,9 @@ Zoom + + NoControl + 78, 106 @@ -1494,6 +2436,9 @@ + + NoControl + @@ -1518,36 +2463,12 @@ 14 - - 4, 22 - - - 3, 3, 3, 3 - - - 666, 393 - - - 1 - - - Hardware - - - tabHardware - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 3 - True + + NoControl + 217, 38 @@ -1572,33 +2493,12 @@ 0 - - 274, 67 - - - 75, 23 - - - 8 - - - Level - - - BUT_levelac2 - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabArducopter - - - 1 - True + + NoControl + 217, 333 @@ -1622,11 +2522,14 @@ will work with hexa's etc tabArducopter - 2 + 1 True + + NoControl + 260, 124 @@ -1649,7 +2552,7 @@ will work with hexa's etc tabArducopter - 3 + 2 NoControl @@ -1676,7 +2579,7 @@ will work with hexa's etc tabArducopter - 4 + 3 NoControl @@ -1703,35 +2606,14 @@ will work with hexa's etc tabArducopter - 5 - - - 4, 22 - - - 666, 393 - - - 2 - - - ArduCopter2 - - - tabArducopter - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - 4 True + + NoControl + 552, 222 @@ -1783,6 +2665,9 @@ will work with hexa's etc True + + NoControl + 540, 202 @@ -1810,6 +2695,9 @@ will work with hexa's etc True + + NoControl + 563, 94 @@ -1861,6 +2749,9 @@ will work with hexa's etc True + + NoControl + 563, 50 @@ -1912,6 +2803,9 @@ will work with hexa's etc True + + NoControl + 537, 170 @@ -1939,6 +2833,9 @@ will work with hexa's etc True + + NoControl + 603, 170 @@ -1966,6 +2863,9 @@ will work with hexa's etc True + + NoControl + 440, 23 @@ -1993,6 +2893,9 @@ will work with hexa's etc True + + NoControl + 290, 23 @@ -2020,6 +2923,9 @@ will work with hexa's etc True + + NoControl + 178, 23 @@ -2047,6 +2953,9 @@ will work with hexa's etc True + + NoControl + 181, 148 @@ -2074,6 +2983,9 @@ will work with hexa's etc True + + NoControl + 13, 258 @@ -2101,6 +3013,9 @@ will work with hexa's etc True + + NoControl + 13, 232 @@ -2128,6 +3043,9 @@ will work with hexa's etc True + + NoControl + 13, 206 @@ -2227,6 +3145,9 @@ will work with hexa's etc True + + NoControl + 181, 125 @@ -2254,6 +3175,9 @@ will work with hexa's etc True + + NoControl + 181, 102 @@ -2281,6 +3205,9 @@ will work with hexa's etc True + + NoControl + 181, 79 @@ -2308,6 +3235,9 @@ will work with hexa's etc True + + NoControl + 38, 23 @@ -2332,6 +3262,267 @@ will work with hexa's etc 22 + + 214, 17 + + + NoControl + + + 214, 161 + + + 195, 23 + + + 0 + + + Reset APM Hardware to Default + + + BUT_reset + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabReset + + + 0 + + + NoControl + + + 482, 340 + + + 134, 23 + + + 102 + + + Calibrate Radio + + + BUT_Calibrateradio + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 4 + + + 17, 17 + + + 446, 240 + + + 170, 25 + + + 101 + + + BAR8 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 5 + + + 17, 17 + + + 446, 185 + + + 170, 25 + + + 100 + + + BAR7 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 6 + + + 446, 130 + + + 170, 25 + + + 99 + + + BAR6 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 7 + + + 446, 75 + + + 170, 25 + + + 98 + + + BAR5 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 8 + + + 143, 61 + + + 47, 211 + + + 96 + + + BARpitch + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 9 + + + 359, 61 + + + 47, 211 + + + 95 + + + BARthrottle + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 10 + + + 21, 304 + + + 288, 23 + + + 94 + + + BARyaw + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 11 + + + 21, 6 + + + 288, 23 + + + 93 + + + BARroll + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 12 + + + NoControl + + + 274, 67 + + + 75, 23 + + + 8 + + + Level + + + BUT_levelac2 + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabArducopter + + + 5 + + + NoControl + 430, 307 @@ -2356,6 +3547,9 @@ will work with hexa's etc 23 + + NoControl + 540, 144 @@ -2422,6 +3616,9 @@ will work with hexa's etc 26 + + NoControl + 276, 203 @@ -2443,6 +3640,9 @@ will work with hexa's etc 27 + + NoControl + 276, 152 @@ -2464,6 +3664,9 @@ will work with hexa's etc 28 + + NoControl + 276, 101 @@ -2485,6 +3688,9 @@ will work with hexa's etc 29 + + NoControl + 276, 50 @@ -2536,54 +3742,6 @@ will work with hexa's etc 31 - - 4, 22 - - - 666, 393 - - - 5 - - - AC2 Heli - BETA - - - tabHeli - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 5 - - - Fill - - - 0, 0 - - - 674, 419 - - - 93 - - - tabControl1 - - - System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 0 - True @@ -2593,21 +3751,24 @@ will work with hexa's etc 674, 419 + + NoControl + APMSetup - - currentStateBindingSource - - - System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - toolTip1 System.Windows.Forms.ToolTip, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + currentStateBindingSource + + + System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + Setup diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx index 8c4784c6d9..1ecdd308f8 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx @@ -192,6 +192,2421 @@ Top, Bottom, Left, Right + + 111, 82 + + + 78, 20 + + + 11 + + + THR_FS_VALUE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + NoControl + + + 6, 86 + + + 50, 13 + + + 12 + + + FS Value + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + THR_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + NoControl + + + 6, 63 + + + 27, 13 + + + 13 + + + Max + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + THR_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + NoControl + + + 6, 40 + + + 24, 13 + + + 14 + + + Min + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 15 + + + Cruise + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 0 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSPD_RATIO + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + NoControl + + + 6, 87 + + + 32, 13 + + + 1 + + + Ratio + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + NoControl + + + 6, 59 + + + 53, 13 + + + 3 + + + FBW max + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + NoControl + + + 6, 40 + + + 50, 13 + + + 5 + + + FBW min + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + NoControl + + + 6, 17 + + + 64, 13 + + + 6 + + + Cruise + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + + 195, 108 + + + 1 + + + Airspeed m/s + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + LIM_PITCH_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + NoControl + + + 6, 63 + + + 51, 13 + + + 10 + + + Pitch Min + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + 111, 36 + + + 78, 20 + + + 7 + + + LIM_PITCH_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + NoControl + + + 6, 40 + + + 54, 13 + + + 11 + + + Pitch Max + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 5 + + + LIM_ROLL_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + NoControl + + + 6, 17 + + + 55, 13 + + + 12 + + + Bank Max + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + + 195, 108 + + + 2 + + + Navigation Angles + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 2 + + + 111, 36 + + + 78, 20 + + + 7 + + + XTRK_ANGLE_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 8 + + + Entry Angle + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + 111, 13 + + + 78, 20 + + + 5 + + + XTRK_GAIN_SC + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + NoControl + + + 6, 17 + + + 52, 13 + + + 9 + + + Gain (cm) + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + + 195, 108 + + + 3 + + + Xtrack Pids + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 13 + + + KFF_PTCH2THR + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 14 + + + P to T + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + KFF_RDDRMIX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + NoControl + + + 6, 63 + + + 61, 13 + + + 15 + + + Rudder Mix + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + KFF_PTCHCOMP + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 16 + + + Pitch Comp + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + + 195, 108 + + + 4 + + + Other Mix's + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 4 + + + 111, 82 + + + 78, 20 + + + 11 + + + ENRGY2THR_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + ENRGY2THR_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + ENRGY2THR_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + + 195, 108 + + + 5 + + + Energy/Alt Pid + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 5 + + + 111, 82 + + + 78, 20 + + + 0 + + + ALT2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ALT2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ALT2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ALT2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + + 195, 108 + + + 6 + + + Nav Pitch Alt Pid + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 6 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSP2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSP2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSP2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ARSP2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + + 195, 108 + + + 7 + + + Nav Pitch AS Pid + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 7 + + + 111, 82 + + + 78, 20 + + + 11 + + + HDNG2RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + HDNG2RLL_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + HDNG2RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + + 195, 108 + + + 8 + + + Nav Roll Pid + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 8 + + + 111, 82 + + + 78, 20 + + + 11 + + + YW2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + YW2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + YW2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + + 195, 108 + + + 9 + + + Servo Yaw Pid + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 9 + + + 111, 82 + + + 78, 20 + + + 11 + + + PTCH2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + PTCH2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + PTCH2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + + 195, 108 + + + 10 + + + Servo Pitch Pid + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 10 + + + 111, 82 + + + 78, 20 + + + 11 + + + RLL2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + RLL2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + RLL2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + + 195, 108 + + + 11 + + + Servo Roll Pid + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + APM 2.x + TabAPM2 @@ -204,6 +2619,1824 @@ 0 + + True + + + 3, 198 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + + + 80, 86 + + + 78, 20 + + + 16 + + + WP_SPEED_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + NoControl + + + 6, 89 + + + 54, 13 + + + 17 + + + m/s + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + NAV_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + NAV_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 1 + + + 80, 86 + + + 78, 20 + + + 18 + + + XTRK_ANGLE_CD1 + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + NoControl + + + 6, 89 + + + 82, 13 + + + 19 + + + Error Max + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + XTRACK_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + XTRACK_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + XTRACK_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 7 + + + 182, 241 + + + 170, 110 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + + + 80, 63 + + + 78, 20 + + + 11 + + + THR_HOLD_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + THR_HOLD_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + THR_HOLD_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 6, 241 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 3 + + + 80, 61 + + + 78, 20 + + + 11 + + + HLD_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + NoControl + + + 6, 64 + + + 65, 13 + + + 12 + + + IMAX + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + HLD_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + HLD_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 4 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 5 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 6 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 7 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 8 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 9 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 10 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + AC2 + TabAC2 @@ -216,6 +4449,1072 @@ 1 + + NoControl + + + 30, 277 + + + 61, 13 + + + 37 + + + Waypoints + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + NoControl + + + 139, 276 + + + 177, 17 + + + 38 + + + Load Waypoints on connect? + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + NoControl + + + 30, 252 + + + 103, 18 + + + 36 + + + Track Length + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + 139, 250 + + + 67, 20 + + + 35 + + + 17, 17 + + + On the Flight Data Tab + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + NoControl + + + 579, 67 + + + 102, 17 + + + 34 + + + Alt Warning + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + NoControl + + + 30, 228 + + + 61, 13 + + + 0 + + + APM Reset + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + NoControl + + + 139, 227 + + + 163, 17 + + + 1 + + + Reset APM on USB Connect + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + Bottom, Left + + + NoControl + + + 33, 411 + + + 144, 17 + + + 2 + + + Mavlink Message Debug + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + NoControl + + + 590, 203 + + + 22, 13 + + + 3 + + + RC + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + 0 + + + 1 + + + 3 + + + 10 + + + 621, 200 + + + 80, 21 + + + 4 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + NoControl + + + 425, 203 + + + 69, 13 + + + 5 + + + Mode/Status + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + NoControl + + + 280, 203 + + + 44, 13 + + + 6 + + + Position + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + NoControl + + + 136, 203 + + + 43, 13 + + + 7 + + + Attitude + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + NoControl + + + 27, 203 + + + 84, 13 + + + 8 + + + Telemetry Rates + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + 0 + + + 1 + + + 3 + + + 10 + + + 499, 200 + + + 80, 21 + + + 9 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + 0 + + + 1 + + + 3 + + + 10 + + + 334, 200 + + + 80, 21 + + + 10 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + 0 + + + 1 + + + 3 + + + 10 + + + 188, 200 + + + 80, 21 + + + 11 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + NoControl + + + 283, 168 + + + 402, 13 + + + 12 + + + NOTE: The Configuration Tab will NOT display these units, as those are raw values. + + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + NoControl + + + 30, 176 + + + 65, 13 + + + 13 + + + Speed Units + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + NoControl + + + 30, 149 + + + 52, 13 + + + 14 + + + Dist Units + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + 139, 173 + + + 138, 21 + + + 15 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + 139, 146 + + + 138, 21 + + + 16 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + NoControl + + + 30, 122 + + + 45, 13 + + + 17 + + + Joystick + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + NoControl + + + 30, 71 + + + 44, 13 + + + 18 + + + Speech + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + NoControl + + + 471, 67 + + + 102, 17 + + + 19 + + + Battery Warning + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + NoControl + + + 378, 67 + + + 87, 17 + + + 20 + + + Time Interval + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + NoControl + + + 322, 67 + + + 56, 17 + + + 21 + + + Mode + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + NoControl + + + 245, 67 + + + 71, 17 + + + 22 + + + Waypoint + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + NoControl + + + 30, 47 + + + 57, 13 + + + 23 + + + OSD Color + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + 139, 40 + + + 138, 21 + + + 24 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + 139, 90 + + + 138, 21 + + + 25 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + NoControl + + + 30, 94 + + + 69, 13 + + + 26 + + + UI Language + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + NoControl + + + 139, 67 + + + 99, 17 + + + 27 + + + Enable Speech + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + NoControl + + + 552, 15 + + + 125, 17 + + + 28 + + + Enable HUD Overlay + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + NoControl + + + 30, 16 + + + 100, 23 + + + 29 + + + Video Device + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + 139, 13 + + + 245, 21 + + + 30 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + NoControl + + + 139, 117 + + + 99, 23 + + + 31 + + + Joystick Setup + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 36 + + + NoControl + + + 471, 11 + + + 75, 23 + + + 32 + + + Stop + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 37 + + + NoControl + + + 390, 11 + + + 75, 23 + + + 33 + + + Start + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 38 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + TabPlanner @@ -228,6 +5527,18 @@ 2 + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + TabSetup @@ -270,5389 +5581,6 @@ 2 - - groupBox3 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 0 - - - groupBox1 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 1 - - - groupBox2 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 2 - - - groupBox15 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 3 - - - groupBox16 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 4 - - - groupBox14 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 5 - - - groupBox13 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 6 - - - groupBox12 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 7 - - - groupBox11 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 8 - - - groupBox10 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 9 - - - groupBox9 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 10 - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - APM 2.x - - - THR_FS_VALUE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 0 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 1 - - - THR_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 2 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 3 - - - THR_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 4 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 5 - - - TRIM_THROTTLE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 6 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 7 - - - 405, 217 - - - 195, 108 - - - 0 - - - Throttle 0-100% - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 50, 13 - - - 12 - - - FS Value - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 27, 13 - - - 13 - - - Max - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 24, 13 - - - 14 - - - Min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 15 - - - Cruise - - - ARSPD_RATIO - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 0 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 1 - - - ARSPD_FBW_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 3 - - - ARSPD_FBW_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 4 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 5 - - - TRIM_ARSPD_CM - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 6 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 7 - - - 406, 325 - - - 195, 108 - - - 1 - - - Airspeed m/s - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 87 - - - 32, 13 - - - 1 - - - Ratio - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 59 - - - 53, 13 - - - 3 - - - FBW max - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 50, 13 - - - 5 - - - FBW min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 64, 13 - - - 6 - - - Cruise * 100 - - - LIM_PITCH_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 0 - - - label39 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 1 - - - LIM_PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 2 - - - label38 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 3 - - - LIM_ROLL_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 4 - - - label37 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 5 - - - 205, 325 - - - 195, 108 - - - 2 - - - Navigation Angles *100 - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 51, 13 - - - 10 - - - Pitch Min - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 54, 13 - - - 11 - - - Pitch Max - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 55, 13 - - - 12 - - - Bank Max - - - XTRK_ANGLE_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 0 - - - label79 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 1 - - - XTRK_GAIN_SC - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 2 - - - label80 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 3 - - - 4, 325 - - - 195, 108 - - - 3 - - - Xtrack Pids - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 8 - - - Entry Angle - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 52, 13 - - - 9 - - - Gain (cm) - - - KFF_PTCH2THR - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 0 - - - label83 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 1 - - - KFF_RDDRMIX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 2 - - - label78 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 3 - - - KFF_PTCHCOMP - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 4 - - - label81 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 5 - - - 205, 217 - - - 195, 108 - - - 4 - - - Other Mix's - - - 111, 13 - - - 78, 20 - - - 13 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 14 - - - P to T - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 61, 13 - - - 15 - - - Rudder Mix - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 16 - - - Pitch Comp - - - ENRGY2THR_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 0 - - - label73 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 1 - - - ENRGY2THR_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 2 - - - label74 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 3 - - - ENRGY2THR_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 4 - - - label75 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 5 - - - ENRGY2THR_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 6 - - - label76 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 7 - - - 4, 217 - - - 195, 108 - - - 5 - - - Energy/Alt Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - ALT2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 0 - - - label69 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 1 - - - ALT2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 2 - - - label70 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 3 - - - ALT2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 4 - - - label71 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 5 - - - ALT2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 6 - - - label72 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 7 - - - 406, 109 - - - 195, 108 - - - 6 - - - Nav Pitch Alt Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - ARSP2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 0 - - - label65 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 1 - - - ARSP2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 2 - - - label66 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 3 - - - ARSP2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 4 - - - label67 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 5 - - - ARSP2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 6 - - - label68 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 7 - - - 205, 109 - - - 195, 108 - - - 7 - - - Nav Pitch AS Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - HDNG2RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 0 - - - label61 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 1 - - - HDNG2RLL_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 2 - - - label62 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 3 - - - HDNG2RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 4 - - - label63 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 5 - - - HDNG2RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 6 - - - label64 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 7 - - - 4, 109 - - - 195, 108 - - - 8 - - - Nav Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - YW2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 0 - - - label57 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 1 - - - YW2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 2 - - - label58 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 3 - - - YW2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 4 - - - label59 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 5 - - - YW2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 6 - - - label60 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 7 - - - 406, 1 - - - 195, 108 - - - 9 - - - Servo Yaw Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - PTCH2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 0 - - - label53 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 1 - - - PTCH2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 2 - - - label54 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 3 - - - PTCH2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 4 - - - label55 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 5 - - - PTCH2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 6 - - - label56 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 7 - - - 205, 1 - - - 195, 108 - - - 10 - - - Servo Pitch Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - RLL2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 0 - - - label49 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 1 - - - RLL2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 2 - - - label50 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 3 - - - RLL2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 4 - - - label51 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 5 - - - RLL2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 6 - - - label52 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 7 - - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - CHK_lockrollpitch - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 0 - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 1 - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 2 - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 3 - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 4 - - - groupBox19 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 5 - - - groupBox20 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 6 - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 7 - - - groupBox22 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 8 - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 9 - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 10 - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 11 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - AC2 - - - True - - - 3, 198 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - - - WP_SPEED_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 0 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 1 - - - NAV_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 2 - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 3 - - - NAV_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 4 - - - label15 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 5 - - - NAV_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 6 - - - label16 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 7 - - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - 80, 86 - - - 78, 20 - - - 16 - - - NoControl - - - 6, 89 - - - 54, 13 - - - 17 - - - cm/s - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - XTRK_ANGLE_CD1 - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 0 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 1 - - - XTRACK_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 2 - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 3 - - - XTRACK_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 4 - - - label17 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 5 - - - XTRACK_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 6 - - - label18 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 7 - - - 182, 241 - - - 170, 110 - - - 2 - - - Crosstrack Correction - - - 80, 86 - - - 78, 20 - - - 18 - - - NoControl - - - 6, 89 - - - 82, 13 - - - 19 - - - Error Max * 100 - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - THR_HOLD_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 0 - - - label19 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 1 - - - THR_HOLD_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 2 - - - label21 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 3 - - - THR_HOLD_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 4 - - - label22 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 5 - - - 6, 241 - - - 170, 110 - - - 3 - - - Altitude Hold - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 0 - - - label27 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 1 - - - 525, 241 - - - 176, 110 - - - 5 - - - Other Mix's - - - 94, 17 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 20 - - - 82, 13 - - - 10 - - - Pitch Max * 100 - - - HLD_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 0 - - - label28 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 1 - - - HLD_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 2 - - - label30 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 3 - - - HLD_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 4 - - - label31 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 5 - - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - - - 80, 61 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 64 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 0 - - - label32 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 1 - - - STB_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 2 - - - label34 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 3 - - - STB_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 4 - - - label35 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 5 - - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 0 - - - label36 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 1 - - - STB_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 2 - - - label41 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 3 - - - STB_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 4 - - - label42 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 5 - - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 0 - - - label43 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 1 - - - STB_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 2 - - - label45 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 3 - - - STB_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 4 - - - label46 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 5 - - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - RATE_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 0 - - - label47 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 1 - - - RATE_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 2 - - - label77 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 3 - - - RATE_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 4 - - - label82 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 5 - - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 0 - - - label84 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 1 - - - RATE_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 2 - - - label86 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 3 - - - RATE_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 4 - - - label87 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 5 - - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 0 - - - label88 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 1 - - - RATE_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 2 - - - label90 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 3 - - - RATE_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 4 - - - label91 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 5 - - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 68, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - label24 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 0 - - - CHK_loadwponconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 1 - - - label23 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 2 - - - NUM_tracklength - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 3 - - - CHK_speechaltwarning - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 4 - - - label108 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 5 - - - CHK_resetapmonconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 6 - - - CHK_mavdebug - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 7 - - - label107 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 8 - - - CMB_raterc - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 9 - - - label104 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 10 - - - label103 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 11 - - - label102 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 12 - - - label101 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 13 - - - CMB_ratestatus - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 14 - - - CMB_rateposition - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 15 - - - CMB_rateattitude - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 16 - - - label99 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 17 - - - label98 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 18 - - - label97 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 19 - - - CMB_speedunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 20 - - - CMB_distunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 21 - - - label96 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 22 - - - label95 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 23 - - - CHK_speechbattery - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 24 - - - CHK_speechcustom - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 25 - - - CHK_speechmode - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 26 - - - CHK_speechwaypoint - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 27 - - - label94 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 28 - - - CMB_osdcolor - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 29 - - - CMB_language - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 30 - - - label93 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 31 - - - CHK_enablespeech - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 32 - - - CHK_hudshow - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 33 - - - label92 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 34 - - - CMB_videosources - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 35 - - - BUT_Joystick - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 36 - - - BUT_videostop - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 37 - - - BUT_videostart - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 38 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - NoControl - - - 30, 277 - - - 61, 13 - - - 37 - - - Waypoints - - - NoControl - - - 139, 276 - - - 177, 17 - - - 38 - - - Load Waypoints on connect? - - - NoControl - - - 30, 252 - - - 103, 18 - - - 36 - - - Track Length - - - 17, 17 - - - 139, 250 - - - 67, 20 - - - 35 - - - On the Flight Data Tab - - - NoControl - - - 579, 67 - - - 102, 17 - - - 34 - - - Alt Warning - - - NoControl - - - 30, 228 - - - 61, 13 - - - 0 - - - APM Reset - - - NoControl - - - 139, 227 - - - 163, 17 - - - 1 - - - Reset APM on USB Connect - - - Bottom, Left - - - NoControl - - - 33, 411 - - - 144, 17 - - - 2 - - - Mavlink Message Debug - - - NoControl - - - 590, 203 - - - 22, 13 - - - 3 - - - RC - - - 0 - - - 1 - - - 3 - - - 10 - - - 621, 200 - - - 80, 21 - - - 4 - - - NoControl - - - 425, 203 - - - 69, 13 - - - 5 - - - Mode/Status - - - NoControl - - - 280, 203 - - - 44, 13 - - - 6 - - - Position - - - NoControl - - - 136, 203 - - - 43, 13 - - - 7 - - - Attitude - - - NoControl - - - 27, 203 - - - 84, 13 - - - 8 - - - Telemetry Rates - - - 0 - - - 1 - - - 3 - - - 10 - - - 499, 200 - - - 80, 21 - - - 9 - - - 0 - - - 1 - - - 3 - - - 10 - - - 334, 200 - - - 80, 21 - - - 10 - - - 0 - - - 1 - - - 3 - - - 10 - - - 188, 200 - - - 80, 21 - - - 11 - - - NoControl - - - 283, 168 - - - 402, 13 - - - 12 - - - NOTE: The Configuration Tab will NOT display these units, as those are raw values. - - - - NoControl - - - 30, 176 - - - 65, 13 - - - 13 - - - Speed Units - - - NoControl - - - 30, 149 - - - 52, 13 - - - 14 - - - Dist Units - - - 139, 173 - - - 138, 21 - - - 15 - - - 139, 146 - - - 138, 21 - - - 16 - - - NoControl - - - 30, 122 - - - 45, 13 - - - 17 - - - Joystick - - - NoControl - - - 30, 71 - - - 44, 13 - - - 18 - - - Speech - - - NoControl - - - 471, 67 - - - 102, 17 - - - 19 - - - Battery Warning - - - NoControl - - - 378, 67 - - - 87, 17 - - - 20 - - - Time Interval - - - NoControl - - - 322, 67 - - - 56, 17 - - - 21 - - - Mode - - - NoControl - - - 245, 67 - - - 71, 17 - - - 22 - - - Waypoint - - - NoControl - - - 30, 47 - - - 57, 13 - - - 23 - - - OSD Color - - - 139, 40 - - - 138, 21 - - - 24 - - - 139, 90 - - - 138, 21 - - - 25 - - - NoControl - - - 30, 94 - - - 69, 13 - - - 26 - - - UI Language - - - NoControl - - - 139, 67 - - - 99, 17 - - - 27 - - - Enable Speech - - - NoControl - - - 552, 15 - - - 125, 17 - - - 28 - - - Enable HUD Overlay - - - NoControl - - - 30, 16 - - - 100, 23 - - - 29 - - - Video Device - - - 139, 13 - - - 245, 21 - - - 30 - - - NoControl - - - 139, 117 - - - 99, 23 - - - 31 - - - Joystick Setup - - - NoControl - - - 471, 11 - - - 75, 23 - - - 32 - - - Stop - - - NoControl - - - 390, 11 - - - 75, 23 - - - 33 - - - Start - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml index a0a06996e2..4ba7a952cc 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml @@ -22,9 +22,8 @@ WP Dist WP Verify Target Bear - Nav Bear - Long Err - Lat Err + Long Err + Lat Err Yaw Sensor @@ -56,6 +55,13 @@ Accel Y Accel Z + + Throttle in + Throttle intergrator + Voltage + Current + Current total + diff --git a/Tools/ArdupilotMegaPlanner/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/dataflashlog.xml index a0a06996e2..4ba7a952cc 100644 --- a/Tools/ArdupilotMegaPlanner/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/dataflashlog.xml @@ -22,9 +22,8 @@ WP Dist WP Verify Target Bear - Nav Bear - Long Err - Lat Err + Long Err + Lat Err Yaw Sensor @@ -56,6 +55,13 @@ Accel Y Accel Z + + Throttle in + Throttle intergrator + Voltage + Current + Current total + diff --git a/Tools/ArdupilotMegaPlanner/georefimage.cs b/Tools/ArdupilotMegaPlanner/georefimage.cs new file mode 100644 index 0000000000..6ab16b82de --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/georefimage.cs @@ -0,0 +1,127 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Drawing; +using System.Drawing.Imaging; +using System.IO; + +namespace ArdupilotMega +{ + class georefimage + { + + public string logFile = ""; + public string dirWithImages = ""; + + DateTime getPhotoTime(string fn) + { + DateTime dtaken = DateTime.MinValue; + + try + { + Image myImage = Image.FromFile(fn); + PropertyItem propItem = myImage.GetPropertyItem(36867); // 36867 // 306 + + //Convert date taken metadata to a DateTime object + string sdate = Encoding.UTF8.GetString(propItem.Value).Trim(); + string secondhalf = sdate.Substring(sdate.IndexOf(" "), (sdate.Length - sdate.IndexOf(" "))); + string firsthalf = sdate.Substring(0, 10); + firsthalf = firsthalf.Replace(":", "-"); + sdate = firsthalf + secondhalf; + dtaken = DateTime.Parse(sdate); + + myImage.Dispose(); + } + catch { } + + return dtaken; + } + + List readLog(string fn) + { + List list = new List(); + + StreamReader sr = new StreamReader(fn); + + string lasttime = "0"; + + while (!sr.EndOfStream) + { + string line = sr.ReadLine(); + + if (line.ToLower().StartsWith("gps")) + { + string[] vals = line.Split(new char[] {',',':'}); + + if (lasttime == vals[1]) + continue; + + lasttime = vals[1]; + + list.Add(vals); + } + } + + sr.Close(); + sr.Dispose(); + + return list; + } + + public void dowork(float offsetseconds) + { + DateTime startTime = DateTime.MinValue; + + logFile = @"C:\Users\hog\Pictures\sams mums 22-6-2011\23-06-11 10-03 4.log"; + + List list = readLog(logFile); + + dirWithImages = @"C:\Users\hog\Pictures\sams mums 22-6-2011"; + + string[] files = Directory.GetFiles(dirWithImages); + + StreamWriter sw2 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.txt"); + + StreamWriter sw = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.tel"); + sw.WriteLine("version=1"); + sw.WriteLine("#longitude and latitude - in degrees"); + sw.WriteLine("#name utc longitude latitude height"); + + foreach (string file in files) + { + if (file.ToLower().EndsWith(".jpg")) + { + DateTime dt = getPhotoTime(file); + + if (startTime == DateTime.MinValue) + startTime = new DateTime(dt.Year,dt.Month,dt.Day,0,0,0,0,DateTimeKind.Utc).ToLocalTime(); + + foreach (string[] arr in list) + { + DateTime crap = startTime.AddMilliseconds(int.Parse(arr[1])).AddSeconds(offsetseconds); + + //Console.Write(dt + " " + crap + "\r"); + + if (dt.Equals(crap)) + { + sw2.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6]); + sw.WriteLine(Path.GetFileNameWithoutExtension(file) + "\t" + crap.ToString("yyyy:MM:dd HH:mm:ss") +"\t"+ arr[5] + "\t" + arr[4] + "\t" + arr[6]); + sw.Flush(); + sw2.Flush(); + Console.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6] + " "); + break; + } + //Console.WriteLine(crap); + } + } + + + } + + sw2.Close(); + sw.Close(); + + } + } +} diff --git a/Tools/ArdupilotMegaPlanner/hudold.cs b/Tools/ArdupilotMegaPlanner/hudold.cs new file mode 100644 index 0000000000..a65755d2a0 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/hudold.cs @@ -0,0 +1,1149 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Drawing; +using System.Data; +using System.Text; +using System.Windows.Forms; +using System.IO; +using System.Drawing.Imaging; + +using System.Threading; + + + +using System.Drawing.Drawing2D; + +using OpenTK; +using OpenTK.Graphics.OpenGL; + +// Control written by Michael Oborne 2011 + +namespace hud +{ + public partial class HUD : MyUserControl //GLControl + { + object paintlock = new object(); + object streamlock = new object(); + MemoryStream _streamjpg = new MemoryStream(); + public MemoryStream streamjpg { get { lock (streamlock) { return _streamjpg; } } set { lock (streamlock) { _streamjpg = value; } } } + /// + /// this is to reduce cpu usage + /// + public bool streamjpgenable = false; + + int huddrawtime = 0; + + public HUD() + { + InitializeComponent(); + //graphicsObject = this; + graphicsObject = Graphics.FromImage(objBitmap); + } + + private void InitializeComponent() + { + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(HUD)); + this.SuspendLayout(); + // + // HUD + // + this.BackColor = System.Drawing.Color.Black; + this.Name = "HUD"; + resources.ApplyResources(this, "$this"); + this.ResumeLayout(false); + + } + + float _roll; + float _navroll; + float _pitch; + float _navpitch; + float _heading; + float _targetheading; + float _alt; + float _targetalt; + float _groundspeed; + float _airspeed; + float _targetspeed; + float _batterylevel; + float _batteryremaining; + float _gpsfix; + float _gpshdop; + float _disttowp; + float _groundcourse; + float _xtrack_error; + float _turnrate; + float _verticalspeed; + string _mode = "Manual"; + int _wpno; + + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float roll { get { return _roll; } set { if (_roll != value) { _roll = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float navroll { get { return _navroll; } set { if (_navroll != value) { _navroll = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float pitch { get { return _pitch; } set { if (_pitch != value) { _pitch = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float navpitch { get { return _navpitch; } set { if (_navpitch != value) { _navpitch = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float heading { get { return _heading; } set { if (_heading != value) { _heading = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetheading { get { return _targetheading; } set { if (_targetheading != value) { _targetheading = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float alt { get { return _alt; } set { if (_alt != value) { _alt = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetalt { get { return _targetalt; } set { if (_targetalt != value) { _targetalt = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float groundspeed { get { return _groundspeed; } set { if (_groundspeed != value) { _groundspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float airspeed { get { return _airspeed; } set { if (_airspeed != value) { _airspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetspeed { get { return _targetspeed; } set { if (_targetspeed != value) { _targetspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float batterylevel { get { return _batterylevel; } set { if (_batterylevel != value) { _batterylevel = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float batteryremaining { get { return _batteryremaining; } set { if (_batteryremaining != value) { _batteryremaining = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float gpsfix { get { return _gpsfix; } set { if (_gpsfix != value) { _gpsfix = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float gpshdop { get { return _gpshdop; } set { if (_gpshdop != value) { _gpshdop = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float disttowp { get { return _disttowp; } set { if (_disttowp != value) { _disttowp = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public string mode { get { return _mode; } set { if (_mode != value) { _mode = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public int wpno { get { return _wpno; } set { if (_wpno != value) { _wpno = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float groundcourse { get { return _groundcourse; } set { if (_groundcourse != value) { _groundcourse = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float xtrack_error { get { return _xtrack_error; } set { if (_xtrack_error != value) { _xtrack_error = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float turnrate { get { return _turnrate; } set { if (_turnrate != value) { _turnrate = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float verticalspeed { get { return _verticalspeed; } set { if (_verticalspeed != value) { _verticalspeed = value; this.Invalidate(); } } } + + public bool bgon = true; + public bool hudon = true; + + [System.ComponentModel.Browsable(true), +System.ComponentModel.Category("Values")] + public Color hudcolor { get { return whitePen.Color; } set { whitePen = new Pen(value, 2); } } + + Pen whitePen = new Pen(Color.White, 2); + + public Image bgimage { set { _bgimage = value; this.Invalidate(); } } + Image _bgimage; + + // move these global as they rarely change - reduce GC + Font font = new Font("Arial", 10); + Bitmap objBitmap = new Bitmap(640, 480); + int count = 0; + DateTime countdate = DateTime.Now; + Graphics graphicsObject; // Graphics + + DateTime starttime = DateTime.MinValue; + + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(HUD)); + + public override void Refresh() + { + base.Refresh(); + OnPaint(new PaintEventArgs(this.CreateGraphics(),this.ClientRectangle)); + } + + protected override void OnLoad(EventArgs e) + { + base.OnLoad(e); + + /* + GL.MatrixMode(MatrixMode.Projection); + GL.LoadIdentity(); + GL.Ortho(0, Width, Height, 0, -1, 1); + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + + GL.Disable(EnableCap.DepthTest); + */ + //GL.Viewport(0, 0, Width, Height); + } + + protected override void OnPaint(PaintEventArgs e) + { + //GL.Enable(EnableCap.AlphaTest); + +// GL.ClearColor(Color.Red); + + // GL.Clear(ClearBufferMask.ColorBufferBit); + + //GL.LoadIdentity(); + +// GL.Viewport(0, 0, Width, Height); + + doPaint(e); + + //this.SwapBuffers(); + + //MakeCurrent(); + } + + void Clear(Color color) + { + GL.ClearColor(color); + } + + const float rad2deg = (float)(180 / Math.PI); + const float deg2rad = (float)(1.0 / rad2deg); + + //graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); + + void DrawArc(Pen penn,RectangleF rect, float start,float degrees) + { + GL.Begin(BeginMode.Lines); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + start -= 90; + float x, y; + for (int i = (int)start; i <= start + degrees; i++) + { + x = (float)Math.Sin(i * deg2rad) * rect.Width / 2; + y = (float)Math.Cos(i * deg2rad) * rect.Height / 2; + x = x + rect.X + rect.Width / 2; + y = y + rect.Y + rect.Height / 2; + GL.Vertex2(x, y); + } + GL.End(); + } + + void DrawEllipse(Pen penn, Rectangle rect) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + float x, y; + for (float i = 0; i < 360; i+=1) + { + x = (float)Math.Sin(i * deg2rad) * rect.Width / 2; + y = (float)Math.Cos(i * deg2rad) * rect.Height / 2; + x = x + rect.X + rect.Width / 2; + y = y + rect.Y + rect.Height / 2; + GL.Vertex2(x, y); + } + GL.End(); + } + + //graphicsObject.DrawImage(_bgimage, 0, 0, this.Width, this.Height); + + void DrawImage(Image img, int x, int y, int width, int height) + { + Bitmap bitmap = (Bitmap)img; + int texture; + + GL.GenTextures(1, out texture); + GL.BindTexture(TextureTarget.Texture2D, texture); + + BitmapData data = bitmap.LockBits(new System.Drawing.Rectangle(0, 0, bitmap.Width, bitmap.Height), + ImageLockMode.ReadOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb); + + GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0, + OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0); + + bitmap.UnlockBits(data); + + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear); + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear); + + + + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + GL.BindTexture(TextureTarget.Texture2D, texture); + + GL.Begin(BeginMode.Quads); + + GL.TexCoord2(0.0f, 1.0f); GL.Vertex2(-0.6f, -0.4f); + GL.TexCoord2(1.0f, 1.0f); GL.Vertex2(0.6f, -0.4f); + GL.TexCoord2(1.0f, 0.0f); GL.Vertex2(0.6f, 0.4f); + GL.TexCoord2(0.0f, 0.0f); GL.Vertex2(-0.6f, 0.4f); + + GL.End(); + } + + void DrawPath(Pen penn,GraphicsPath gp) + { + try + { + DrawPolygon(penn, gp.PathPoints); + } + catch { } + } + + void FillPath(Brush brushh,GraphicsPath gp) + { + try + { + FillPolygon(brushh, gp.PathPoints); + } + catch { } + } + + SmoothingMode SmoothingMode; + + void SetClip(Rectangle rect) + { + + } + + void ResetClip() + { + + } + + void ResetTransform() + { + GL.LoadIdentity(); + } + + void RotateTransform(float angle) + { + GL.Rotate(angle,0,0,1); + } + + void TranslateTransform(float x, float y) + { + GL.Translate(x, y, 0f); + } + + void FillPolygon(Brush brushh, Point[] list) + { + GL.Begin(BeginMode.TriangleFan); + GL.Color4(((SolidBrush)brushh).Color); + foreach (Point pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + GL.Vertex2(list[list.Length - 1].X, list[list.Length - 1].Y); + GL.End(); + } + + void FillPolygon(Brush brushh, PointF[] list) + { + GL.Begin(BeginMode.Quads); + GL.Color4(((SolidBrush)brushh).Color); + foreach (PointF pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + GL.Vertex2(list[0].X, list[0].Y); + GL.End(); + } + + //graphicsObject.DrawPolygon(redPen, pointlist); + + void DrawPolygon(Pen penn, Point[] list) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + foreach (Point pnt in list) + { + GL.Vertex2(pnt.X,pnt.Y); + } + GL.End(); + } + + void DrawPolygon(Pen penn, PointF[] list) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + foreach (PointF pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + //GL.Vertex2(list[0].X, list[0].Y); + GL.End(); + } + + //graphicsObject.FillRectangle(linearBrush, bg); + + void FillRectangle(Brush brushh,RectangleF rectf) + { + float x1 = rectf.X; + float y1 = rectf.Y; + + float width = rectf.Width; + float height = rectf.Height; + + GL.Begin(BeginMode.Quads); + + if (((Type)brushh.GetType()) == typeof(LinearGradientBrush)) + { + LinearGradientBrush temp = (LinearGradientBrush)brushh; + GL.Color4(temp.LinearColors[0]); + } + else + { + GL.Color4(((SolidBrush)brushh).Color); + } + + GL.Vertex2(x1, y1); + GL.Vertex2(x1 + width, y1); + + if (((Type)brushh.GetType()) == typeof(LinearGradientBrush)) + { + LinearGradientBrush temp = (LinearGradientBrush)brushh; + GL.Color4(temp.LinearColors[1]); + } + else + { + GL.Color4(((SolidBrush)brushh).Color); + } + + GL.Vertex2(x1 + width, y1 + height); + GL.Vertex2(x1, y1 + height); + GL.End(); + } + + //graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); + + void DrawRectangle(Pen penn, RectangleF rect) + { + DrawRectangle(penn, rect.X, rect.Y, rect.Width, rect.Height); + } + + void DrawRectangle(Pen penn,double x1,double y1, double width,double height) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + GL.Vertex2(x1, y1); + GL.Vertex2(x1+width, y1); + GL.Vertex2(x1+width, y1+height); + GL.Vertex2(x1, y1+height); + GL.End(); + } + + void DrawLine(Pen penn,double x1,double y1, double x2,double y2) + { + GL.Begin(BeginMode.Lines); + GL.Color4(penn.Color); + GL.LineWidth(penn.Width); + GL.Vertex2(x1, y1); + GL.Vertex2(x2, y2); + GL.End(); + } + + void doPaint(object o) + { + PaintEventArgs e = (PaintEventArgs)o; + + try + { + // limit to 10hz ish + if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) + { + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + return; + } + + starttime = DateTime.Now; + + base.OnPaint(e); + + if (objBitmap.Width != this.Width || objBitmap.Height != this.Height) + { + objBitmap = new Bitmap(this.Width, this.Height); + graphicsObject = Graphics.FromImage(objBitmap); + + graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + graphicsObject.InterpolationMode = InterpolationMode.NearestNeighbor; + graphicsObject.CompositingMode = CompositingMode.SourceOver; + graphicsObject.CompositingQuality = CompositingQuality.HighSpeed; + graphicsObject.PixelOffsetMode = PixelOffsetMode.HighSpeed; + graphicsObject.TextRenderingHint = System.Drawing.Text.TextRenderingHint.SystemDefault; + } + + if (huddrawtime < 100) + { + graphicsObject.TextRenderingHint = System.Drawing.Text.TextRenderingHint.SystemDefault; + + graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + } + else + { + graphicsObject.SmoothingMode = SmoothingMode.HighSpeed; + } + + graphicsObject.Clear(Color.Gray); + + if (_bgimage != null) + { + bgon = false; + graphicsObject.DrawImage(_bgimage, 0, 0, this.Width, this.Height); + + if (hudon == false) + { + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + return; + } + } + else + { + bgon = true; + } + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2); + + graphicsObject.RotateTransform(-roll); + + int fontsize = this.Height / 30; // = 10 + int fontoffset = fontsize - 10; + + float every5deg = -this.Height / 60; + + float pitchoffset = -pitch * every5deg; + + int halfwidth = this.Width / 2; + int halfheight = this.Height / 2; + + SolidBrush whiteBrush = new SolidBrush(whitePen.Color); + + Pen blackPen = new Pen(Color.Black, 2); + Pen greenPen = new Pen(Color.Green, 2); + Pen redPen = new Pen(Color.Red, 2); + + // draw sky + if (bgon == true) + { + RectangleF bg = new RectangleF(-halfwidth * 2, -halfheight * 2, this.Width * 2, halfheight * 2 + pitchoffset); + + if (bg.Height != 0) + { + LinearGradientBrush linearBrush = new LinearGradientBrush(bg, Color.Blue, + Color.LightBlue, LinearGradientMode.Vertical); + + Pen transPen = new Pen(Color.Transparent, 0); + + graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); + + graphicsObject.FillRectangle(linearBrush, bg); + } + // draw ground + + bg = new RectangleF(-halfwidth * 2, pitchoffset, this.Width * 2, halfheight * 2 - pitchoffset); + + if (bg.Height != 0) + { + LinearGradientBrush linearBrush = new LinearGradientBrush(bg, Color.FromArgb(0x9b, 0xb8, 0x24), + Color.FromArgb(0x41, 0x4f, 0x07), LinearGradientMode.Vertical); + + Pen transPen = new Pen(Color.Transparent, 0); + + graphicsObject.DrawRectangle(transPen, bg.X, bg.Y, bg.Width, bg.Height); + + graphicsObject.FillRectangle(linearBrush, bg); + } + + //draw centerline + + graphicsObject.DrawLine(whitePen, -halfwidth * 2, pitchoffset + 0, halfwidth * 2, pitchoffset + 0); + } + + graphicsObject.ResetTransform(); + + graphicsObject.SetClip(new Rectangle(0, this.Height / 14, this.Width, this.Height - this.Height / 14)); + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2); + graphicsObject.RotateTransform(-roll); + + //draw pitch + + int lengthshort = this.Width / 12; + int lengthlong = this.Width / 8; + + for (int a = -90; a <= 90; a += 5) + { + // limit to 40 degrees + if (a >= pitch - 34 && a <= pitch + 29) + { + if (a % 10 == 0) + { + if (a == 0) + { + graphicsObject.DrawLine(greenPen, this.Width / 2 - lengthlong - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthlong - halfwidth, pitchoffset + a * every5deg); + } + else + { + graphicsObject.DrawLine(whitePen, this.Width / 2 - lengthlong - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthlong - halfwidth, pitchoffset + a * every5deg); + } + drawstring(graphicsObject, a.ToString(), font, fontsize + 2, whiteBrush, this.Width / 2 - lengthlong - 30 - halfwidth - (int)(fontoffset * 1.7), pitchoffset + a * every5deg - 8 - fontoffset); + } + else + { + graphicsObject.DrawLine(whitePen, this.Width / 2 - lengthshort - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthshort - halfwidth, pitchoffset + a * every5deg); + //drawstring(e,a.ToString(), new Font("Arial", 10), whiteBrush, this.Width / 2 - lengthshort - 20 - halfwidth, this.Height / 2 + pitchoffset + a * every5deg - 8); + } + } + } + + graphicsObject.ResetTransform(); + + // draw roll ind needle + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2 + this.Height / 14); + + graphicsObject.RotateTransform(-roll); + + Point[] pointlist = new Point[3]; + + lengthlong = this.Height / 66; + + int extra = this.Height / 15 * 7; + + pointlist[0] = new Point(0, -lengthlong * 2 - extra); + pointlist[1] = new Point(-lengthlong, -lengthlong - extra); + pointlist[2] = new Point(lengthlong, -lengthlong - extra); + + if (Math.Abs(roll) > 45) + { + redPen.Width = 10; + } + + graphicsObject.DrawPolygon(redPen, pointlist); + + redPen.Width = 2; + + for (int a = -45; a <= 45; a += 15) + { + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2 + this.Height / 14); + graphicsObject.RotateTransform(a); + drawstring(graphicsObject, Math.Abs(a).ToString("##"), font, fontsize, whiteBrush, 0 - 6 - fontoffset, -lengthlong * 2 - extra); + graphicsObject.DrawLine(whitePen, 0, -halfheight, 0, -halfheight - 10); + } + + graphicsObject.ResetTransform(); + + //draw centre / current att + + Rectangle centercircle = new Rectangle(halfwidth - 10, halfheight - 10, 20, 20); + + graphicsObject.DrawEllipse(redPen, centercircle); + graphicsObject.DrawLine(redPen, centercircle.Left - 10, halfheight, centercircle.Left, halfheight); + graphicsObject.DrawLine(redPen, centercircle.Right, halfheight, centercircle.Right + 10, halfheight); + graphicsObject.DrawLine(redPen, centercircle.Left + centercircle.Width / 2, centercircle.Top, centercircle.Left + centercircle.Width / 2, centercircle.Top - 10); + + // draw roll ind + + Rectangle arcrect = new Rectangle(this.Width / 2 - this.Height / 2, this.Height / 14, this.Height, this.Height); + + graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); + + //draw heading ind + + graphicsObject.ResetClip(); + + Rectangle headbg = new Rectangle(0, 0, this.Width - 0, this.Height / 14); + + graphicsObject.DrawRectangle(blackPen, headbg); + + SolidBrush solidBrush = new SolidBrush(Color.FromArgb(0x55, 0xff, 0xff, 0xff)); + + graphicsObject.FillRectangle(solidBrush, headbg); + + // center + graphicsObject.DrawLine(redPen, headbg.Width / 2, headbg.Bottom, headbg.Width / 2, headbg.Top); + + //bottom line + graphicsObject.DrawLine(whitePen, headbg.Left + 5, headbg.Bottom - 5, headbg.Width - 5, headbg.Bottom - 5); + + float space = (headbg.Width - 10) / 60.0f; + int start = ((int)heading - 30); + + // draw for outside the 60 deg + if (targetheading < start) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * 0, headbg.Bottom, headbg.Left + 5 + space * (0), headbg.Top); + } + if (targetheading > heading + 30) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * 60, headbg.Bottom, headbg.Left + 5 + space * (60), headbg.Top); + } + + for (int a = start; a <= heading + 30; a += 1) + { + // target heading + if (((a + 360) % 360) == targetheading) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * (a - start), headbg.Bottom, headbg.Left + 5 + space * (a - start), headbg.Top); + } + + if (((a + 360) % 360) == (int)groundcourse) + { + blackPen.Width = 6; + graphicsObject.DrawLine(blackPen, headbg.Left + 5 + space * (a - start), headbg.Bottom, headbg.Left + 5 + space * (a - start), headbg.Top); + blackPen.Width = 2; + } + + if (a % 5 == 0) + { + //Console.WriteLine(space +" " + a +" "+ (headbg.Left + 5 + space * (a - start))); + graphicsObject.DrawLine(whitePen, headbg.Left + 5 + space * (a - start), headbg.Bottom - 5, headbg.Left + 5 + space * (a - start), headbg.Bottom - 10); + int disp = a; + if (disp < 0) + disp += 360; + disp = disp % 360; + if (disp == 0) + { + drawstring(graphicsObject, "N".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 90) + { + drawstring(graphicsObject, "E".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 180) + { + drawstring(graphicsObject, "S".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 270) + { + drawstring(graphicsObject, "W".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else + { + drawstring(graphicsObject, (disp % 360).ToString().PadLeft(3), font, fontsize, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + } + } + + // Console.WriteLine("HUD 0 " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); + + // xtrack error + // center + + float xtspace = this.Width / 10.0f / 3.0f; + int pad = 10; + + float myxtrack_error = xtrack_error; + + myxtrack_error = Math.Min(myxtrack_error, 40); + myxtrack_error = Math.Max(myxtrack_error, -40); + + // xtrack - distance scale - space + float loc = myxtrack_error / 20.0f * xtspace; + + // current xtrack + if (Math.Abs(myxtrack_error) == 40) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + } + + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc, headbg.Bottom + 5, this.Width / 10 + loc, headbg.Bottom + this.Height / 10); + + greenPen.Color = Color.FromArgb(255, greenPen.Color); + + graphicsObject.DrawLine(whitePen, this.Width / 10, headbg.Bottom + 5, this.Width / 10, headbg.Bottom + this.Height / 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace, headbg.Bottom + 5 + pad, this.Width / 10 - xtspace, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 2, headbg.Bottom + 5 + pad, this.Width / 10 - xtspace * 2, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace, headbg.Bottom + 5 + pad, this.Width / 10 + xtspace, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace * 2, headbg.Bottom + 5 + pad, this.Width / 10 + xtspace * 2, headbg.Bottom + this.Height / 10 - pad); + + // rate of turn + + whitePen.Width = 4; + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 2 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 - xtspace * 2 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 0 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 - xtspace * 0 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace * 2 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 + xtspace * 2 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + float myturnrate = turnrate; + float trwidth = (this.Width / 10 + xtspace * 2 - xtspace / 2) - (this.Width / 10 - xtspace * 2 - xtspace / 2); + + float range = 12; + + myturnrate = Math.Min(myturnrate, range / 2); + myturnrate = Math.Max(myturnrate, (range / 2) * -1.0f); + + loc = myturnrate / range * trwidth; + + greenPen.Width = 4; + + if (Math.Abs(myturnrate) == (range / 2)) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + } + + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc - xtspace / 2, headbg.Bottom + this.Height / 10 + 10 + 3, this.Width / 10 + loc + xtspace / 2, headbg.Bottom + this.Height / 10 + 10 + 3); + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc, headbg.Bottom + this.Height / 10 + 10 + 3, this.Width / 10 + loc, headbg.Bottom + this.Height / 10 + 10 + 10); + + greenPen.Color = Color.FromArgb(255, greenPen.Color); + + whitePen.Width = 2; + + + + // left scroller + + Rectangle scrollbg = new Rectangle(0, halfheight - halfheight / 2, this.Width / 10, this.Height / 2); + + graphicsObject.DrawRectangle(whitePen, scrollbg); + + graphicsObject.FillRectangle(solidBrush, scrollbg); + + Point[] arrow = new Point[5]; + + arrow[0] = new Point(0, -10); + arrow[1] = new Point(scrollbg.Width - 10, -10); + arrow[2] = new Point(scrollbg.Width - 5, 0); + arrow[3] = new Point(scrollbg.Width - 10, 10); + arrow[4] = new Point(0, 10); + + graphicsObject.TranslateTransform(0, this.Height / 2); + + int viewrange = 26; + + float speed = airspeed; + if (speed == 0) + speed = groundspeed; + + space = (scrollbg.Height) / (float)viewrange; + start = ((int)speed - viewrange / 2); + + if (start > targetspeed) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top, scrollbg.Left + scrollbg.Width, scrollbg.Top); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + if ((speed + viewrange / 2) < targetspeed) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * viewrange, scrollbg.Left + scrollbg.Width, scrollbg.Top - space * viewrange); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + + for (int a = start; a <= (speed + viewrange / 2); a += 1) + { + if (a == (int)targetspeed && targetspeed != 0) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + scrollbg.Width, scrollbg.Top - space * (a - start)); + } + if (a % 5 == 0) + { + //Console.WriteLine(a + " " + scrollbg.Right + " " + (scrollbg.Top - space * (a - start)) + " " + (scrollbg.Right - 20) + " " + (scrollbg.Top - space * (a - start))); + graphicsObject.DrawLine(whitePen, scrollbg.Right, scrollbg.Top - space * (a - start), scrollbg.Right - 10, scrollbg.Top - space * (a - start)); + drawstring(graphicsObject, a.ToString().PadLeft(5), font, fontsize, whiteBrush, scrollbg.Right - 50 - 4 * fontoffset, scrollbg.Top - space * (a - start) - 6 - fontoffset); + } + } + + graphicsObject.DrawPolygon(blackPen, arrow); + graphicsObject.FillPolygon(Brushes.Black, arrow); + drawstring(graphicsObject, ((int)speed).ToString("0"), font, 10, Brushes.AliceBlue, 0, -9); + + graphicsObject.ResetTransform(); + + // extra text data + + drawstring(graphicsObject, "AS " + airspeed.ToString("0.0"), font, fontsize, whiteBrush, 1, scrollbg.Bottom + 5); + drawstring(graphicsObject, "GS " + groundspeed.ToString("0.0"), font, fontsize, whiteBrush, 1, scrollbg.Bottom + fontsize + 2 + 10); + + //drawstring(e,, new Font("Arial", fontsize + 2), whiteBrush, 1, scrollbg.Bottom + fontsize + 2 + 10); + + // right scroller + + scrollbg = new Rectangle(this.Width - this.Width / 10, halfheight - halfheight / 2, this.Width / 10, this.Height / 2); + + graphicsObject.DrawRectangle(whitePen, scrollbg); + + graphicsObject.FillRectangle(solidBrush, scrollbg); + + arrow = new Point[5]; + + arrow[0] = new Point(0, -10); + arrow[1] = new Point(scrollbg.Width - 10, -10); + arrow[2] = new Point(scrollbg.Width - 5, 0); + arrow[3] = new Point(scrollbg.Width - 10, 10); + arrow[4] = new Point(0, 10); + + + + graphicsObject.TranslateTransform(0, this.Height / 2); + + + + + viewrange = 26; + + space = (scrollbg.Height) / (float)viewrange; + start = ((int)alt - viewrange / 2); + + if (start > targetalt) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top, scrollbg.Left + scrollbg.Width, scrollbg.Top); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + if ((alt + viewrange / 2) < targetalt) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * viewrange, scrollbg.Left + scrollbg.Width, scrollbg.Top - space * viewrange); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + + for (int a = start; a <= (alt + viewrange / 2); a += 1) + { + if (a == Math.Round(targetalt) && targetalt != 0) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + scrollbg.Width, scrollbg.Top - space * (a - start)); + } + if (a % 5 == 0) + { + //Console.WriteLine(a + " " + scrollbg.Left + " " + (scrollbg.Top - space * (a - start)) + " " + (scrollbg.Left + 20) + " " + (scrollbg.Top - space * (a - start))); + graphicsObject.DrawLine(whitePen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + 10, scrollbg.Top - space * (a - start)); + drawstring(graphicsObject, a.ToString().PadLeft(5), font, fontsize, whiteBrush, scrollbg.Left + 7 + (int)(0 * fontoffset), scrollbg.Top - space * (a - start) - 6 - fontoffset); + } + } + + + + // vsi + + graphicsObject.ResetTransform(); + + PointF[] poly = new PointF[4]; + + poly[0] = new PointF(scrollbg.Left, scrollbg.Top); + poly[1] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + scrollbg.Width / 4); + poly[2] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Bottom - scrollbg.Width / 4); + poly[3] = new PointF(scrollbg.Left, scrollbg.Bottom); + + //verticalspeed + + viewrange = 12; + + verticalspeed = Math.Min(viewrange / 2, verticalspeed); + verticalspeed = Math.Max(viewrange / -2, verticalspeed); + + float scaledvalue = verticalspeed / -viewrange * (scrollbg.Bottom - scrollbg.Top); + + float linespace = (float)1 / -viewrange * (scrollbg.Bottom - scrollbg.Top); + + PointF[] polyn = new PointF[4]; + + polyn[0] = new PointF(scrollbg.Left, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2); + polyn[1] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2); + polyn[2] = polyn[1]; + float peak = 0; + if (scaledvalue > 0) + { + peak = -scrollbg.Width / 4; + if (scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak < scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2) + peak = -scaledvalue; + } + else if (scaledvalue < 0) + { + peak = +scrollbg.Width / 4; + if (scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak > scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2) + peak = -scaledvalue; + } + + polyn[2] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak); + polyn[3] = new PointF(scrollbg.Left, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue); + + //graphicsObject.DrawPolygon(redPen, poly); + graphicsObject.FillPolygon(Brushes.Blue, polyn); + + // draw outsidebox + graphicsObject.DrawPolygon(whitePen, poly); + + for (int a = 1; a < viewrange; a++) + { + graphicsObject.DrawLine(whitePen, scrollbg.Left - scrollbg.Width / 4, scrollbg.Top - linespace * a, scrollbg.Left - scrollbg.Width / 8, scrollbg.Top - linespace * a); + } + + // draw arrow and text + + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(this.Width, this.Height / 2); + graphicsObject.RotateTransform(180); + + graphicsObject.DrawPolygon(blackPen, arrow); + graphicsObject.FillPolygon(Brushes.Black, arrow); + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(0, this.Height / 2); + + drawstring(graphicsObject, ((int)alt).ToString("0"), font, 10, Brushes.AliceBlue, scrollbg.Left + 10, -9); + graphicsObject.ResetTransform(); + + // mode and wp dist and wp + + drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5); + drawstring(graphicsObject, (int)disttowp + ">" + wpno, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + fontsize + 2 + 10); + + // battery + + graphicsObject.ResetTransform(); + + drawstring(graphicsObject, resources.GetString("Bat"), font, fontsize + 2, whiteBrush, fontsize, this.Height - 30 - fontoffset); + drawstring(graphicsObject, batterylevel.ToString("0.00v"), font, fontsize + 2, whiteBrush, fontsize * 4, this.Height - 30 - fontoffset); + drawstring(graphicsObject, batteryremaining.ToString("0%"), font, fontsize + 2, whiteBrush, fontsize * 9, this.Height - 30 - fontoffset); + + // gps + + string gps = ""; + + if (gpsfix == 0) + { + gps = resources.GetString("GPS: No Fix.Text"); + } + else if (gpsfix == 1) + { + gps = resources.GetString("GPS: No Fix.Text"); + } + else if (gpsfix == 2) + { + gps = resources.GetString("GPS: 2D Fix.Text"); + } + else if (gpsfix == 3) + { + gps = resources.GetString("GPS: 3D Fix.Text"); + } + + drawstring(graphicsObject, gps, font, fontsize + 2, whiteBrush, this.Width - 10 * fontsize, this.Height - 30 - fontoffset); + + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + + if (DesignMode) + { + return; + } + + // Console.WriteLine("HUD 1 " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); + + ImageCodecInfo ici = GetImageCodec("image/jpeg"); + EncoderParameters eps = new EncoderParameters(1); + eps.Param[0] = new EncoderParameter(System.Drawing.Imaging.Encoder.Quality, 50L); // or whatever other quality value you want + + lock (streamlock) + { + if (streamjpgenable || streamjpg == null) // init image and only update when needed + { + streamjpg = new MemoryStream(); + objBitmap.Save(streamjpg, ici, eps); + //objBitmap.Save(streamjpg,ImageFormat.Bmp); + } + } + } + catch (Exception ex) + { + Console.WriteLine("hud error "+ex.ToString()); + //MessageBox.Show(ex.ToString()); + } + + count++; + + if (DateTime.Now.Second != countdate.Second) + { + countdate = DateTime.Now; + //Console.WriteLine("HUD " + count + " hz"); + count = 0; + } + huddrawtime = (int)(DateTime.Now - starttime).TotalMilliseconds; +#if DEBUG + // Console.WriteLine("HUD e " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); +#endif + } + + protected override void OnPaintBackground(PaintEventArgs e) + { + //base.OnPaintBackground(e); + } + + ImageCodecInfo GetImageCodec(string mimetype) + { + foreach (ImageCodecInfo ici in ImageCodecInfo.GetImageEncoders()) + { + if (ici.MimeType == mimetype) return ici; + } + return null; + } + + + float wrap360(float noin) + { + if (noin < 0) + return noin + 360; + return noin; + } + + /// + /// pen for drawstring + /// + Pen P = new Pen(Color.FromArgb(0x26, 0x27, 0x28), 2f); + /// + /// pth for drawstring + /// + GraphicsPath pth = new GraphicsPath(); + + void drawstring(Graphics e, string text, Font font, float fontsize, Brush brush, float x, float y) + { + if (text == null || text == "") + return; + + pth.Reset(); + + + if (text != null) + pth.AddString(text, font.FontFamily, 0, fontsize + 5, new Point((int)x, (int)y), StringFormat.GenericTypographic); + + //Draw the edge + // this uses lots of cpu time + + //e.SmoothingMode = SmoothingMode.HighSpeed; + + e.DrawPath(P, pth); + + //Draw the face + + e.FillPath(brush, pth); + + //pth.Dispose(); + } + + protected override void OnResize(EventArgs e) + { + if (DesignMode) + return; + this.Height = (int)(this.Width / 1.333f); + base.OnResize(e); + /* + try + { + GL.MatrixMode(MatrixMode.Projection); + GL.LoadIdentity(); + GL.Ortho(0, Width, Height, 0, -1, 1); + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + + GL.Viewport(0, 0, Width, Height); + } + catch { } + */ + } + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl b/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl new file mode 100644 index 0000000000..b22c7dd9e7 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl @@ -0,0 +1,132 @@ +$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/message_definitions/"; +#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/pixhawk-mavlink-c91adfb/include/common/"; + +opendir(DIR,$dir) || die print $!; +@files = readdir(DIR); +closedir(DIR); + +open(OUT,">MAVLinkTypes.cs"); + +print OUT <) { + if ($line =~ /enum name="(MAV_.*)"/) { + $start = 1; + print OUT "\t\tpublic enum $1\n\t\t{ \n"; + } + + if ($line =~ //) { + $name = lc($2); + + print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n"; + print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; + print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n"; + $no = $1; + + $start = 1; + + #__mavlink_gps_raw_t + $structs[$no] = "__mavlink_".$name."_t"; + } # __mavlink_heartbeat_t + + $line =~ s/MAV_CMD_NAV_//; + + $line =~ s/MAV_CMD_//; + + if ($line =~ //) + { + + + print OUT "\t\t\t$2 = $1,\n"; + + } + + # + if ($line =~ /(.*)<\/field>/) + { + + $type = $1; + $name = $2; + $desc = $3; + + print "$type = $name\n"; + + $type =~ s/byte_mavlink_version/public byte/; + + $type =~ s/array/public byte/; + + + + $type =~ s/uint8_t/public byte/; + $type =~ s/int8_t/public byte/; + $type =~ s/float/public float/; + $type =~ s/uint16_t/public ushort/; + $type =~ s/uint32_t/public uint/; + $type =~ s/uint64_t/public ulong/; + $type =~ s/int16_t/public short/; + $type =~ s/int32_t/public int/; + $type =~ s/int64_t/public long/; + + if ($type =~ /\[(.*)\]/) { # array + print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n"; + $type =~ s/\[.*\]//; + $type =~ s/public\s+([^\s]+)/public $1\[\]/o; + } + + print OUT "\t\t\t$type $name; ///< $desc\n"; + + } + + if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) { + print OUT "\t\t};\n\n"; + $start = 0; + } + + } + + close(F); +} + +print OUT "Type[] mavstructs = new Type[] {"; +for ($a = 0; $a <= 256;$a++) +{ + if (defined($structs[$a])) { + print OUT "typeof(".$structs[$a] .") ,"; + } else { + print OUT "null ,"; + } +} +print OUT "};\n\n"; + +print OUT <MAVLinkTypes.cs"); +$crcs = 0; + print OUT <) { - if ($line =~ /enum name="(MAV_.*)"/) { - $start = 1; - print OUT "\t\tpublic enum $1\n\t\t{ \n"; + if ($line =~ /(MAVLINK_MESSAGE_LENGTHS|MAVLINK_MESSAGE_CRCS) (.*)/ && $crcs < 2) { + print OUT "\t\tpublic byte[] $1 = new byte[] $2;\n"; + $crcs++; } - if ($line =~ //) { - $name = lc($2); - - print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n"; - print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; - print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n"; - $no = $1; - + if ($line =~ /enum (MAV_.*)/) { $start = 1; - + print OUT "\t\tpublic "; + } + + if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) { + print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n"; + $no = $2; + } + if ($line =~ /typedef struct(.*)/) { + if ($1 =~ /__mavlink_system|param_union/) { + last; + } + $start = 1; + print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; #__mavlink_gps_raw_t - $structs[$no] = "__mavlink_".$name."_t"; - } # __mavlink_heartbeat_t - - $line =~ s/MAV_CMD_NAV_//; - - $line =~ s/MAV_CMD_//; - - if ($line =~ //) - { - - - print OUT "\t\t\t$2 = $1,\n"; - + $structs[$no] = $1; } - - # - if ($line =~ /(.*)<\/field>/) - { - - $type = $1; - $name = $2; - $desc = $3; + if ($start) { + $line =~ s/MAV_CMD_NAV_//; - print "$type = $name\n"; + $line =~ s/MAV_CMD_//; - $type =~ s/byte_mavlink_version/public byte/; - - $type =~ s/array/public byte/; + $line =~ s/typedef/public/; + $line =~ s/uint8_t/public byte/; + $line =~ s/int8_t/public byte/; + $line =~ s/^\s+float/public float/; + $line =~ s/uint16_t/public ushort/; + $line =~ s/uint32_t/public uint/; + $line =~ s/uint64_t/public ulong/; + $line =~ s/int16_t/public short/; + $line =~ s/int32_t/public int/; + $line =~ s/int64_t/public long/; + $line =~ s/typedef/public/; + $line =~ s/}.*/};\n/; - - $type =~ s/uint8_t/public byte/; - $type =~ s/int8_t/public byte/; - $type =~ s/float/public float/; - $type =~ s/uint16_t/public ushort/; - $type =~ s/uint32_t/public uint/; - $type =~ s/uint64_t/public ulong/; - $type =~ s/int16_t/public short/; - $type =~ s/int32_t/public int/; - $type =~ s/int64_t/public long/; - - if ($type =~ /\[(.*)\]/) { # array - print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n"; - $type =~ s/\[.*\]//; - $type =~ s/public\s+([^\s]+)/public $1\[\]/o; - } + if ($line =~ /\[(.*)\].*;/) { # array + print OUT "\t\t[MarshalAs( + UnmanagedType.ByValArray, + SizeConst=". $1 .")] \n"; + $line =~ s/\[.*\]//; + $line =~ s/public\s+([^\s]+)/public $1\[\]/o; + } - print OUT "\t\t\t$type $name; ///< $desc\n"; - - } - - if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) { - print OUT "\t\t};\n\n"; + print OUT "\t\t".$line; + } + if ($line =~ /}/) { $start = 0; } @@ -129,4 +128,6 @@ EOF close OUT; +; + 1; \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs new file mode 100644 index 0000000000..4b75ee5c52 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -0,0 +1,171 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +namespace ArdupilotMega +{ + class srtm + { + public static string datadirectory; + + public static int getAltitude(double lat, double lng) + { + short alt = -32768; + + lat += 0.00083333333333333; + //lng += 0.0008; + + int x = (int)Math.Floor(lng); + int y = (int)Math.Floor(lat); + + string ns; + if (y > 0) + ns = "N"; + else + ns = "S"; + + string ew; + if (x > 0) + ew = "E"; + else + ew = "W"; + + string filename = ns + Math.Abs(y).ToString("00") + ew + Math.Abs(x).ToString("000") + ".hgt"; + + string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc"; + + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + { // srtm hgt files + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read); + + float posx = 0; + float row = 0; + + if (fs.Length <= (1201 * 1201 * 2)) + { + posx = (int)(((float)(lng - x)) * (1201 * 2)); + row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); + row = (1201 * 1201 * 2) - row; + } + else + { + posx = (int)(((float)(lng - x)) * (3601 * 2)); + row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); + row = (3601 * 3601 * 2) - row; + } + + if (posx % 2 == 1) + { + posx--; + } + + //Console.WriteLine(filename + " row " + row + " posx" + posx); + + byte[] data = new byte[2]; + + fs.Seek((int)(row + posx), SeekOrigin.Begin); + fs.Read(data, 0, data.Length); + + fs.Close(); + fs.Dispose(); + + Array.Reverse(data); + + alt = BitConverter.ToInt16(data, 0); + + return alt; + } + else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) + { + // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read); + + StreamReader sr = new StreamReader(fs); + + int nox = 0; + int noy = 0; + float left = 0; + float top = 0; + int nodata = -9999; + float cellsize = 0; + + int rowcounter = 0; + + float wantrow = 0; + float wantcol = 0; + + + while (!sr.EndOfStream) + { + string line = sr.ReadLine(); + + if (line.StartsWith("ncols")) + { + nox = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("nrows")) + { + noy = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("xllcorner")) + { + left = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("yllcorner")) + { + top = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("cellsize")) + { + cellsize = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("NODATA_value")) + { + nodata = int.Parse(line.Substring(line.IndexOf(' '))); + } + else + { + string[] data = line.Split(new char[] { ' ' }); + + if (data.Length == (nox + 1)) + { + + + + wantcol = (float)((lng - Math.Round(left,0))); + + wantrow = (float)((lat - Math.Round(top,0))); + + wantrow =(int)( wantrow / cellsize); + wantcol =(int)( wantcol / cellsize); + + wantrow = noy - wantrow; + + if (rowcounter == wantrow) + { + Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize)); + + return int.Parse(data[(int)wantcol]); + } + + rowcounter++; + } + } + + + + } + + return alt; + } + + return alt; + } + } +} diff --git a/Tools/CPUInfo/CPUInfo.pde b/Tools/CPUInfo/CPUInfo.pde new file mode 100644 index 0000000000..4083ca57e1 --- /dev/null +++ b/Tools/CPUInfo/CPUInfo.pde @@ -0,0 +1,122 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/* + test CPU speed + Andrew Tridgell September 2011 +*/ + +#include +#include + +FastSerialPort0(Serial); + +#define SERIAL0_BAUD 115200 + +void setup() { + Serial.begin(SERIAL0_BAUD, 128, 128); +} + +static void show_sizes(void) +{ + Serial.println("Type sizes:"); + Serial.printf("char : %d\n", sizeof(char)); + Serial.printf("short : %d\n", sizeof(short)); + Serial.printf("int : %d\n", sizeof(int)); + Serial.printf("long : %d\n", sizeof(long)); + Serial.printf("long long : %d\n", sizeof(long long)); + Serial.printf("bool : %d\n", sizeof(bool)); + Serial.printf("void* : %d\n", sizeof(void *)); +} + +#define TENTIMES(x) do { x; x; x; x; x; x; x; x; x; x; } while (0) +#define FIFTYTIMES(x) do { TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); } while (0) + +#define TIMEIT(name, op, count) do { \ + uint32_t us_end, us_start; \ + us_start = micros(); \ + for (uint8_t i=0; i 1: + dname = sys.argv[1] +else: + dname = '.' + +for root, dirs, files in os.walk(dname): + for f in files: + if f.endswith(".lst"): + process_lst(os.path.join(root, f)) + +sorted_frames = sorted(frames, + key=operator.attrgetter('frame_size'), + reverse=True) + +print("FrameSize Code") +for frame in sorted_frames: + if frame.frame_size > 0: + print("%9u %s" % (frame.frame_size, frame.code)) + diff --git a/cmake/PdeP.jar b/cmake/PdeP.jar new file mode 100644 index 0000000000..ae76f9b762 Binary files /dev/null and b/cmake/PdeP.jar differ diff --git a/cmake/modules/ArduinoProcessing.cmake b/cmake/modules/ArduinoProcessing.cmake new file mode 100644 index 0000000000..c7a2407a2a --- /dev/null +++ b/cmake/modules/ArduinoProcessing.cmake @@ -0,0 +1,103 @@ +# 1. Concatenate all PDE files +# 2. Write #include "WProgram.h" +# 3. Write prototypes +# 4. Write original sources +# +# +# Prefix Writer +# 1. Scrub comments +# 2. Optionally subsitute Unicode +# 3. Find imports +# 4. Find prototypes +# +# Find prototypes +# 1. Strip comments, quotes, preprocessor directives +# 2. Collapse braches +# 3. Regex + + +set(SINGLE_QUOTES_REGEX "('.')") +set(DOUBLE_QUOTES_REGEX "(\"([^\"\\\\]|\\\\.)*\")") +set(SINGLE_COMMENT_REGEX "([ ]*//[^\n]*)") +set(MULTI_COMMENT_REGEX "(/[*][^/]*[*]/)") +set(PREPROC_REGEX "([ ]*#(\\\\[\n]|[^\n])*)") + +#"[\w\[\]\*]+\s+[&\[\]\*\w\s]+\([&,\[\]\*\w\s]*\)(?=\s*\{)" +set(PROTOTPYE_REGEX "([a-zA-Z0-9]+[ ]*)*[a-zA-Z0-9]+[ ]*\([^{]*\)[ ]*{") + +function(READ_SKETCHES VAR_NAME ) + set(SKETCH_SOURCE) + foreach(SKETCH ${ARGN}) + if(EXISTS ${SKETCH}) + message(STATUS "${SKETCH}") + file(READ ${SKETCH} SKETCH_CONTENTS) + set(SKETCH_SOURCE "${SKETCH_SOURCE}\n${SKETCH_CONTENTS}") + else() + message(FATAL_ERROR "Sketch file does not exist: ${SKETCH}") + endif() + endforeach() + set(${VAR_NAME} "${SKETCH_SOURCE}" PARENT_SCOPE) +endfunction() + +function(STRIP_SOURCES VAR_NAME SOURCES) + string(REGEX REPLACE "${SINGLE_QUOTES_REGEX}|${DOUBLE_QUOTES_REGEX}|${SINGLE_COMMENT_REGEX}|${MULTI_COMMENT_REGEX}|${PREPROC_REGEX}" + "" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +function(COLLAPSE_BRACES VAR_NAME SOURCES) + set(PARSED_SOURCES) + string(LENGTH "${SOURCES}" SOURCES_LENGTH) + math(EXPR SOURCES_LENGTH "${SOURCES_LENGTH}-1") + + set(NESTING 0) + set(START 0) + foreach(INDEX RANGE ${SOURCES_LENGTH}) + string(SUBSTRING "${SOURCES}" ${INDEX} 1 CURRENT_CHAR) + #message("${CURRENT_CHAR}") + if(CURRENT_CHAR STREQUAL "{") + if(NESTING EQUAL 0) + math(EXPR SUBLENGTH "${INDEX}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + #message("INDEX: ${INDEX} START: ${START} LENGTH: ${SUBLENGTH}") + endif() + math(EXPR NESTING "${NESTING}+1") + elseif(CURRENT_CHAR STREQUAL "}") + math(EXPR NESTING "${NESTING}-1") + if(NESTING EQUAL 0) + set(START ${INDEX}) + endif() + endif() + endforeach() + + math(EXPR SUBLENGTH "${SOURCES_LENGTH}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + + set(${VAR_NAME} "${PARSED_SOURCES}" PARENT_SCOPE) +endfunction() + +function(extract_prototypes VAR_NAME SOURCES) + string(REGEX MATCHALL "${PROTOTPYE_REGEX}" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +read_sketches(SKETCH_SOURCE ${FILES}) +strip_sources(SKETCH_SOURCE "${SKETCH_SOURCE}") +collapse_braces(SKETCH_SOURCE "${SKETCH_SOURCE}") +extract_prototypes(SKETCH_SOURCE "${SKETCH_SOURCE}") + + + + +message("===============") +foreach(ENTRY ${SKETCH_SOURCE}) + message("START]]]${ENTRY}[[[END") +endforeach() +message("===============") +#message("${SKETCH_SOURCE}") diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake new file mode 100644 index 0000000000..7faadeba15 --- /dev/null +++ b/cmake/modules/FindArduino.cmake @@ -0,0 +1,581 @@ +# - Generate firmware and libraries for Arduino Devices +# generate_arduino_firmware(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# _SRCS # Sources +# _HDRS # Headers +# _SKETCHES # Arduino sketch files +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _PORT # Serial port, for upload and serial targets [OPTIONAL] +# _AFLAGS # Override global Avrdude flags for target +# _SERIAL # Serial command for serial target [OPTIONAL] +# _NO_AUTOLIBS # Disables Arduino library detection +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_firmware(test) +# +# +# generate_arduino_library(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# +# _SRCS # Sources +# _HDRS # Headers +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _NO_AUTOLIBS # Disables Arduino library detection +# +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_library(test) + + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt hardware libraries + PATH_SUFFIXES share/arduino + DOC "Arduino Development Kit path.") + + +# load_board_settings() +# +# Load the Arduino SDK board settings from the boards.txt file. +# +function(LOAD_BOARD_SETTINGS) + if(NOT ARDUINO_BOARDS AND ARDUINO_BOARDS_PATH) + file(STRINGS ${ARDUINO_BOARDS_PATH} BOARD_SETTINGS) + foreach(BOARD_SETTING ${BOARD_SETTINGS}) + if("${BOARD_SETTING}" MATCHES "^.*=.*") + string(REGEX MATCH "^[^=]+" SETTING_NAME ${BOARD_SETTING}) + string(REGEX MATCH "[^=]+$" SETTING_VALUE ${BOARD_SETTING}) + string(REPLACE "." ";" SETTING_NAME_TOKENS ${SETTING_NAME}) + + list(LENGTH SETTING_NAME_TOKENS SETTING_NAME_TOKENS_LEN) + + + # Add Arduino to main list of arduino boards + list(GET SETTING_NAME_TOKENS 0 BOARD_ID) + list(FIND ARDUINO_BOARDS ${BOARD_ID} ARDUINO_BOARD_INDEX) + if(ARDUINO_BOARD_INDEX LESS 0) + list(APPEND ARDUINO_BOARDS ${BOARD_ID}) + endif() + + # Add setting to board settings list + list(GET SETTING_NAME_TOKENS 1 BOARD_SETTING) + list(FIND ${BOARD_ID}.SETTINGS ${BOARD_SETTING} BOARD_SETTINGS_LEN) + if(BOARD_SETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.SETTINGS ${BOARD_SETTING}) + set(${BOARD_ID}.SETTINGS ${${BOARD_ID}.SETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board settings list") + endif() + + set(ARDUINO_SETTING_NAME ${BOARD_ID}.${BOARD_SETTING}) + + # Add sub-setting to board sub-settings list + if(SETTING_NAME_TOKENS_LEN GREATER 2) + list(GET SETTING_NAME_TOKENS 2 BOARD_SUBSETTING) + list(FIND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING} BOARD_SUBSETTINGS_LEN) + if(BOARD_SUBSETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING}) + set(${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board sub-settings list") + endif() + set(ARDUINO_SETTING_NAME ${ARDUINO_SETTING_NAME}.${BOARD_SUBSETTING}) + endif() + + # Save setting value + set(${ARDUINO_SETTING_NAME} ${SETTING_VALUE} CACHE INTERNAL "Arduino ${BOARD_ID} Board setting") + + + endif() + endforeach() + set(ARDUINO_BOARDS ${ARDUINO_BOARDS} CACHE STRING "List of detected Arduino Board configurations") + mark_as_advanced(ARDUINO_BOARDS) + endif() +endfunction() + +# print_board_settings(ARDUINO_BOARD) +# +# ARDUINO_BOARD - Board id +# +# Print the detected Arduino board settings. +# +function(PRINT_BOARD_SETTINGS ARDUINO_BOARD) + if(${ARDUINO_BOARD}.SETTINGS) + + message(STATUS "Arduino ${ARDUINO_BOARD} Board:") + foreach(BOARD_SETTING ${${ARDUINO_BOARD}.SETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}}") + endif() + if(${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS) + foreach(BOARD_SUBSETTING ${${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}}") + endif() + endforeach() + endif() + message(STATUS "") + endforeach() + endif() +endfunction() + + + +# generate_arduino_library(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_LIBRARY TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD) # Board name (such as uno, mega2560, ...) + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS}) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + add_library(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) +endfunction() + +# generate_arduino_firmware(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_FIRMWARE TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD # Board name (such as uno, mega2560, ...) + _PORT # Serial port, for upload and serial targets + _AFLAGS # Override global Avrdude flags for target + _SKETCHES # Arduino sketch files + _SERIAL) # Serial command for serial target + + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + message(STATUS "Sketches ${INPUT_SKETCHES}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SKETCHES} ${INPUT_SRCS} ${INPUT_HDRS} ) + #set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS} ) + + #set compile flags and file properties for pde files + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES LANGUAGE CXX) + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES COMPILE_FLAGS "-x c++" ) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + #setup_arduino_sketch(SKETCH_SRCS ${INPUT_SKETCHES}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + setup_arduino_target(${TARGET_NAME} "${ALL_SRCS}" "${ALL_LIBS}") + #SET_TARGET_PROPERTIES(${TARGET_NAME} PROPERTIES LINKER_LANGUAGE CXX) + #setup_arduino_target(${TARGET_NAME} "${INPUT_SKETCHES}" ${INPUT_HDRS} "${ALL_LIBS}") + + if(INPUT_PORT) + setup_arduino_upload(${INPUT_BOARD} ${TARGET_NAME} ${INPUT_PORT}) + endif() + + if(INPUT_SERIAL) + setup_serial_target(${TARGET_NAME} "${INPUT_SERIAL}") + endif() +endfunction() + + +# load_generator_settings(TARGET_NAME PREFIX [SUFFIX_1 SUFFIX_2 .. SUFFIX_N]) +# +# TARGET_NAME - The base name of the user settings +# PREFIX - The prefix name used for generator settings +# SUFFIX_XX - List of suffixes to load +# +# Loads a list of user settings into the generators scope. User settings have +# the following syntax: +# +# ${BASE_NAME}${SUFFIX} +# +# The BASE_NAME is the target name and the suffix is a specific generator settings. +# +# For every user setting found a generator setting is created of the follwoing fromat: +# +# ${PREFIX}${SUFFIX} +# +# The purpose of loading the settings into the generator is to not modify user settings +# and to have a generic naming of the settings within the generator. +# +function(LOAD_GENERATOR_SETTINGS TARGET_NAME PREFIX) + foreach(GEN_SUFFIX ${ARGN}) + if(${TARGET_NAME}${GEN_SUFFIX}) + set(${PREFIX}${GEN_SUFFIX} ${${TARGET_NAME}${GEN_SUFFIX}} PARENT_SCOPE) + endif() + endforeach() +endfunction() + +# setup_arduino_compiler(BOARD_ID) +# +# BOARD_ID - The board id name +# +# Configures the the build settings for the specified Arduino Board. +# +macro(setup_arduino_compiler BOARD_ID) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + include_directories(${BOARD_CORE_PATH}) + include_directories(${ARDUINO_LIBRARIES_PATH}) + add_definitions(-DF_CPU=${${BOARD_ID}.build.f_cpu} + -DARDUINO=${ARDUINO_SDK_VERSION} + -mmcu=${${BOARD_ID}.build.mcu} + ) + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + endif() +endmacro() + +# setup_arduino_core(VAR_NAME BOARD_ID) +# +# VAR_NAME - Variable name that will hold the generated library name +# BOARD_ID - Arduino board id +# +# Creates the Arduino Core library for the specified board, +# each board gets it's own version of the library. +# +function(setup_arduino_core VAR_NAME BOARD_ID) + set(CORE_LIB_NAME ${BOARD_ID}_CORE) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME}) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + find_sources(CORE_SRCS ${BOARD_CORE_PATH}) + add_library(${CORE_LIB_NAME} ${CORE_SRCS}) + set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE) + endif() +endfunction() + +# find_arduino_libraries(VAR_NAME SRCS) +# +# VAR_NAME - Variable name which will hold the results +# SRCS - Sources that will be analized +# +# returns a list of paths to libraries found. +# +# Finds all Arduino type libraries included in sources. Available libraries +# are ${ARDUINO_SDK_PATH}/libraries and ${CMAKE_CURRENT_SOURCE_DIR}. +# +# A Arduino library is a folder that has the same name as the include header. +# For example, if we have a include "#include " then the following +# directory structure is considered a Arduino library: +# +# LibraryName/ +# |- LibraryName.h +# `- LibraryName.c +# +# If such a directory is found then all sources within that directory are considred +# to be part of that Arduino library. +# +function(find_arduino_libraries VAR_NAME SRCS) + set(ARDUINO_LIBS ) + foreach(SRC ${SRCS}) + file(STRINGS ${SRC} SRC_CONTENTS) + foreach(SRC_LINE ${SRC_CONTENTS}) + if("${SRC_LINE}" MATCHES "^ *#include *[<\"](.*)[>\"]") + get_filename_component(INCLUDE_NAME ${CMAKE_MATCH_1} NAME_WE) + foreach(LIB_SEARCH_PATH ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) + if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1}) + list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}) + break() + endif() + endforeach() + endif() + endforeach() + endforeach() + if(ARDUINO_LIBS) + list(REMOVE_DUPLICATES ARDUINO_LIBS) + endif() + set(${VAR_NAME} ${ARDUINO_LIBS} PARENT_SCOPE) +endfunction() + +# setup_arduino_library(VAR_NAME BOARD_ID LIB_PATH) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board name +# LIB_PATH - path of the library +# +# Creates an Arduino library, with all it's library dependencies. +# +function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH) + set(LIB_TARGETS) + get_filename_component(LIB_NAME ${LIB_PATH} NAME) + set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME}) + if(NOT TARGET ${TARGET_LIB_NAME}) + find_sources(LIB_SRCS ${LIB_PATH}) + if(LIB_SRCS) + + message(STATUS "Generating Arduino ${LIB_NAME} library") + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + add_library(${TARGET_LIB_NAME} STATIC ${LIB_SRCS}) + + find_arduino_libraries(LIB_DEPS "${LIB_SRCS}") + foreach(LIB_DEP ${LIB_DEPS}) + setup_arduino_library(DEP_LIB_SRCS ${BOARD_ID} ${LIB_DEP}) + list(APPEND LIB_TARGETS ${DEP_LIB_SRCS}) + endforeach() + + target_link_libraries(${TARGET_LIB_NAME} ${BOARD_ID}_CORE ${LIB_TARGETS}) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + else() + # Target already exists, skiping creating + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + if(LIB_TARGETS) + list(REMOVE_DUPLICATES LIB_TARGETS) + endif() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + +# setup_arduino_libraries(VAR_NAME BOARD_ID SRCS) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board ID +# SRCS - source files +# +# Finds and creates all dependency libraries based on sources. +# +function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS) + set(LIB_TARGETS) + find_arduino_libraries(TARGET_LIBS ${SRCS}) + foreach(TARGET_LIB ${TARGET_LIBS}) + setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources + list(APPEND LIB_TARGETS ${LIB_DEPS}) + endforeach() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + + +# setup_arduino_target(TARGET_NAME ALL_SRCS ALL_LIBS) +# +# TARGET_NAME - Target name +# ALL_SRCS - All sources +# ALL_LIBS - All libraries +# +# Creates an Arduino firmware target. +# +function(setup_arduino_target TARGET_NAME ALL_SRCS ALL_LIBS) + add_executable(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) + set_target_properties(${TARGET_NAME} PROPERTIES SUFFIX ".elf") + + set(TARGET_PATH ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_EEP_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.eep + VERBATIM) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_HEX_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.hex + VERBATIM) +endfunction() + +# setup_arduino_upload(BOARD_ID TARGET_NAME PORT) +# +# BOARD_ID - Arduino board id +# TARGET_NAME - Target name +# PORT - Serial port for upload +# +# Create an upload target (${TARGET_NAME}-upload) for the specified Arduino target. +# +function(setup_arduino_upload BOARD_ID TARGET_NAME PORT) + set(AVRDUDE_FLAGS ${ARDUINO_AVRDUDE_FLAGS}) + if(DEFINED ${TARGET_NAME}_AFLAGS) + set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS}) + endif() + add_custom_target(${TARGET_NAME}-upload + ${ARDUINO_AVRDUDE_PROGRAM} + -U flash:w:${TARGET_NAME}.hex + ${AVRDUDE_FLAGS} + -C ${ARDUINO_AVRDUDE_CONFIG_PATH} + -p ${${BOARD_ID}.build.mcu} + -c ${${BOARD_ID}.upload.protocol} + -b ${${BOARD_ID}.upload.speed} + -P ${PORT} + DEPENDS ${TARGET_NAME}) + if(NOT TARGET upload) + add_custom_target(upload) + endif() + add_dependencies(upload ${TARGET_NAME}-upload) +endfunction() + +# find_sources(VAR_NAME LIB_PATH) +# +# VAR_NAME - Variable name that will hold the detected sources +# LIB_PATH - The base path +# +# Finds all C/C++ sources located at the specified path. +# +function(find_sources VAR_NAME LIB_PATH) + file(GLOB_RECURSE LIB_FILES ${LIB_PATH}/*.cpp + ${LIB_PATH}/*.c + ${LIB_PATH}/*.cc + ${LIB_PATH}/*.cxx + ${LIB_PATH}/*.h + ${LIB_PATH}/*.hh + ${LIB_PATH}/*.hxx) + if(LIB_FILES) + set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE) + endif() +endfunction() + +# setup_serial_target(TARGET_NAME CMD) +# +# TARGET_NAME - Target name +# CMD - Serial terminal command +# +# Creates a target (${TARGET_NAME}-serial) for launching the serial termnial. +# +function(setup_serial_target TARGET_NAME CMD) + string(CONFIGURE "${CMD}" FULL_CMD @ONLY) + add_custom_target(${TARGET_NAME}-serial + ${FULL_CMD}) +endfunction() + + +# detect_arduino_version(VAR_NAME) +# +# VAR_NAME - Variable name where the detected version will be saved +# +# Detects the Arduino SDK Version based on the revisions.txt file. +# +function(detect_arduino_version VAR_NAME) + if(ARDUINO_VERSION_PATH) + file(READ ${ARDUINO_VERSION_PATH} ARD_VERSION) + if("${ARD_VERSION}" MATCHES " *[0]+([0-9]+)") + set(${VAR_NAME} ${CMAKE_MATCH_1} PARENT_SCOPE) + endif() + endif() +endfunction() + + +function(convert_arduino_sketch VAR_NAME SRCS) +endfunction() + + +# Setting up Arduino enviroment settings +if(NOT ARDUINO_FOUND) + find_file(ARDUINO_CORES_PATH + NAMES cores + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_LIBRARIES_PATH + NAMES libraries + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_BOARDS_PATH + NAMES boards.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_PROGRAMMERS_PATH + NAMES programmers.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_REVISIONS_PATH + NAMES revisions.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_VERSION_PATH + NAMES lib/version.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_program(ARDUINO_AVRDUDE_PROGRAM + NAMES avrdude + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/tools) + + find_program(ARDUINO_AVRDUDE_CONFIG_PATH + NAMES avrdude.conf + PATHS ${ARDUINO_SDK_PATH} /etc/avrdude + PATH_SUFFIXES hardware/tools + hardware/tools/avr/etc) + + set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0 + CACHE STRING "") + set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom + CACHE STRING "") + set(ARDUINO_AVRDUDE_FLAGS -V -F + CACHE STRING "Arvdude global flag list.") + + if(ARDUINO_SDK_PATH) + detect_arduino_version(ARDUINO_SDK_VERSION) + set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version") + endif(ARDUINO_SDK_PATH) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Arduino + REQUIRED_VARS ARDUINO_SDK_PATH + ARDUINO_SDK_VERSION + VERSION_VAR ARDUINO_SDK_VERSION) + + + mark_as_advanced(ARDUINO_CORES_PATH + ARDUINO_SDK_VERSION + ARDUINO_LIBRARIES_PATH + ARDUINO_BOARDS_PATH + ARDUINO_PROGRAMMERS_PATH + ARDUINO_REVISIONS_PATH + ARDUINO_AVRDUDE_PROGRAM + ARDUINO_AVRDUDE_CONFIG_PATH + ARDUINO_OBJCOPY_EEP_FLAGS + ARDUINO_OBJCOPY_HEX_FLAGS) + load_board_settings() + +endif() + diff --git a/ArduCopter/flight_control.pde b/cmake/modules/Platform/Arduino.cmake similarity index 100% rename from ArduCopter/flight_control.pde rename to cmake/modules/Platform/Arduino.cmake diff --git a/cmake/modules/Platform/ArduinoPaths.cmake b/cmake/modules/Platform/ArduinoPaths.cmake new file mode 100644 index 0000000000..27372098cc --- /dev/null +++ b/cmake/modules/Platform/ArduinoPaths.cmake @@ -0,0 +1,21 @@ +if(UNIX) + include(Platform/UnixPaths) + if(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ~/Applications + /Applications + /Developer/Applications + /sw # Fink + /opt/local) # MacPorts + endif() +elseif(WIN32) + include(Platform/WindowsPaths) +endif() + +if(ARDUINO_SDK_PATH) + if(WIN32) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/utils/bin) + elseif(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + endif() +endif() diff --git a/cmake/toolchains/Arduino.cmake b/cmake/toolchains/Arduino.cmake new file mode 100644 index 0000000000..616c7b9160 --- /dev/null +++ b/cmake/toolchains/Arduino.cmake @@ -0,0 +1,66 @@ +set(CMAKE_SYSTEM_NAME Arduino) + +set(CMAKE_C_COMPILER avr-gcc) +set(CMAKE_CXX_COMPILER avr-g++) + +#=============================================================================# +# C Flags # +#=============================================================================# +set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections") +set(CMAKE_C_FLAGS "-g -Os ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_DEBUG "-g ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELEASE "-Os -DNDEBUG -w ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELWITHDEBINFO "-Os -g -w ${ARDUINO_C_FLAGS}" CACHE STRING "") + +#=============================================================================# +# C++ Flags # +#=============================================================================# +set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions") +set(CMAKE_CXX_FLAGS "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_DEBUG "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELEASE "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-Os -g ${ARDUINO_CXX_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Executable Linker Flags # +#=============================================================================# +set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections -lc -lm") +set(CMAKE_EXE_LINKER_FLAGS "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_MINSIZEREL "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Shared Lbrary Linker Flags # +#=============================================================================# +set(CMAKE_SHARED_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + +set(CMAKE_MODULE_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + + + + +set(ARDUINO_PATHS) +foreach(VERSION RANGE 22 1) + list(APPEND ARDUINO_PATHS arduino-00${VERSION}) +endforeach() + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt + PATH_SUFFIXES share/arduino + Arduino.app/Contents/Resources/Java/ + ${ARDUINO_PATHS} + DOC "Arduino Development Kit path.") + +include(Platform/ArduinoPaths) diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h index 1263f3aa5d..e8a5085d0f 100644 --- a/libraries/APM_BMP085/APM_BMP085.h +++ b/libraries/APM_BMP085/APM_BMP085.h @@ -2,7 +2,7 @@ #define APM_BMP085_h #define TEMP_FILTER_SIZE 16 -#define PRESS_FILTER_SIZE 8 +#define PRESS_FILTER_SIZE 10 class APM_BMP085_Class { diff --git a/libraries/APM_BMP085/CMakeLists.txt b/libraries/APM_BMP085/CMakeLists.txt new file mode 100644 index 0000000000..342fe4668f --- /dev/null +++ b/libraries/APM_BMP085/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_BMP085) + +set(${LIB_NAME}_SRCS + APM_BMP085.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_BMP085.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde index bab2661b19..4d2d23fd16 100644 --- a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde +++ b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde @@ -12,9 +12,11 @@ unsigned long timer; void setup() { - APM_BMP085.Init(); // APM ADC initialization - Serial.begin(38400); + Serial.begin(115200); Serial.println("ArduPilot Mega BMP085 library test"); + Serial.println("Initialising barometer..."); delay(100); + APM_BMP085.Init(); // APM ADC initialization + Serial.println("initialisation complete."); delay(100); delay(1000); timer = millis(); } @@ -28,11 +30,11 @@ void loop() if((millis()- timer) > 50){ timer = millis(); APM_BMP085.Read(); - Serial.print("Pressure:"); + Serial.print("Pressure:"); Serial.print(APM_BMP085.Press); - Serial.print(" Temperature:"); + Serial.print(" Temperature:"); Serial.print(APM_BMP085.Temp / 10.0); - Serial.print(" Altitude:"); + Serial.print(" Altitude:"); tmp_float = (APM_BMP085.Press / 101325.0); tmp_float = pow(tmp_float, 0.190295); Altitude = 44330 * (1.0 - tmp_float); diff --git a/libraries/APM_PI/CMakeLists.txt b/libraries/APM_PI/CMakeLists.txt new file mode 100644 index 0000000000..e65e76f6a4 --- /dev/null +++ b/libraries/APM_PI/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME APM_PI) + +set(${LIB_NAME}_SRCS + APM_PI.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_PI.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..1c2064562e 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,19 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 +// Radio channels +// Note channels are from 0! +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 +#define CH_10 9 //PB5 +#define CH_11 10 //PE3 + #include class APM_RC_Class diff --git a/libraries/APM_RC/CMakeLists.txt b/libraries/APM_RC/CMakeLists.txt new file mode 100644 index 0000000000..3e4aab0011 --- /dev/null +++ b/libraries/APM_RC/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_RC) + +set(${LIB_NAME}_SRCS + APM_RC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_RC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 196a6741c3..5d749c9ae9 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -1,6 +1,8 @@ #ifndef AP_ADC_H #define AP_ADC_H +#include + /* AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega Code by James Goppert. DIYDrones.com @@ -13,7 +15,7 @@ Methods: Init() : Initialization of ADC. (interrupts etc) Ch(ch_num) : Return the ADC channel value - + Ch6(channel_numbers, result) : Return 6 ADC channel values */ class AP_ADC @@ -21,11 +23,24 @@ class AP_ADC public: AP_ADC() {}; // Constructor virtual void Init() {}; - virtual int Ch(unsigned char ch_num) = 0; - virtual int Ch_raw(unsigned char ch_num) = 0; - private: -}; - + + /* read one channel value */ + virtual uint16_t Ch(uint8_t ch_num) = 0; + + /* read 6 channels values as a set, used by IMU for 3 gyros + and 3 accelerometeres. + + Pass in an array of 6 channel numbers and results are + returned in result[] + + The function returns the amount of time (in microseconds) + since the last call to Ch6(). + */ + virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + + private: +}; + #include "AP_ADC_ADS7844.h" #include "AP_ADC_HIL.h" diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 27bbc0a477..be90bba00a 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -19,9 +19,11 @@ XCK2 = SCK = pin PH2 Chip Select pin is PC4 (33) [PH6 (9)] We are using the 16 clocks per conversion timming to increase efficiency (fast) - The sampling frequency is 400Hz (Timer2 overflow interrupt) + + The sampling frequency is 1kHz (Timer2 overflow interrupt) + So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so - we have an 4x oversampling and averaging. + we have an 10x oversampling and averaging. Methods: Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) @@ -44,6 +46,7 @@ extern "C" { // AVR LibC Includes #include + #include #include #include "WConstants.h" } @@ -53,54 +56,77 @@ extern "C" { // Commands for reading ADC channels on ADS7844 static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; -static volatile uint16_t _filter[8][ADC_FILTER_SIZE]; -static volatile uint8_t _filter_index; - -static unsigned char ADC_SPI_transfer(unsigned char data) -{ - /* Wait for empty transmit buffer */ - while ( !( UCSR2A & (1 << UDRE2)) ); - /* Put data into buffer, sends the data */ - UDR2 = data; - /* Wait for data to be received */ + +// the sum of the values since last read +static volatile uint32_t _sum[8]; + +// how many values we've accumulated since last read +static volatile uint16_t _count[8]; + +static uint32_t last_ch6_micros; + +// TCNT2 values for various interrupt rates, +// assuming 256 prescaler. Note that these values +// assume a zero-time ISR. The actual rate will be a +// bit lower than this +#define TCNT2_781_HZ (256-80) +#define TCNT2_1008_HZ (256-62) +#define TCNT2_1302_HZ (256-48) + +static inline unsigned char ADC_SPI_transfer(unsigned char data) +{ + /* Put data into buffer, sends the data */ + UDR2 = data; + /* Wait for data to be received */ while ( !(UCSR2A & (1 << RXC2)) ); /* Get and return received data from buffer */ return UDR2; } -ISR (TIMER2_OVF_vect) -{ - uint8_t ch; - uint16_t adc_tmp; - - //bit_set(PORTL,6); // To test performance - - bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) - ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel - - for (ch = 0; ch < 8; ch++){ - adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte - adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command - - // Fill our Moving average filter - _filter[ch][_filter_index] = adc_tmp >> 3; - } - - // increment our filter - _filter_index++; - - // loop our filter - if(_filter_index == ADC_FILTER_SIZE) - _filter_index = 0; - - - bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400 Hz -} - - +ISR (TIMER2_OVF_vect) +{ + uint8_t ch; + static uint8_t timer_offset; + + bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) + ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel + + for (ch = 0; ch < 8; ch++) { + uint16_t v; + + v = ADC_SPI_transfer(0) << 8; // Read first byte + v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command + + if (v & 0x8007) { + // this is a 12-bit ADC, shifted by 3 bits. + // if we get other bits set then the value is + // bogus and should be ignored + continue; + } + + if (++_count[ch] == 0) { + // overflow ... shouldn't happen too often + // unless we're just not using the + // channel. Notice that we overflow the count + // to 1 here, not zero, as otherwise the + // reader below could get a division by zero + _sum[ch] = 0; + _count[ch] = 1; + last_ch6_micros = micros(); + } + _sum[ch] += (v >> 3); + } + + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) + + // this gives us a sample rate between 781Hz and 1302Hz. We + // randomise it to try to minimise aliasing effects + timer_offset = (timer_offset + 49) % 32; + TCNT2 = TCNT2_781_HZ + timer_offset; +} + + // Constructors //////////////////////////////////////////////////////////////// AP_ADC_ADS7844::AP_ADC_ADS7844() { @@ -108,57 +134,99 @@ AP_ADC_ADS7844::AP_ADC_ADS7844() // Public Methods ////////////////////////////////////////////////////////////// void AP_ADC_ADS7844::Init(void) +{ + pinMode(ADC_CHIP_SELECT, OUTPUT); + + digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) + + // Setup Serial Port2 in SPI mode + UBRR2 = 0; + DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode + // Set MSPI mode of operation and SPI data mode 0. + UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); + // Enable receiver and transmitter. + UCSR2B = (1 << RXEN2) | (1 << TXEN2); + // Set Baud rate + UBRR2 = 2; // SPI clock running at 2.6MHz + + // get an initial value for each channel. This ensures + // _count[] is never zero + for (uint8_t i=0; i<8; i++) { + uint16_t adc_tmp; + adc_tmp = ADC_SPI_transfer(0) << 8; + adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]); + _count[i] = 1; + _sum[i] = adc_tmp; + } + + last_ch6_micros = micros(); + + // Enable Timer2 Overflow interrupt to capture ADC data + TIMSK2 = 0; // Disable interrupts + TCCR2A = 0; // normal counting mode + TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256 + TCNT2 = 0; + TIFR2 = _BV(TOV2); // clear pending interrupts; + TIMSK2 = _BV(TOIE2); // enable the overflow interrupt +} + +// Read one channel value +uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) { - pinMode(ADC_CHIP_SELECT, OUTPUT); + uint16_t count; + uint32_t sum; - digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) + // ensure we have at least one value + while (_count[ch_num] == 0) /* noop */ ; - // Setup Serial Port2 in SPI mode - UBRR2 = 0; - DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode - // Set MSPI mode of operation and SPI data mode 0. - UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); - // Enable receiver and transmitter. - UCSR2B = (1 << RXEN2) | (1 << TXEN2); - // Set Baud rate - UBRR2 = 2; // SPI clock running at 2.6MHz - - - // Enable Timer2 Overflow interrupt to capture ADC data - TIMSK2 = 0; // Disable interrupts - TCCR2A = 0; // normal counting mode - TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 - TCNT2 = 0; - TIFR2 = _BV(TOV2); // clear pending interrupts; - TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt -} - -// Read one channel value -int AP_ADC_ADS7844::Ch(unsigned char ch_num) -{ - uint16_t result = 0; - - //while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator - - // stop interrupts + // grab the value with interrupts disabled, and clear the count cli(); - - // sum our filter - for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){ - result += _filter[ch_num][i]; + count = _count[ch_num]; + sum = _sum[ch_num]; + _count[ch_num] = 0; + _sum[ch_num] = 0; + sei(); + + return sum/count; +} + +// Read 6 channel values +// this assumes that the counts for all of the 6 channels are +// equal. This will only be true if we always consistently access a +// sensor by either Ch6() or Ch() and never mix them. If you mix them +// then you will get very strange results +uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ + uint16_t count[6]; + uint32_t sum[6]; + uint8_t i; + + // ensure we have at least one value + for (i=0; i<6; i++) { + while (_count[channel_numbers[i]] == 0) /* noop */; } - // resume interrupts + // grab the values with interrupts disabled, and clear the counts + cli(); + for (i=0; i<6; i++) { + count[i] = _count[channel_numbers[i]]; + sum[i] = _sum[channel_numbers[i]]; + _count[channel_numbers[i]] = 0; + _sum[channel_numbers[i]] = 0; + } sei(); - // average our sampels - result /= ADC_FILTER_SIZE; - - return(result); -} - -// Read one channel value -int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num) -{ - return _filter[ch_num][_filter_index]; // close enough -} + // calculate averages. We keep this out of the cli region + // to prevent us stalling the ISR while doing the + // division. That costs us 36 bytes of stack, but I think its + // worth it. + for (i=0; i<6; i++) { + result[i] = sum[i] / count[i]; + } + + // return number of microseconds since last call + uint32_t us = micros(); + uint32_t ret = us - last_ch6_micros; + last_ch6_micros = us; + return ret; +} diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 532dfd97de..35f72d36db 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -9,7 +9,7 @@ #define ADC_DATAIN 50 // MISO #define ADC_SPICLOCK 52 // SCK #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 -#define ADC_FILTER_SIZE 6 +#define ADC_FILTER_SIZE 3 #include "AP_ADC.h" #include @@ -19,10 +19,14 @@ class AP_ADC_ADS7844 : public AP_ADC public: AP_ADC_ADS7844(); // Constructor void Init(); - int Ch(unsigned char ch_num); - int Ch_raw(unsigned char ch_num); - - private: -}; + + // Read 1 sensor value + uint16_t Ch(unsigned char ch_num); + + // Read 6 sensors at once + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + private: +}; #endif diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 07cfdbd8f0..108bb1120f 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL() // set diff press and temp to zero setGyroTemp(0); setPressure(0); + + last_hil_time = millis(); } void AP_ADC_HIL::Init(void) @@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void) } // Read one channel value -int AP_ADC_HIL::Ch(unsigned char ch_num) +uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) { - return adcValue[ch_num]; -} -// Read one channel value -int AP_ADC_HIL::Ch_raw(unsigned char ch_num) -{ - return adcValue[ch_num]; + return adcValue[ch_num]; +} + +// Read 6 channel values +uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ + for (uint8_t i=0; i<6; i++) { + result[i] = Ch(channel_numbers[i]); + } + return ((millis() - last_hil_time)*2)/5; } // Set one channel value diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index 736e99a882..1794da8eaf 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -32,13 +32,14 @@ class AP_ADC_HIL : public AP_ADC /// // Read the sensor, part of public AP_ADC interface - int Ch(unsigned char ch_num); - /// - // Read the sensor, part of public AP_ADC interface - int Ch_raw(unsigned char ch_num); - - /// - // Set the adc raw values given the current rotations rates, + uint16_t Ch(unsigned char ch_num); + + /// + // Read 6 sensors at once + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + /// + // Set the adc raw values given the current rotations rates, // temps, accels, and pressures void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); @@ -49,16 +50,19 @@ class AP_ADC_HIL : public AP_ADC // The raw adc array uint16_t adcValue[8]; + // the time in milliseconds when we last got a HIL update + uint32_t last_hil_time; + /// // sensor constants // constants declared in cpp file // @see AP_ADC_HIL.cpp - static const uint8_t sensors[6]; - static const float gyroBias[3]; - static const float gyroScale[3]; + static const uint8_t sensors[6]; + static const float gyroBias[3]; + static const float gyroScale[3]; static const float accelBias[3]; - static const float accelScale[3]; - static const int8_t sensorSign[6]; + static const float accelScale[3]; + static const int8_t sensorSign[6]; /// // gyro set function diff --git a/libraries/AP_ADC/CMakeLists.txt b/libraries/AP_ADC/CMakeLists.txt new file mode 100644 index 0000000000..df85645fb2 --- /dev/null +++ b/libraries/AP_ADC/CMakeLists.txt @@ -0,0 +1,26 @@ +set(LIB_NAME AP_ADC) + +set(${LIB_NAME}_SRCS + AP_ADC_HIL.cpp + AP_ADC_ADS7844.cpp + AP_ADC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_ADC_HIL.h + AP_ADC_ADS7844.h + AP_ADC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index ba4b74baea..93069bf928 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -1,37 +1,94 @@ + /* Example of APM_ADC library. Code by Jordi Muñoz and Jose Julio. DIYDrones.com */ +#include #include // ArduPilot Mega ADC Library -unsigned long timer; +FastSerialPort0(Serial); // FTDI/console +unsigned long timer; AP_ADC_ADS7844 adc; void setup() { - adc.Init(); // APM ADC initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega ADC library test"); - delay(1000); - timer = millis(); -} - -void loop() -{ - int ch; - - if((millis()- timer) > 20) - { - timer = millis(); - for (ch=0;ch<7;ch++) - { - Serial.print(adc.Ch(ch)); - Serial.print(","); - } - Serial.println(); - } -} - -// end of test program + Serial.begin(115200, 128, 128); + Serial.println("ArduPilot Mega ADC library test"); + delay(1000); + adc.Init(); // APM ADC initialization + delay(1000); + timer = millis(); +} + +static const uint8_t channel_map[6] = { 1, 2, 0, 4, 5, 6}; +float v; +uint32_t last_usec = 0; + +static void show_timing() +{ + uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0; + uint32_t start_time = millis(); + uint16_t result[6]; + uint32_t count = 0; + + Serial.println("Starting timing test"); + + adc.Ch6(channel_map, result); + + do { + uint32_t deltat = adc.Ch6(channel_map, result); + if (deltat > maxt) maxt = deltat; + if (deltat < mint) mint = deltat; + totalt += deltat; + count++; + } while ((millis() - start_time) < 5000); + + Serial.printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count); +} + +static void show_data() +{ + uint16_t result[6]; + uint32_t deltat = 0; + uint16_t ch3; + uint16_t min[6], max[6]; + uint8_t i; + + for (i=0;i<6;i++) { + min[i] = 0xFFFF; + max[i] = 0; + } + + + do { + ch3 = adc.Ch(3); + deltat += adc.Ch6(channel_map, result); + for (i=0;i<6;i++) { + if (result[i] < min[i]) min[i] = result[i]; + if (result[i] > max[i]) max[i] = result[i]; + if (result[i] & 0x8000) { + Serial.printf("result[%u]=0x%04x\n", (unsigned)i, result[i]); + } + } + } while ((millis() - timer) < 200); + + timer = millis(); + Serial.printf("g=(%u,%u,%u) a=(%u,%u,%u) +/-(%u,%u,%u,%u,%u,%u) gt=%u dt=%u\n", + result[0], result[1], result[2], + result[3], result[4], result[5], + (max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]), + (max[3]-min[3]), (max[4]-min[4]), (max[5]-min[5]), + ch3, (unsigned)(deltat/1000)); +} + + +void loop() +{ + if (millis() < 5000) { + show_timing(); + } else { + show_data(); + } +} diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index ae3f3e572e..3d19e12593 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -60,6 +60,12 @@ endif # ifeq ($(SKETCHBOOK),) SKETCHBOOK := $(shell cd $(SRCROOT)/.. && pwd) + ifeq ($(wildcard $(SKETCHBOOK)/libraries),) + SKETCHBOOK := $(shell cd $(SRCROOT)/../.. && pwd) + endif + ifeq ($(wildcard $(SKETCHBOOK)/libraries),) + SKETCHBOOK := $(shell cd $(SRCROOT)/../../.. && pwd) + endif ifeq ($(wildcard $(SKETCHBOOK)/libraries),) SKETCHBOOK := $(shell cd $(SRCROOT)/../../../.. && pwd) endif @@ -124,7 +130,7 @@ ifeq ($(ARDUINO),) # ifeq ($(SYSTYPE),Darwin) # use Spotlight to find Arduino.app - ARDUINO_QUERY = 'kMDItemKind == Application && kMDItemDisplayName == Arduino.app' + ARDUINO_QUERY = 'kMDItemKind == Application && kMDItemFSName == Arduino.app' ARDUINOS := $(addsuffix /Contents/Resources/Java,$(shell mdfind -literal $(ARDUINO_QUERY))) ifeq ($(ARDUINOS),) $(error ERROR: Spotlight cannot find Arduino on your system.) @@ -205,10 +211,11 @@ DEPFLAGS = -MD -MT $@ CXXOPTS = -mcall-prologues -ffunction-sections -fdata-sections -fno-exceptions COPTS = -mcall-prologues -ffunction-sections -fdata-sections ASOPTS = -assembler-with-cpp +LISTOPTS = -adhlns=$(@:.o=.lst) -CXXFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) -CFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) -ASFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(DEPFLAGS) $(ASOPTS) +CXXFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) +CFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) +ASFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(LISTOPTS) $(DEPFLAGS) $(ASOPTS) LDFLAGS = -g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP) LIBS = -lm @@ -259,7 +266,11 @@ SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS) # make. # SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +ifeq ($(SYSTYPE),Darwin) + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) +else + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +endif # # Find sketchbook libraries referenced by the sketch. @@ -316,7 +327,9 @@ HARDWARE_CORE := $(shell grep $(BOARD).build.core $(BOARDFILE) | cut -d = -f 2) UPLOAD_SPEED := $(shell grep $(BOARD).upload.speed $(BOARDFILE) | cut -d = -f 2) # This simply does not work, so hardcode it to the correct value #UPLOAD_PROTOCOL := $(shell grep $(BOARD).upload.protocol $(BOARDFILE) | cut -d = -f 2) -UPLOAD_PROTOCOL := arduino +ifeq ($(UPLOAD_PROTOCOL),) + UPLOAD_PROTOCOL := arduino +endif ifeq ($(MCU),) $(error ERROR: Could not locate board $(BOARD) in $(BOARDFILE)) diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt new file mode 100644 index 0000000000..7d89ba1e40 --- /dev/null +++ b/libraries/AP_Common/CMakeLists.txt @@ -0,0 +1,33 @@ +set(LIB_NAME AP_Common) + +set(${LIB_NAME}_SRCS + AP_Common.cpp + AP_Loop.cpp + AP_MetaClass.cpp + AP_Var.cpp + AP_Var_menufuncs.cpp + c++.cpp + menu.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Common.h + AP_Loop.h + AP_MetaClass.h + AP_Var.h + AP_Test.h + c++.h + AP_Vector.h +) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Compass/CMakeLists.txt b/libraries/AP_Compass/CMakeLists.txt new file mode 100644 index 0000000000..613b751ef4 --- /dev/null +++ b/libraries/AP_Compass/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_Compass) + +set(${LIB_NAME}_SRCS + AP_Compass_HIL.cpp + AP_Compass_HMC5843.cpp + Compass.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + Compass.h + AP_Compass_HIL.h + AP_Compass_HMC5843.h + AP_Compass.h + ) + + + +include_directories( + + - + ${ARDUINO_LIBRARIES_PATH}/Wire + #${CMAKE_SOURCE_DIR}/libraries/FastSerial + # +) +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index b6dcaaf4b1..4cbcdfeb02 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -10,7 +10,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix @@ -41,13 +41,17 @@ AP_DCM::set_compass(Compass *compass) /**************************************************/ void -AP_DCM::update_DCM_fast(float _G_Dt) +AP_DCM::update_DCM_fast(void) { + float delta_t; + _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors - matrix_update(_G_Dt); // Integrate the DCM matrix + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix switch(_toggle++){ case 0: @@ -82,13 +86,17 @@ AP_DCM::update_DCM_fast(float _G_Dt) /**************************************************/ void -AP_DCM::update_DCM(float _G_Dt) +AP_DCM::update_DCM(void) { + float delta_t; + _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors - matrix_update(_G_Dt); // Integrate the DCM matrix + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 9a10eb03bc..503f2ab1fa 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// teporarily include all other classes here +// temporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file @@ -49,8 +49,8 @@ public: void set_compass(Compass *compass); // Methods - void update_DCM(float _G_Dt); - void update_DCM_fast(float _G_Dt); + void update_DCM(void); + void update_DCM_fast(void); float get_health(void); diff --git a/libraries/AP_DCM/AP_DCM_HIL.cpp b/libraries/AP_DCM/AP_DCM_HIL.cpp index 2597d9eb58..66adbbee7e 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.cpp +++ b/libraries/AP_DCM/AP_DCM_HIL.cpp @@ -8,7 +8,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 447f4476f6..a8266fcb09 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -31,7 +31,8 @@ public: void set_compass(Compass *compass) {} // Methods - void update_DCM(float _G_Dt) {} + void update_DCM(void) {} + void update_DCM_fast(void) {} float get_health(void) { return 1.0; } diff --git a/libraries/AP_DCM/CMakeLists.txt b/libraries/AP_DCM/CMakeLists.txt new file mode 100644 index 0000000000..0131eeb495 --- /dev/null +++ b/libraries/AP_DCM/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_DCM) + +set(${LIB_NAME}_SRCS + AP_DCM.cpp + AP_DCM_HIL.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_DCM.h + AP_DCM_HIL.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_DCM + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_GPS/CMakeLists.txt b/libraries/AP_GPS/CMakeLists.txt new file mode 100644 index 0000000000..57409c6d24 --- /dev/null +++ b/libraries/AP_GPS/CMakeLists.txt @@ -0,0 +1,44 @@ +set(LIB_NAME AP_GPS) + +set(${LIB_NAME}_SRCS + AP_GPS_406.cpp + AP_GPS_Auto.cpp + AP_GPS_HIL.cpp + AP_GPS_IMU.cpp + AP_GPS_MTK.cpp + AP_GPS_MTK16.cpp + AP_GPS_NMEA.cpp + AP_GPS_SIRF.cpp + AP_GPS_UBLOX.cpp + GPS.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_GPS_406.h + AP_GPS_Auto.h + AP_GPS_HIL.h + AP_GPS_IMU.h + AP_GPS_MTK.h + AP_GPS_MTK_Common.h + AP_GPS_MTK16.h + AP_GPS_NMEA.h + AP_GPS_None.h + AP_GPS_Shim.h + AP_GPS_SIRF.h + AP_GPS_UBLOX.h + AP_GPS.h + GPS.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index c4e14de3a1..e638f085f0 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -18,7 +18,7 @@ AP_GPS_Auto GPS(&Serial1, &gps); void setup() { - Serial.begin(38400); + Serial.begin(115200); Serial1.begin(38400); Serial.println("GPS AUTO library test"); diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index 6a8f17c2e0..73cef75e4e 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -77,6 +77,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) float prev[3] = {0,0,0}; float total_change; float max_offset; + uint16_t adc_values[6]; // cold start tc_temp = _adc->Ch(_gyro_temp_ch); @@ -88,8 +89,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) digitalWrite(C_LED_PIN, HIGH); callback(20); - for (int i = 0; i < 6; i++) - adc_in = _adc->Ch(_sensors[i]); + _adc->Ch6(_sensors, adc_values); digitalWrite(A_LED_PIN, HIGH); digitalWrite(C_LED_PIN, LOW); @@ -100,16 +100,23 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time do { + // get 6 sensor values + _adc->Ch6(_sensors, adc_values); + for (int j = 0; j <= 2; j++){ prev[j] = _sensor_cal[j]; - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, tc_temp); _sensor_cal[j] = adc_in; } for(int i = 0; i < 50; i++){ + + // get 6 sensor values + _adc->Ch6(_sensors, adc_values); + for (int j = 0; j < 3; j++){ - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; // Subtract temp compensated typical gyro bias adc_in -= _sensor_compensation(j, tc_temp); // filter @@ -159,6 +166,7 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) float prev[6] = {0,0,0}; float total_change; float max_offset; + uint16_t adc_values[6]; // cold start callback(500); @@ -168,9 +176,11 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time do { + _adc->Ch6(_sensors, adc_values); + for (int j = 3; j <= 5; j++){ prev[j] = _sensor_cal[j]; - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, 0); // temperature ignored _sensor_cal[j] = adc_in; } @@ -179,8 +189,10 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) callback(20); + _adc->Ch6(_sensors, adc_values); + for (int j = 3; j < 6; j++){ - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, 0); // temperature ignored _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; } @@ -222,7 +234,7 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const // do gyro temperature compensation if (channel < 3) { - return 1658; + return 1658.0; /* return _gyro_temp_curve[channel][0] + _gyro_temp_curve[channel][1] * temperature + @@ -231,17 +243,17 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const } // do fixed-offset accelerometer compensation - return 2041; // Average raw value from a 20 board sample + return 2041.0; // Average raw value from a 20 board sample } float -AP_IMU_Oilpan::_sensor_in(uint8_t channel, int temperature) +AP_IMU_Oilpan::_sensor_in(uint8_t channel, uint16_t adc_value, int temperature) { float adc_in; // get the compensated sensor value // - adc_in = _adc->Ch(_sensors[channel]) - _sensor_compensation(channel, temperature); + adc_in = adc_value - _sensor_compensation(channel, temperature); // adjust for sensor sign and apply calibration offset // @@ -265,18 +277,21 @@ bool AP_IMU_Oilpan::update(void) { int tc_temp = _adc->Ch(_gyro_temp_ch); + uint16_t adc_values[6]; + + _sample_time = _adc->Ch6(_sensors, adc_values); // convert corrected gyro readings to delta acceleration // - _gyro.x = ToRad(_gyro_gain_x) * _sensor_in(0, tc_temp); - _gyro.y = ToRad(_gyro_gain_y) * _sensor_in(1, tc_temp); - _gyro.z = ToRad(_gyro_gain_z) * _sensor_in(2, tc_temp); + _gyro.x = _gyro_gain_x * _sensor_in(0, adc_values[0], tc_temp); + _gyro.y = _gyro_gain_y * _sensor_in(1, adc_values[1], tc_temp); + _gyro.z = _gyro_gain_z * _sensor_in(2, adc_values[2], tc_temp); // convert corrected accelerometer readings to acceleration // - _accel.x = _accel_scale * _sensor_in(3, tc_temp); - _accel.y = _accel_scale * _sensor_in(4, tc_temp); - _accel.z = _accel_scale * _sensor_in(5, tc_temp); + _accel.x = _accel_scale * _sensor_in(3, adc_values[3], tc_temp); + _accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp); + _accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp); _accel_filtered.x = _accel_filtered.x * .98 + _accel.x * .02; _accel_filtered.y = _accel_filtered.y * .98 + _accel.y * .02; diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index 90b683e300..5941f0a361 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -68,8 +68,8 @@ private: virtual void _init_accel(void (*callback)(unsigned long t)); ///< no-save implementation virtual void _init_gyro(void (*callback)(unsigned long t)); ///< no-save implementation + float _sensor_in(uint8_t channel, uint16_t adc_value, int temperature); float _sensor_compensation(uint8_t channel, int temp) const; - float _sensor_in(uint8_t channel, int temperature); // constants static const uint8_t _sensors[6]; ///< ADC channel mappings for the sensors @@ -83,14 +83,15 @@ private: // static const float _gravity = 423.8; ///< 1G in the raw data coming from the accelerometer // Value based on actual sample data from 20 boards + static const float _accel_scale = 9.80665 / 423.8; ///< would like to use _gravity here, but cannot // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 // - static const float _gyro_gain_x = 0.4; // X axis Gyro gain - static const float _gyro_gain_y = 0.41; // Y axis Gyro gain - static const float _gyro_gain_z = 0.41; // Z axis Gyro gain + static const float _gyro_gain_x = ToRad(0.4); // X axis Gyro gain + static const float _gyro_gain_y = ToRad(0.41); // Y axis Gyro gain + static const float _gyro_gain_z = ToRad(0.41); // Z axis Gyro gain // Maximum possible value returned by an offset-corrected sensor channel // diff --git a/libraries/AP_IMU/CMakeLists.txt b/libraries/AP_IMU/CMakeLists.txt new file mode 100644 index 0000000000..2447a078c4 --- /dev/null +++ b/libraries/AP_IMU/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_IMU) + +set(${LIB_NAME}_SRCS + AP_IMU_Oilpan.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_IMU.h + AP_IMU_Shim.h + AP_IMU_Oilpan.h + IMU.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 19754fc365..1b881bc355 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -70,12 +70,19 @@ public: /// Vector3f get_accel(void) { return _accel; } + /// Fetch the current accelerometer values /// /// @returns vector of current accelerations in m/s/s /// Vector3f get_accel_filtered(void) { return _accel_filtered; } + /// return the number of seconds that the last update represents + /// + /// @returns number of seconds + /// + float get_delta_time(void) { return _sample_time * 1.0e-6; } + /// A count of bad sensor readings /// /// @todo This should be renamed, as there's no guarantee that sensors @@ -90,6 +97,10 @@ protected: /// Most recent gyro reading obtained by ::update Vector3f _gyro; + + /// number of microseconds that the accel and gyro values + /// were sampled over + uint32_t _sample_time; }; #endif diff --git a/libraries/AP_Math/CMakeLists.txt b/libraries/AP_Math/CMakeLists.txt new file mode 100644 index 0000000000..f4a83b3102 --- /dev/null +++ b/libraries/AP_Math/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_Math) + +set(${LIB_NAME}_SRCS + + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Math.h + matrix3.h + vector2.h + vector3.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 67d2821ac0..da47395701 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -12,15 +12,11 @@ #define FORTYFIVE_DEGREES 0.78539816 -AP_OpticalFlow::AP_OpticalFlow() : raw_dx(0),raw_dy(0),x(0),y(0),surface_quality(0),dx(0),dy(0),last_update(0),field_of_view(1),scaler(0),num_pixels(0) -{ -} - // init - initCommAPI parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) -bool +bool AP_OpticalFlow::init(bool initCommAPI) { - _orientation_matrix = Matrix3f(1,0,0,0,1,0,0,0,1); + _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); update_conversion_factors(); return true; // just return true by default } @@ -35,31 +31,33 @@ AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) // read latest values from sensor and fill in x,y and totals int AP_OpticalFlow::read() { + return 0; } // reads a value from the sensor (will be sensor specific) -byte +byte AP_OpticalFlow::read_register(byte address) { + return 0; } // writes a value to one of the sensor's register (will be sensor specific) -void +void AP_OpticalFlow::write_register(byte address, byte value) { } // rotate raw values to arrive at final x,y,dx and dy values -void +void AP_OpticalFlow::apply_orientation_matrix() { Vector3f rot_vector; - + // next rotate dx and dy - rot_vector = _orientation_matrix * Vector3f(raw_dx,raw_dy,0); + rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); dx = rot_vector.x; dy = rot_vector.y; - + // add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) x += dx; y += dy; @@ -69,45 +67,69 @@ AP_OpticalFlow::apply_orientation_matrix() void AP_OpticalFlow::update_conversion_factors() { - conv_factor = (1.0/(float)(num_pixels*scaler))*2.0*tan(field_of_view/2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) - radians_to_pixels = (num_pixels*scaler) / field_of_view; + conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) + // 0.00615 + radians_to_pixels = (num_pixels * scaler) / field_of_view; + // 162.99 } // updates internal lon and lat with estimation based on optical flow void -AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude) +AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) { - float diff_roll = roll - _last_roll; - float diff_pitch = pitch - _last_pitch; - float avg_altitude = (altitude + _last_altitude)/2; - //float exp_change_x, exp_change_y; - //float change_x, change_y; - //float x_cm, y_cm; - float cos_yaw = cos(yaw); - float sin_yaw = sin(yaw); - int i; - + float diff_roll = roll - _last_roll; + float diff_pitch = pitch - _last_pitch; + // only update position if surface quality is good and angle is not over 45 degrees if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); // calculate expected x,y diff due to roll and pitch change exp_change_x = diff_roll * radians_to_pixels; exp_change_y = -diff_pitch * radians_to_pixels; - + // real estimated raw change from mouse change_x = dx - exp_change_x; change_y = dy - exp_change_y; + float avg_altitude = (altitude + _last_altitude)*0.5; + // convert raw change to horizontal movement in cm x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less - - // use yaw to convert x and y into lon and lat - lat += y_cm * cos_yaw - x_cm * sin_yaw; - lng += x_cm * cos_yaw + y_cm * sin_yaw; + + + vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; + vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; } - - // capture roll and pitch for next iteration - _last_roll = roll; - _last_pitch = pitch; - _last_altitude = altitude; -} \ No newline at end of file + + _last_altitude = altitude; + _last_roll = roll; + _last_pitch = pitch; +} + + +/* +{ + // only update position if surface quality is good and angle is not over 45 degrees + if( surface_quality >= 10 && fabs(_dcm->roll) <= FORTYFIVE_DEGREES && fabs(_dcm->pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); + Vector3f omega = _dcm->get_gyro(); + + // calculate expected x,y diff due to roll and pitch change + float exp_change_x = omega.x * radians_to_pixels; + float exp_change_y = -omega.y * radians_to_pixels; + + // real estimated raw change from mouse + float change_x = dx - exp_change_x; + float change_y = dy - exp_change_y; + + // convert raw change to horizontal movement in cm + float x_cm = -change_x * altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? + float y_cm = -change_y * altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less + + vlon = (float)x_cm * sin_yaw_y - (float)y_cm * cos_yaw_x; + vlat = (float)y_cm * sin_yaw_y - (float)x_cm * cos_yaw_x; + } +} + +*/ diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index dec1d0bc28..7d57977174 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -15,11 +15,11 @@ read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter read_register() : reads a value from the sensor (will be sensor specific) write_register() : writes a value to one of the sensor's register (will be sensor specific) - */ -#include "WProgram.h" #include +#include +#include // return value definition #define OPTICALFLOW_FAIL 0 @@ -37,12 +37,12 @@ class AP_OpticalFlow { - public: + public: int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) int surface_quality; // image quality (below 15 you really can't trust the x,y values returned) int x,y; // total x,y position int dx,dy; // rotated change in x and y position - float lng, lat; // position as offsets from original position + float vlon, vlat; // position as offsets from original position unsigned long last_update; // millis() time of last update float field_of_view; // field of view in Radians float scaler; // number returned from sensor when moved one pixel @@ -50,26 +50,28 @@ class AP_OpticalFlow // temp variables - delete me! float exp_change_x, exp_change_y; float change_x, change_y; - float x_cm, y_cm; - public: - AP_OpticalFlow(); // Constructor + float x_cm, y_cm; + + //AP_OpticalFlow(AP_DCM *dcm); virtual bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) virtual byte read_register(byte address); virtual void write_register(byte address, byte value); virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor virtual int read(); // read latest values from sensor and fill in x,y and totals - virtual void get_position(float roll, float pitch, float yaw, float altitude); // updates internal lon and lat with estimation based on optical flow - + virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow + + + //protected: Matrix3f _orientation_matrix; float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) float radians_to_pixels; - float _last_roll, _last_pitch, _last_yaw, _last_altitude; + float _last_roll, _last_pitch, _last_altitude; virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values virtual void update_conversion_factors(); }; -#include "AP_OpticalFlow_ADNS3080.h" - +#include "AP_OpticalFlow_ADNS3080.h" + #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 15a2c35cb7..b387e9ed91 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -33,18 +33,18 @@ union NumericIntType { int intValue; unsigned int uintValue; - byte byteValue[2]; + byte byteValue[2]; }; - -// Constructors //////////////////////////////////////////////////////////////// + // Constructors //////////////////////////////////////////////////////////////// AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080() { - num_pixels = ADNS3080_PIXELS_X; - field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; + num_pixels = ADNS3080_PIXELS_X; + field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; scaler = AP_OPTICALFLOW_ADNS3080_SCALER; } + // Public Methods ////////////////////////////////////////////////////////////// // init - initialise sensor // initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface) @@ -52,7 +52,7 @@ bool AP_OpticalFlow_ADNS3080::init(bool initCommAPI) { int retry = 0; - + pinMode(AP_SPI_DATAOUT,OUTPUT); pinMode(AP_SPI_DATAIN,INPUT); pinMode(AP_SPI_CLOCK,OUTPUT); @@ -63,20 +63,20 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // reset the device reset(); - + // start the SPI library: if( initCommAPI ) { SPI.begin(); } - + // check the sensor is functioning if( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) return true; - else - retry++; - }else - return false; + retry++; + } + + return false; } // @@ -84,16 +84,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // byte AP_OpticalFlow_ADNS3080::backup_spi_settings() -{ +{ // store current spi values orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); orig_spi_settings_spsr = SPSR & SPI2X; - + // set the values that we need SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE3); - SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed - + SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed + return orig_spi_settings_spcr; } @@ -102,7 +102,7 @@ byte AP_OpticalFlow_ADNS3080::restore_spi_settings() { byte temp; - + // restore SPSR temp = SPSR; temp &= ~SPI2X; @@ -114,35 +114,35 @@ AP_OpticalFlow_ADNS3080::restore_spi_settings() temp &= ~(DORD | CPOL | CPHA); // zero out the important bits temp |= orig_spi_settings_spcr; // restore important bits SPCR = temp; - + return temp; } // Read a register from the sensor byte -AP_OpticalFlow_ADNS3080::read_register(byte address) +AP_OpticalFlow_ADNS3080::read_register(byte address) { byte result = 0, junk = 0; backup_spi_settings(); - + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send the device the register you want to read: junk = SPI.transfer(address); - + // small delay delayMicroseconds(50); - + // send a value of 0 to read the first byte returned: result = SPI.transfer(0x00); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - + restore_spi_settings(); - + return result; } @@ -152,24 +152,24 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value) { byte junk = 0; - backup_spi_settings(); - + backup_spi_settings(); + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send register address junk = SPI.transfer(address | 0x80 ); - + // small delay - delayMicroseconds(50); - + delayMicroseconds(50); + // send data junk = SPI.transfer(value); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - - restore_spi_settings(); + + restore_spi_settings(); } // reset sensor by holding a pin high (or is it low?) for 10us. @@ -187,7 +187,7 @@ AP_OpticalFlow_ADNS3080::read() { surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); delayMicroseconds(50); // small delay - + // check for movement, update x,y values if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) { raw_dx = ((char)read_register(ADNS3080_DELTA_X)); @@ -198,11 +198,11 @@ AP_OpticalFlow_ADNS3080::read() raw_dx = 0; raw_dy = 0; } - + last_update = millis(); - + apply_orientation_matrix(); - + return OPTICALFLOW_SUCCESS; } @@ -218,7 +218,7 @@ void AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn ) { byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); - regVal = regVal & 0xBf | (alwaysOn << 6); + regVal = (regVal & 0xbf) | (alwaysOn << 6); delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); } @@ -238,13 +238,13 @@ void AP_OpticalFlow_ADNS3080::set_resolution(int resolution) { byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); - + if( resolution == ADNS3080_RESOLUTION_400 ) { regVal &= ~0x10; - }else if( resolution == ADNS3080_RESOLUTION_1200) { + }else if( resolution == ADNS3080_RESOLUTION_1200) { regVal |= 0x10; } - + delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); } @@ -254,7 +254,7 @@ bool AP_OpticalFlow_ADNS3080::get_frame_rate_auto() { byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); - if( regVal & 0x01 > 0 ) { + if( (regVal & 0x01) != 0 ) { return false; }else{ return true; @@ -273,7 +273,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) delayMicroseconds(50); // small delay write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); delayMicroseconds(50); // small delay - + // decide what value to update in extended config regVal = (regVal & ~0x01); }else{ @@ -287,7 +287,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) unsigned int AP_OpticalFlow_ADNS3080::get_frame_period() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); @@ -300,11 +300,11 @@ AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period) { NumericIntType aNum; aNum.uintValue = period; - + // set frame rate to manual set_frame_rate_auto(false); delayMicroseconds(50); // small delay - + // set specific frame period write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); delayMicroseconds(50); // small delay @@ -325,7 +325,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate) { unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate); - + set_frame_period(period); } @@ -349,53 +349,53 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) delayMicroseconds(50); // small delay if( auto_shutter_speed ) { // return shutter speed max to default - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); - delayMicroseconds(50); // small delay - + delayMicroseconds(50); // small delay + // determine value to put into extended config - regVal = regVal & ~0x02; + regVal &= ~0x02; }else{ // determine value to put into extended config - regVal = regVal & ~0x02 | 0x02; + regVal |= 0x02; } write_register(ADNS3080_EXTENDED_CONFIG, regVal); - delayMicroseconds(50); // small delay + delayMicroseconds(50); // small delay } // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER); return aNum.uintValue; } - - + + // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) -unsigned int +void AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed) { NumericIntType aNum; aNum.uintValue = shutter_speed; - + // set shutter speed to manual set_shutter_speed_auto(false); delayMicroseconds(50); // small delay - + // set specific shutter speed - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); delayMicroseconds(50); // small delay - + // larger delay delay(50); - + // need to update frame period to cause shutter value to take effect aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay @@ -420,25 +420,25 @@ AP_OpticalFlow_ADNS3080::clear_motion() } // get_pixel_data - captures an image from the sensor and stores it to the pixe_data array -int +void AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) { int i,j; bool isFirstPixel = true; byte regValue; byte pixelValue; - + // write to frame capture register to force capture of frame write_register(ADNS3080_FRAME_CAPTURE,0x83); - + // wait 3 frame periods + 10 nanoseconds for frame to be captured delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510 - + // display the pixel data for( i=0; iprintln("failed to find first pixel"); } isFirstPixel = false; @@ -450,7 +450,7 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) } serPort->println(); } - + // hardware reset to restore sensor to normal operation reset(); } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 41d233ebae..cdb6a2e975 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -1,8 +1,9 @@ #ifndef AP_OPTICALFLOW_ADNS3080_H #define AP_OPTICALFLOW_ADNS3080_H -#include -#include +//#include +//#include +//#include #include "AP_OpticalFlow.h" // orientations for ADNS3080 sensor @@ -29,10 +30,10 @@ #define ADNS3080_CHIP_SELECT 34 // PC3 #define ADNS3080_RESET 40 // PG1 was 35 / PC2 #else // normal arduino SPI pins...these need to be checked - #define AP_SPI_DATAIN 12 //MISO + #define AP_SPI_DATAIN 12 //MISO #define AP_SPI_DATAOUT 11 //MOSI #define AP_SPI_CLOCK 13 //SCK - #define ADNS3080_CHIP_SELECT 10 //SS + #define ADNS3080_CHIP_SELECT 10 //SS #define ADNS3080_RESET 9 //RESET #endif @@ -91,48 +92,49 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow // bytes to store SPI settings byte orig_spi_settings_spcr; byte orig_spi_settings_spsr; - + // save and restore SPI settings byte backup_spi_settings(); byte restore_spi_settings(); - + bool _motion; // true if there has been motion - + public: - AP_OpticalFlow_ADNS3080(); // Constructor + AP_OpticalFlow_ADNS3080(); + //AP_OpticalFlow_ADNS3080(); // Constructor bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) byte read_register(byte address); void write_register(byte address, byte value); void reset(); // reset sensor by holding a pin high (or is it low?) for 10us. int read(); // read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read - + // ADNS3080 specific features bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called - + bool get_led_always_on(); // returns true if LED is always on, false if only on when required void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required - + int get_resolution(); // returns resolution (either 400 or 1200 counts per inch) void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch - + bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) - unsigned int get_frame_period(); // get_frame_period - + unsigned int get_frame_period(); // get_frame_period - void set_frame_period(unsigned int period); unsigned int get_frame_rate(); void set_frame_rate(unsigned int rate); - + bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) unsigned int get_shutter_speed(); - unsigned int set_shutter_speed(unsigned int shutter_speed); + void set_shutter_speed(unsigned int shutter_speed); void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared - - int print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port + + void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port }; #endif diff --git a/libraries/AP_OpticalFlow/CMakeLists.txt b/libraries/AP_OpticalFlow/CMakeLists.txt new file mode 100644 index 0000000000..74d7457ac4 --- /dev/null +++ b/libraries/AP_OpticalFlow/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_OpticalFlow) + +set(${LIB_NAME}_SRCS + AP_OpticalFlow.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_OpticalFlow.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_PID/CMakeLists.txt b/libraries/AP_PID/CMakeLists.txt new file mode 100644 index 0000000000..14e9782e51 --- /dev/null +++ b/libraries/AP_PID/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_PID) + +set(${LIB_NAME}_SRCS + AP_PID.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_PID.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp index b23879e0fd..9c8c8a6ca4 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.cpp +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -79,15 +79,15 @@ AP_RC_Channel::set_pwm(int pwm) //Serial.print("range "); control_in = pwm_to_range(); control_in = (control_in < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; - } }else{ control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; + + if(expo) { + long temp = control_in; + temp = (temp * temp) / (long)_high; + control_in = (int)((control_in >= 0) ? temp : -temp); } } } diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.h b/libraries/AP_RC_Channel/AP_RC_Channel.h index d39806d904..db5dc68cd3 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.h +++ b/libraries/AP_RC_Channel/AP_RC_Channel.h @@ -23,17 +23,13 @@ class AP_RC_Channel{ _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} AP_RC_Channel() : _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} // setup min and max radio values in CLI void update_min_max(); @@ -94,8 +90,6 @@ class AP_RC_Channel{ int16_t pwm_to_range(); int16_t range_to_pwm(); - float scale_output; - private: bool _filter; int8_t _reverse; diff --git a/libraries/AP_RangeFinder/CMakeLists.txt b/libraries/AP_RangeFinder/CMakeLists.txt new file mode 100644 index 0000000000..add2e2928e --- /dev/null +++ b/libraries/AP_RangeFinder/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_RangeFinder) + +set(${LIB_NAME}_SRCS + AP_RangeFinder_MaxsonarXL.cpp + AP_RangeFinder_SharpGP2Y.cpp + RangeFinder.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_RangeFinder.h + AP_RangeFinder_MaxsonarXL.h + AP_RangeFinder_SharpGP2Y.h + RangeFinder.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 4621a90be8..5a53183c17 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -37,8 +37,9 @@ void RangeFinder::set_orientation(int x, int y, int z) int RangeFinder::read() { // read from the analog port or pitot tube - if( _ap_adc != NULL ){ - raw_value = _ap_adc->Ch_raw(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; // values from ADC are twice as big as you'd expect + if( _ap_adc != NULL ){ + // values from ADC are twice as big as you'd expect + raw_value = _ap_adc->Ch(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; }else{ // read raw sensor value and convert to distance raw_value = analogRead(_analogPort); diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt new file mode 100644 index 0000000000..ed27448a23 --- /dev/null +++ b/libraries/CMakeLists.txt @@ -0,0 +1,24 @@ +add_subdirectory(DataFlash) +add_subdirectory(AP_Math) +add_subdirectory(PID) +add_subdirectory(AP_Common) +add_subdirectory(RC_Channel) +add_subdirectory(AP_OpticalFlow) +add_subdirectory(ModeFilter) +add_subdirectory(memcheck) +add_subdirectory(APM_PI) +add_subdirectory(AP_GPS) +add_subdirectory(AP_DCM) +add_subdirectory(AP_Compass) +add_subdirectory(AP_ADC) +add_subdirectory(AP_IMU) +add_subdirectory(AP_RangeFinder) + +add_subdirectory(APM_RC) +add_subdirectory(APM_BMP085) + +#add_subdirectory(APO) +add_subdirectory(FastSerial) +add_subdirectory(GCS_MAVLink) + +#add_subdirectory(playgroundlib) diff --git a/libraries/DataFlash/CMakeLists.txt b/libraries/DataFlash/CMakeLists.txt new file mode 100644 index 0000000000..2e13294932 --- /dev/null +++ b/libraries/DataFlash/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME DataFlash) + +set(${LIB_NAME}_SRCS + DataFlash.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + DataFlash.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt new file mode 100644 index 0000000000..78e94e9023 --- /dev/null +++ b/libraries/FastSerial/CMakeLists.txt @@ -0,0 +1,28 @@ +set(LIB_NAME FastSerial) + +set(${LIB_NAME}_SRCS + BetterStream.cpp + FastSerial.cpp + vprintf.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + BetterStream.h + FastSerial.h + ftoa_engine.h + ntz.h + xtoa_fast.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/GCS_MAVLink/CMakeLists.txt b/libraries/GCS_MAVLink/CMakeLists.txt new file mode 100644 index 0000000000..326d2b959c --- /dev/null +++ b/libraries/GCS_MAVLink/CMakeLists.txt @@ -0,0 +1,19 @@ +set(LIB_NAME GCS_MAVLink) + +set(${LIB_NAME}_SRCS + GCS_MAVLink.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + GCS_MAVLink.h +) + +include_directories( + #${CMAKE_SOURCE_DIR}/libraries/ + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 2cecade369..0ecc270f5a 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -10,3 +10,5 @@ BetterStream *mavlink_comm_1_port; // this might need to move to the flight software mavlink_system_t mavlink_system = {7,1,0,0}; + +#include "include/mavlink_helpers.h" diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index d89f3ee34d..5de542bc4a 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -8,6 +8,8 @@ #include +#define MAVLINK_SEPARATE_HELPERS + #include "include/ardupilotmega/version.h" // this allows us to make mavlink_message_t much smaller diff --git a/libraries/ModeFilter/CMakeLists.txt b/libraries/ModeFilter/CMakeLists.txt new file mode 100644 index 0000000000..cba4eec1b8 --- /dev/null +++ b/libraries/ModeFilter/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME ModeFilter) + +set(${LIB_NAME}_SRCS + ModeFilter.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + ModeFilter.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/PID/CMakeLists.txt b/libraries/PID/CMakeLists.txt new file mode 100644 index 0000000000..c814e841d1 --- /dev/null +++ b/libraries/PID/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME PID) + +set(${LIB_NAME}_SRCS + PID.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + PID.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/RC_Channel/CMakeLists.txt b/libraries/RC_Channel/CMakeLists.txt new file mode 100644 index 0000000000..94ed383fcb --- /dev/null +++ b/libraries/RC_Channel/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME RC_Channel) + +set(${LIB_NAME}_SRCS + RC_Channel.cpp + RC_Channel_aux.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + RC_Channel.h + RC_Channel_aux.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/APM_RC + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 24fae0f5ea..0d41ee6ad8 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -98,6 +98,12 @@ RC_Channel::set_pwm(int pwm) //if (fabs(scale_output) > 0){ // control_in *= scale_output; //} + /* + if(expo) { + long temp = control_in; + temp = (temp * temp) / (long)_high; + control_in = (int)((control_in >= 0) ? temp : -temp); + }*/ } } @@ -120,7 +126,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = pwm_out + radio_min; + radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index aec2ebc998..922b15540e 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,12 +7,11 @@ #define RC_Channel_h #include -#include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - private: + protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -23,9 +22,9 @@ class RC_Channel{ /// RC_Channel(AP_Var::Key key, const prog_char_t *name) : _group(key, name), - radio_min (&_group, 0, 1500, name ? PSTR("MIN") : 0), // suppress name if group has no name + radio_min (&_group, 0, 1100, name ? PSTR("MIN") : 0), // suppress name if group has no name radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0), - radio_max (&_group, 2, 1500, name ? PSTR("MAX") : 0), + radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0), _high(1), _filter(true), _reverse (&_group, 3, 1, name ? PSTR("REV") : 0), @@ -103,8 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino +#include "RC_Channel_aux.h" + #endif - - - - diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp new file mode 100644 index 0000000000..3a2f1e6a7d --- /dev/null +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -0,0 +1,56 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include "RC_Channel_aux.h" + +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function + +// map a function to a servo channel and output it +void +RC_Channel_aux::output_ch(unsigned char ch_nr) +{ + // take care or two corner cases + switch(function) + { + case k_none: // disabled + return; + break; + case k_manual: // manual + radio_out = radio_in; + break; + } + + APM_RC.OutputCh(ch_nr, radio_out); +} + +// Update the g_rc_function array of pointers to rc_x channels +// This is to be done before rc_init so that the channels get correctly initialized. +// It also should be called periodically because the user might change the configuration and +// expects the changes to take effect instantly +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) +{ + // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function + RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos + aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get(); + aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get(); + aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get(); + aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get(); + + // Assume that no auxiliary function is used + for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) + { + g_rc_function[i] = NULL; + } + + // assign the RC channel to each function + g_rc_function[aux_servo_function[CH_5]] = rc_5; + g_rc_function[aux_servo_function[CH_6]] = rc_6; + g_rc_function[aux_servo_function[CH_7]] = rc_7; + g_rc_function[aux_servo_function[CH_8]] = rc_8; + + //set auxiliary ranges + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(4500); + G_RC_AUX(k_flaperon)->set_range(0,100); +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h new file mode 100644 index 0000000000..7e091bf18f --- /dev/null +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -0,0 +1,48 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file RC_Channel_aux.h +/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants. +/// @author Amilcar Lucas + +#ifndef RC_CHANNEL_AUX_H_ +#define RC_CHANNEL_AUX_H_ + +#include "RC_Channel.h" + +// Macro to simplify accessing the auxiliary servos +#define G_RC_AUX(_t) if (g_rc_function[RC_Channel_aux::_t]) g_rc_function[RC_Channel_aux::_t] + +/// @class RC_Channel_aux +/// @brief Object managing one aux. RC channel (CH5-8), with information about its function +class RC_Channel_aux : public RC_Channel{ +public: + /// Constructor + /// + /// @param key EEPROM storage key for the channel trim parameters. + /// @param name Optional name for the group. + /// + RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : + RC_Channel(key, name), + function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0) // suppress name if group has no name + {} + + typedef enum + { + k_none = 0, // disabled + k_manual = 1, // manual, just pass-thru the RC in signal + k_flap = 2, // flap + k_flap_auto = 3, // flap automated + k_aileron = 4, // aileron + k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) + } Aux_servo_function_t; + + AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop + + void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it + +}; + +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8); + +#endif /* RC_CHANNEL_AUX_H_ */ diff --git a/libraries/memcheck/CMakeLists.txt b/libraries/memcheck/CMakeLists.txt new file mode 100644 index 0000000000..ec9121b1ce --- /dev/null +++ b/libraries/memcheck/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME memcheck) + +set(${LIB_NAME}_SRCS + memcheck.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + memcheck.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME})