diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 602d5fffac..8a949023d6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.5.1" +#define THISFIRMWARE "ArduCopter V2.5.3" /* ArduCopter Version 2.5 Lead author: Jason Short @@ -95,10 +95,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include "Parameters.h" #include "GCS.h" -#if AUTOMATIC_DECLINATION == ENABLED -// this is in an #if to avoid the static data #include // ArduPilot Mega Declination Helper Library -#endif //////////////////////////////////////////////////////////////////////////////// // Serial ports @@ -982,7 +979,7 @@ static void medium_loop() // Calculate heading Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); - compass.null_offsets(m); + compass.null_offsets(); } } #endif @@ -1381,12 +1378,10 @@ static void update_GPS(void) ground_start_count = 5; }else{ -#if AUTOMATIC_DECLINATION == ENABLED - if(g.compass_enabled) { + if (g.compass_enabled) { // Set compass declination automatically - compass.set_initial_location(g_gps->latitude, g_gps->longitude, false); + compass.set_initial_location(g_gps->latitude, g_gps->longitude); } -#endif // save home to eeprom (we must have a good fix to have reached this point) init_home(); ground_start_count = 0; @@ -1580,7 +1575,7 @@ void update_throttle_mode(void) int16_t throttle_out; #if AUTO_THROTTLE_HOLD != 0 - static float throttle_avg = THROTTLE_CRUISE; + static float throttle_avg = 0; // this is initialised to g.throttle_cruise later #endif switch(throttle_mode){ @@ -1598,6 +1593,10 @@ void update_throttle_mode(void) #endif #if AUTO_THROTTLE_HOLD != 0 + // ensure throttle_avg has been initialised + if( throttle_avg == 0 ) { + throttle_avg = g.throttle_cruise; + } // calc average throttle if ((g.rc_3.control_in > MINIMUM_THROTTLE) && abs(climb_rate) < 60){ throttle_avg = throttle_avg * .98 + (float)g.rc_3.control_in * .02; @@ -2025,7 +2024,6 @@ static void tuning(){ break; case CH6_RATE_KD: - tuning_value = (float)g.rc_6.control_in / 100000.0; g.pid_rate_roll.kD(tuning_value); g.pid_rate_pitch.kD(tuning_value); break; @@ -2040,6 +2038,14 @@ static void tuning(){ g.pi_stabilize_pitch.kI(tuning_value); break; + case CH6_STABILIZE_KD: + g.stabilize_d = tuning_value; + break; + + case CH6_ACRO_KP: + g.acro_p = tuning_value; + break; + case CH6_RATE_KP: g.pid_rate_roll.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value); @@ -2054,10 +2060,18 @@ static void tuning(){ g.pi_stabilize_yaw.kP(tuning_value); break; + case CH6_YAW_KI: + g.pi_stabilize_yaw.kI(tuning_value); + break; + case CH6_YAW_RATE_KP: g.pid_rate_yaw.kP(tuning_value); break; + case CH6_YAW_RATE_KD: + g.pid_rate_yaw.kD(tuning_value); + break; + case CH6_THROTTLE_KP: g.pid_throttle.kP(tuning_value); break; @@ -2075,22 +2089,32 @@ static void tuning(){ g.waypoint_speed_max = g.rc_6.control_in; break; - case CH6_LOITER_P: + case CH6_LOITER_KP: g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break; - case CH6_NAV_P: + case CH6_LOITER_KI: + g.pi_loiter_lat.kI(tuning_value); + g.pi_loiter_lon.kI(tuning_value); + break; + + case CH6_NAV_KP: g.pid_nav_lat.kP(tuning_value); g.pid_nav_lon.kP(tuning_value); break; - case CH6_LOITER_RATE_P: + case CH6_LOITER_RATE_KP: g.pid_loiter_rate_lon.kP(tuning_value); g.pid_loiter_rate_lat.kP(tuning_value); break; - case CH6_LOITER_RATE_D: + case CH6_LOITER_RATE_KI: + g.pid_loiter_rate_lon.kI(tuning_value); + g.pid_loiter_rate_lat.kI(tuning_value); + break; + + case CH6_LOITER_RATE_KD: g.pid_loiter_rate_lon.kD(tuning_value); g.pid_loiter_rate_lat.kD(tuning_value); break; @@ -2102,7 +2126,7 @@ static void tuning(){ #if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO: - g.heli_ext_gyro_gain = tuning_value * 1000; + g.heli_ext_gyro_gain = tuning_value; break; #endif diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ac7f272dda..7ab489e91e 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -59,30 +59,49 @@ get_stabilize_pitch(int32_t target_angle) static int16_t get_stabilize_yaw(int32_t target_angle) { - // angle error - target_angle = wrap_180(target_angle - ahrs.yaw_sensor); + static int8_t log_counter = 0; + int32_t target_rate,i_term; + int32_t angle_error; + int32_t output; + + // angle error + angle_error = wrap_180(target_angle - ahrs.yaw_sensor); -#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters // limit the error we're feeding to the PID - target_angle = constrain(target_angle, -4500, 4500); +#if FRAME_CONFIG == HELI_FRAME + angle_error = constrain(angle_error, -4500, 4500); #else - // limit the error we're feeding to the PID - target_angle = constrain(target_angle, -2000, 2000); + angle_error = constrain(angle_error, -2000, 2000); #endif - // conver to desired Rate: - int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle); - int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt); + // convert angle error to desired Rate: + target_rate = g.pi_stabilize_yaw.get_p(angle_error); + i_term = g.pi_stabilize_yaw.get_i(angle_error, G_Dt); -#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters + // do not use rate controllers for helicotpers with external gyros +#if FRAME_CONFIG == HELI_FRAME if(!g.heli_ext_gyro_enabled){ - return get_rate_yaw(target_rate) + iterm; + output = get_rate_yaw(target_rate) + i_term; }else{ - return constrain((target_rate + iterm), -4500, 4500); + output = constrain((target_rate + i_term), -4500, 4500); } #else - return get_rate_yaw(target_rate) + iterm; + output = get_rate_yaw(target_rate) + i_term; #endif + +#if LOGGING_ENABLED == ENABLED + // log output if PID logging is on and we are tuning the yaw + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) { + log_counter++; + if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 + log_counter = 0; + Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, g.pi_stabilize_yaw.kP()); + } + } +#endif + + // ensure output does not go beyond barries of what an int16_t can hold + return constrain(output,-32000,32000); } static int16_t @@ -172,15 +191,40 @@ get_rate_pitch(int32_t target_rate) static int16_t get_rate_yaw(int32_t target_rate) { + static int8_t log_counter = 0; + int32_t p,i,d; + int32_t rate_error; + int32_t output; + // rate control - target_rate = target_rate - (omega.z * DEGX100); - target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt); + rate_error = target_rate - (omega.z * DEGX100); + + // separately calculate p, i, d values for logging + p = g.pid_rate_yaw.get_p(rate_error); + i = g.pid_rate_yaw.get_i(rate_error, G_Dt); + d = g.pid_rate_yaw.get_d(rate_error, G_Dt); + + output = p+i+d; // output control: int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); - // smoother Yaw control: - return constrain(target_rate, -yaw_limit, yaw_limit); + // constrain output + output = constrain(output, -yaw_limit, yaw_limit); + +#if LOGGING_ENABLED == ENABLED + // log output if PID loggins is on and we are tuning the yaw + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) { + log_counter++; + if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 + log_counter = 0; + Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, g.pid_rate_yaw.kP()); + } + } +#endif + + // constrain output + return output; } static int16_t diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 7047beaf09..6a1fc4182c 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -43,6 +43,7 @@ public: void init(FastSerial *port) { _port = port; initialised = true; + last_gps_satellites = 255; } /// Update GCS state. @@ -89,6 +90,9 @@ public: // set to true if this GCS link is active bool initialised; + // used to prevent wasting bandwidth with GPS_STATUS messages + uint8_t last_gps_satellites; + protected: /// The stream we are communicating over FastSerial *_port; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d97f89adc1..bdd0f217cd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -697,11 +697,18 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { send_message(MSG_EXTENDED_STATUS1); send_message(MSG_EXTENDED_STATUS2); - send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); - //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); + + if (last_gps_satellites != g_gps->num_sats) { + // this message is mostly a huge waste of bandwidth, + // except it is the only message that gives the number + // of visible satellites. So only send it when that + // changes. + send_message(MSG_GPS_STATUS); + last_gps_satellites = g_gps->num_sats; + } } if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { @@ -1471,7 +1478,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif if (packet.param_value < 0) rounding_addition = -rounding_addition; float v = packet.param_value+rounding_addition; - v = constrain(v, -2147483648, 2147483647); + v = constrain(v, -2147483648.0, 2147483647.0); ((AP_Int32 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT16) { #if LOGGING_ENABLED == ENABLED diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 4727884dc8..a4a3e4a157 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -72,6 +72,7 @@ print_log_menu(void) if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT")); if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS")); if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); + if (g.log_bitmask & MASK_LOG_PID) Serial.printf_P(PSTR(" PID")); } Serial.println(); @@ -193,6 +194,7 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(CUR); TARG(MOTORS); TARG(OPTFLOW); + TARG(PID); #undef TARG } @@ -783,6 +785,46 @@ static void Log_Read_Data() Serial.printf_P(PSTR("DATA: %d, %ld\n"), temp1, temp2); } +// Write an PID packet. Total length : 28 bytes +static void Log_Write_PID(int8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_PID_MSG); + + DataFlash.WriteByte(pid_id); // 1 + DataFlash.WriteLong(error); // 2 + DataFlash.WriteLong(p); // 3 + DataFlash.WriteLong(i); // 4 + DataFlash.WriteLong(d); // 5 + DataFlash.WriteLong(output); // 6 + DataFlash.WriteLong(gain * 1000); // 7 + + DataFlash.WriteByte(END_BYTE); +} + +// Read a PID packet +static void Log_Read_PID() +{ + int8_t temp1 = DataFlash.ReadByte(); // pid id + int32_t temp2 = DataFlash.ReadLong(); // error + int32_t temp3 = DataFlash.ReadLong(); // p + int32_t temp4 = DataFlash.ReadLong(); // i + int32_t temp5 = DataFlash.ReadLong(); // d + int32_t temp6 = DataFlash.ReadLong(); // output + float temp7 = DataFlash.ReadLong() / 1000.f; // gain + + // 1 2 3 4 5 6 7 + Serial.printf_P(PSTR("PID-%d, %ld, %ld, %ld, %ld, %ld, %4.4f\n"), + (int)temp1, // pid id + (long)temp2, // error + (long)temp3, // p + (long)temp4, // i + (long)temp5, // d + (long)temp6, // output + temp7); // gain +} + // Read the DataFlash log memory static void Log_Read(int start_page, int end_page) { @@ -791,6 +833,7 @@ static void Log_Read(int start_page, int end_page) #ifdef AIRFRAME_NAME Serial.printf_P(PSTR((AIRFRAME_NAME) #endif + Serial.printf_P(PSTR("\n" THISFIRMWARE "\nFree RAM: %u\n"), memcheck_available_memory()); @@ -890,6 +933,10 @@ static int Log_Read_Process(int start_page, int end_page) case LOG_DATA_MSG: Log_Read_Data(); break; + + case LOG_PID_MSG: + Log_Read_PID(); + break; } break; case 3: @@ -925,6 +972,7 @@ static void Log_Write_Nav_Tuning() {} static void Log_Write_Control_Tuning() {} static void Log_Write_Motors() {} static void Log_Write_Performance() {} +static void Log_Write_PID() {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } #endif // LOGGING_DISABLED diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 8c489897f9..6670088ba3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -425,11 +425,6 @@ # define GROUND_START_DELAY 3 #endif -#ifndef AUTOMATIC_DECLINATION - #define AUTOMATIC_DECLINATION DISABLED -#endif - - ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // FLIGHT AND NAVIGATION CONTROL @@ -722,6 +717,10 @@ ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // +#ifndef AUTO_THROTTLE_HOLD +# define AUTO_THROTTLE_HOLD 1 +#endif + #ifndef THROTTLE_CRUISE # define THROTTLE_CRUISE 450 // #endif @@ -826,27 +825,31 @@ #ifndef LOG_MOTORS # define LOG_MOTORS DISABLED #endif -// guess! +// optical flow #ifndef LOG_OPTFLOW # define LOG_OPTFLOW DISABLED #endif +#ifndef LOG_PID +# define LOG_PID DISABLED +#endif // calculate the default log_bitmask #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) #define DEFAULT_LOG_BITMASK \ - LOGBIT(ATTITUDE_FAST) | \ - LOGBIT(ATTITUDE_MED) | \ - LOGBIT(GPS) | \ - LOGBIT(PM) | \ - LOGBIT(CTUN) | \ - LOGBIT(NTUN) | \ - LOGBIT(MODE) | \ - LOGBIT(RAW) | \ - LOGBIT(CMD) | \ - LOGBIT(CUR) | \ - LOGBIT(MOTORS) | \ - LOGBIT(OPTFLOW) + LOGBIT(ATTITUDE_FAST) | \ + LOGBIT(ATTITUDE_MED) | \ + LOGBIT(GPS) | \ + LOGBIT(PM) | \ + LOGBIT(CTUN) | \ + LOGBIT(NTUN) | \ + LOGBIT(MODE) | \ + LOGBIT(RAW) | \ + LOGBIT(CMD) | \ + LOGBIT(CUR) | \ + LOGBIT(MOTORS) | \ + LOGBIT(OPTFLOW) | \ + LOGBIT(PID) // if we are using fast, Disable Medium //#if LOG_ATTITUDE_FAST == ENABLED diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index ebca424d6c..27c4051427 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -72,11 +72,6 @@ static void read_trim_switch() } } - #elif CH7_OPTION == CH7_AUTO_TRIM - if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ - auto_level_counter = 10; - } - #else // this is the normal operation set by the mission planner @@ -152,6 +147,10 @@ static void read_trim_switch() // 3 = command total } } + }else if (g.ch7_option == CH7_AUTO_TRIM){ + if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ + auto_level_counter = 10; + } } #endif @@ -212,7 +211,7 @@ static void trim_accel() trim_roll = constrain(trim_roll, -1.5, 1.5); trim_pitch = constrain(trim_pitch, -1.5, 1.5); - if(g.rc_1.control_in > 200){ // Roll RIght + if(g.rc_1.control_in > 200){ // Roll Right imu.ay(imu.ay() - trim_roll); }else if (g.rc_1.control_in < -200){ imu.ay(imu.ay() - trim_roll); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2ddca06d3f..9f446232d3 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -141,21 +141,28 @@ // Attitude #define CH6_STABILIZE_KP 1 #define CH6_STABILIZE_KI 2 -#define CH6_YAW_KP 3 +#define CH6_STABILIZE_KD 29 +#define CH6_YAW_KP 3 +#define CH6_YAW_KI 24 // Rate +#define CH6_ACRO_KP 25 #define CH6_RATE_KP 4 #define CH6_RATE_KI 5 #define CH6_RATE_KD 21 -#define CH6_YAW_RATE_KP 6 +#define CH6_YAW_RATE_KP 6 +#define CH6_YAW_RATE_KD 26 // Altitude rate controller #define CH6_THROTTLE_KP 7 // Extras #define CH6_TOP_BOTTOM_RATIO 8 #define CH6_RELAY 9 -#define CH6_TRAVERSE_SPEED 10 +// Navigation +#define CH6_TRAVERSE_SPEED 10 // maximum speed to next way point +#define CH6_NAV_KP 11 +#define CH6_LOITER_KP 12 +#define CH6_LOITER_KI 27 -#define CH6_NAV_P 11 -#define CH6_LOITER_P 12 +// Trad Heli specific #define CH6_HELI_EXTERNAL_GYRO 13 // altitude controller @@ -169,8 +176,9 @@ #define CH6_OPTFLOW_KD 19 #define CH6_NAV_I 20 -#define CH6_LOITER_RATE_P 22 -#define CH6_LOITER_RATE_D 23 +#define CH6_LOITER_RATE_KP 22 +#define CH6_LOITER_RATE_KI 28 +#define CH6_LOITER_RATE_KD 23 // nav byte mask @@ -265,6 +273,7 @@ enum gcs_severity { #define LOG_MOTORS_MSG 0x0B #define LOG_OPTFLOW_MSG 0x0C #define LOG_DATA_MSG 0x0D +#define LOG_PID_MSG 0x0E #define LOG_INDEX_MSG 0xF0 #define MAX_NUM_LOGS 50 @@ -280,6 +289,7 @@ enum gcs_severity { #define MASK_LOG_CUR (1<<9) #define MASK_LOG_MOTORS (1<<10) #define MASK_LOG_OPTFLOW (1<<11) +#define MASK_LOG_PID (1<<12) // Waypoint Modes // ---------------- diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 0744b52855..0671fd2090 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -75,6 +75,14 @@ static void init_arm_motors() gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); #endif + // we don't want writes to the serial port to cause us to pause + // mid-flight, so set the serial ports non-blocking once we arm + // the motors + Serial.set_blocking_writes(false); + if (gcs3.initialised) { + Serial3.set_blocking_writes(false); + } + motor_armed = true; #if PIEZO_ARMING == 1 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 1debcdef96..273384606d 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -76,25 +76,12 @@ static void init_ardupilot() // Console serial port // // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); + // the MAVLink protocol efficiently + // + Serial.begin(SERIAL0_BAUD, 128, 256); // GPS serial port. // - // Not used if the IMU/X-Plane GPS is in use. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // #if GPS_PROTOCOL != GPS_PROTOCOL_IMU Serial1.begin(38400, 128, 16); #endif @@ -169,11 +156,11 @@ static void init_ardupilot() if (!usb_connected) { // we are not connected via USB, re-init UART0 with right // baud rate - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } #else // we have a 2nd serial port for telemetry - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); gcs3.init(&Serial3); #endif @@ -620,9 +607,9 @@ static void check_usb_mux(void) // the user has switched to/from the telemetry port usb_connected = usb_check; if (usb_connected) { - Serial.begin(SERIAL0_BAUD, 128, 128); + Serial.begin(SERIAL0_BAUD); } else { - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } } #endif diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 2ea0a9318c..714d102839 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -332,7 +332,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) Matrix3f m = dcm.get_dcm_matrix(); compass.read(); // Read magnetometer compass.calculate(m); - compass.null_offsets(m); + compass.null_offsets(); medium_loopCounter = 0; } } @@ -551,7 +551,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) compass.read(); // Read magnetometer Matrix3f m = dcm.get_dcm_matrix(); compass.calculate(m); - compass.null_offsets(m); + compass.null_offsets(); } } fast_loopTimer = millis(); diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 676a8720bc..8235efef78 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPlane V2.30" +#define THISFIRMWARE "ArduPlane V2.32" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier @@ -59,10 +59,7 @@ version 2.1 of the License, or (at your option) any later version. #include "Parameters.h" #include "GCS.h" -#if AUTOMATIC_DECLINATION == ENABLED -// this is in an #if to avoid the static data #include // ArduPilot Mega Declination Helper Library -#endif //////////////////////////////////////////////////////////////////////////////// // Serial ports @@ -784,7 +781,7 @@ static void medium_loop() // Calculate heading Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); - compass.null_offsets(m); + compass.null_offsets(); } else { ahrs.set_compass(NULL); } @@ -975,12 +972,10 @@ static void update_GPS(void) init_home(); } -#if AUTOMATIC_DECLINATION == ENABLED if (g.compass_enabled) { // Set compass declination automatically - compass.set_initial_location(g_gps->latitude, g_gps->longitude, false); + compass.set_initial_location(g_gps->latitude, g_gps->longitude); } -#endif ground_start_count = 0; } } diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 7047beaf09..6a1fc4182c 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -43,6 +43,7 @@ public: void init(FastSerial *port) { _port = port; initialised = true; + last_gps_satellites = 255; } /// Update GCS state. @@ -89,6 +90,9 @@ public: // set to true if this GCS link is active bool initialised; + // used to prevent wasting bandwidth with GPS_STATUS messages + uint8_t last_gps_satellites; + protected: /// The stream we are communicating over FastSerial *_port; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index dd80bae476..3d7e2d35b3 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -900,11 +900,19 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { send_message(MSG_EXTENDED_STATUS1); send_message(MSG_EXTENDED_STATUS2); - send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); send_message(MSG_FENCE_STATUS); + + if (last_gps_satellites != g_gps->num_sats) { + // this message is mostly a huge waste of bandwidth, + // except it is the only message that gives the number + // of visible satellites. So only send it when that + // changes. + send_message(MSG_GPS_STATUS); + last_gps_satellites = g_gps->num_sats; + } } if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { @@ -1083,7 +1091,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1 || packet.param2 == 1 || packet.param3 == 1) { - startup_IMU_ground(); + startup_IMU_ground(true); } if (packet.param4 == 1) { trim_radio(); @@ -1188,7 +1196,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_IMU_ground(); + startup_IMU_ground(true); result=1; break; @@ -1792,7 +1800,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (var_type == AP_PARAM_INT32) { if (packet.param_value < 0) rounding_addition = -rounding_addition; float v = packet.param_value+rounding_addition; - v = constrain(v, -2147483648, 2147483647); + v = constrain(v, -2147483648.0, 2147483647.0); ((AP_Int32 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT16) { if (packet.param_value < 0) rounding_addition = -rounding_addition; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f508e6c38d..5519f0cd22 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -49,6 +49,7 @@ public: k_param_num_resets, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_reset_switch_chan, + k_param_manual_level, // 110: Telemetry control @@ -300,6 +301,7 @@ public: AP_Int16 log_bitmask; AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change AP_Int8 reset_switch_chan; + AP_Int8 manual_level; AP_Int16 airspeed_cruise; AP_Int16 min_gndspeed; AP_Int16 pitch_trim; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 4b0bda9ae0..3619095c05 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -23,6 +23,7 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"), GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"), GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"), + GSCALAR(manual_level, "MANUAL_LEVEL"), GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"), GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"), diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 90515198a2..07b777f4b4 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -418,10 +418,6 @@ # define GROUND_START_DELAY 0 #endif -#ifndef AUTOMATIC_DECLINATION - #define AUTOMATIC_DECLINATION DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // ENABLE_AIR_START // diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 0dc50c54d9..3a856b827c 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -79,23 +79,12 @@ static void init_ardupilot() // Console serial port // // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. + // the MAVLink protocol efficiently // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); + Serial.begin(SERIAL0_BAUD, 128, 256); // GPS serial port. // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // // standard gps running Serial1.begin(38400, 128, 16); @@ -145,11 +134,11 @@ static void init_ardupilot() if (!usb_connected) { // we are not connected via USB, re-init UART0 with right // baud rate - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } #else // we have a 2nd serial port for telemetry - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256); gcs3.init(&Serial3); #endif @@ -318,7 +307,7 @@ static void startup_ground(void) //IMU ground start //------------------------ // - startup_IMU_ground(); + startup_IMU_ground(false); // read the radio to set trims // --------------------------- @@ -351,6 +340,14 @@ static void startup_ground(void) // ----------------------- demo_servos(3); + // we don't want writes to the serial port to cause us to pause + // mid-flight, so set the serial ports non-blocking once we are + // ready to fly + Serial.set_blocking_writes(false); + if (gcs3.initialised) { + Serial3.set_blocking_writes(false); + } + gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } @@ -441,7 +438,7 @@ static void check_short_failsafe() } -static void startup_IMU_ground(void) +static void startup_IMU_ground(bool force_accel_level) { #if HIL_MODE != HIL_MODE_ATTITUDE gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); @@ -454,7 +451,12 @@ static void startup_IMU_ground(void) mavlink_delay(1000); imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); - imu.init_accel(mavlink_delay, flash_leds); + if (force_accel_level || g.manual_level == 0) { + // when MANUAL_LEVEL is set to 1 we don't do accelerometer + // levelling on each boot, and instead rely on the user to do + // it once via the ground station + imu.init_accel(mavlink_delay, flash_leds); + } ahrs.set_centripetal(1); ahrs.reset(); @@ -551,9 +553,9 @@ static void check_usb_mux(void) // the user has switched to/from the telemetry port usb_connected = usb_check; if (usb_connected) { - Serial.begin(SERIAL0_BAUD, 128, 128); + Serial.begin(SERIAL0_BAUD); } else { - Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } } #endif diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index ec988bbdd3..b0e1c27462 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -603,7 +603,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) // Calculate heading Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); - compass.null_offsets(m); + compass.null_offsets(); } medium_loopCounter = 0; } diff --git a/Tools/ArdupilotMegaPlanner/Antenna/ArduTracker.cs b/Tools/ArdupilotMegaPlanner/Antenna/ArduTracker.cs new file mode 100644 index 0000000000..8665324335 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Antenna/ArduTracker.cs @@ -0,0 +1,155 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace ArdupilotMega.Antenna +{ + class ArduTracker : ITrackerOutput + { + public SerialPort ComPort { get; set; } + /// + /// 0-360 + /// + public double TrimPan { get; set; } + /// + /// -90 - 90 + /// + public double TrimTilt { get; set; } + + public int PanStartRange { get; set; } + public int TiltStartRange { get; set; } + public int PanEndRange { get; set; } + public int TiltEndRange { get; set; } + public int PanPWMRange { get; set; } + public int TiltPWMRange { get; set; } + + public bool PanReverse { get { return _panreverse == 1; } set { _panreverse = value == true ? -1 : 1; } } + public bool TiltReverse { get { return _tiltreverse == 1; } set { _tiltreverse = value == true ? -1 : 1; } } + + int _panreverse = 1; + int _tiltreverse = 1; + + int currentpan = 1500; + int currenttilt = 1500; + + public bool Init() + { + + if ((PanStartRange - PanEndRange) == 0) + { + System.Windows.Forms.CustomMessageBox.Show("Invalid Pan Range", "Error"); + return false; + } + + if ((TiltStartRange - TiltEndRange) == 0) + { + System.Windows.Forms.CustomMessageBox.Show("Invalid Tilt Range", "Error"); + return false; + } + + try + { + ComPort.Open(); + } + catch (Exception ex) { System.Windows.Forms.CustomMessageBox.Show("Connect failed " + ex.Message, "Error"); return false; } + + return true; + } + public bool Setup() + { + + + return true; + } + + double wrap_180(double input) + { + if (input > 180) + return input - 360; + if (input < -180) + return input + 360; + return input; + } + + double wrap_range(double input, double range) + { + if (input > range) + return input - 360; + if (input < -range) + return input + 360; + return input; + } + + public bool Pan(double Angle) + { + double range = Math.Abs(PanStartRange - PanEndRange); + + // get relative center based on tracking center + double rangeleft = PanStartRange - TrimPan; + double rangeright = PanEndRange - TrimPan; + double centerpos = 1500; + + // get the output angle the tracker needs to point and constrain the output to the allowed options + short PointAtAngle = Constrain(wrap_180(Angle - TrimPan), PanStartRange, PanEndRange); + + // conver the angle into a 0-pwmrange value + int target = (int)((((PointAtAngle / range) * 2.0) * (PanPWMRange / 2) * _panreverse + centerpos)); + + // Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); + + currentpan = target; + + return false; + } + + public bool Tilt(double Angle) + { + double range = Math.Abs(TiltStartRange - TiltEndRange); + + short PointAtAngle = Constrain((Angle - TrimTilt), TiltStartRange, TiltEndRange); + + int target = (int)((((PointAtAngle / range) * 2.0) * (TiltPWMRange / 2) * _tiltreverse + 1500)); + + // Console.WriteLine("T " + Angle + " " + target + " " + PointAtAngle); + + currenttilt = target; + + return false; + } + + public bool PanAndTilt(double pan, double tilt) + { + Tilt(tilt); + Pan(pan); + + string command = string.Format("!!!PAN:{0:0000},TLT:{1:0000}\n", currentpan, currenttilt); + + Console.Write(command); + + ComPort.Write(command); + + return false; + } + + public bool Close() + { + try + { + ComPort.Close(); + } + catch { } + return true; + } + + short Constrain(double input, double min, double max) + { + if (input < min) + return (short)min; + if (input > max) + return (short)max; + return (short)input; + } + + } +} diff --git a/Tools/ArdupilotMegaPlanner/Antenna/ITrackerOutput.cs b/Tools/ArdupilotMegaPlanner/Antenna/ITrackerOutput.cs index deb5e84f42..bee2aa98df 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/ITrackerOutput.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/ITrackerOutput.cs @@ -16,6 +16,8 @@ namespace ArdupilotMega.Antenna int TiltStartRange { get; set; } int PanEndRange { get; set; } int TiltEndRange { get; set; } + int PanPWMRange { get; set; } + int TiltPWMRange { get; set; } bool PanReverse { get; set; } bool TiltReverse { get; set; } diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs b/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs index 635b5048bb..f77df09450 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs @@ -21,6 +21,8 @@ namespace ArdupilotMega.Antenna public int TiltStartRange { get; set; } public int PanEndRange { get; set; } public int TiltEndRange { get; set; } + public int PanPWMRange { get; set; } + public int TiltPWMRange { get; set; } public bool PanReverse { get { return _panreverse == -1; } set { _panreverse = value == true ? -1 : 1 ; } } public bool TiltReverse { get { return _tiltreverse == -1; } set { _tiltreverse = value == true ? -1 : 1; } } @@ -94,7 +96,7 @@ namespace ArdupilotMega.Antenna } public bool Pan(double Angle) - { + { double range = Math.Abs(PanStartRange - PanEndRange); // get relative center based on tracking center @@ -108,7 +110,7 @@ namespace ArdupilotMega.Antenna // conver the angle into a 0-255 value byte target = (byte)((((PointAtAngle / range) * 2.0) * 127 + centerpos) * _panreverse); - //Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); + Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); var buffer = new byte[] { 0xff,PanAddress,target}; ComPort.Write(buffer, 0, buffer.Length); diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs index fd998debfe..f748e44462 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs @@ -33,7 +33,6 @@ this.label1 = new System.Windows.Forms.Label(); this.CMB_baudrate = new System.Windows.Forms.ComboBox(); this.CMB_serialport = new System.Windows.Forms.ComboBox(); - this.BUT_connect = new ArdupilotMega.MyButton(); this.TRK_pantrim = new System.Windows.Forms.TrackBar(); this.TXT_panrange = new System.Windows.Forms.TextBox(); this.label3 = new System.Windows.Forms.Label(); @@ -44,6 +43,18 @@ this.TRK_tilttrim = new System.Windows.Forms.TrackBar(); this.label2 = new System.Windows.Forms.Label(); this.label7 = new System.Windows.Forms.Label(); + this.CHK_revpan = new System.Windows.Forms.CheckBox(); + this.CHK_revtilt = new System.Windows.Forms.CheckBox(); + this.TXT_pwmrangepan = new System.Windows.Forms.TextBox(); + this.TXT_pwmrangetilt = new System.Windows.Forms.TextBox(); + this.label8 = new System.Windows.Forms.Label(); + this.label9 = new System.Windows.Forms.Label(); + this.label10 = new System.Windows.Forms.Label(); + this.label11 = new System.Windows.Forms.Label(); + this.label12 = new System.Windows.Forms.Label(); + this.BUT_connect = new ArdupilotMega.MyButton(); + this.LBL_pantrim = new System.Windows.Forms.Label(); + this.LBL_tilttrim = new System.Windows.Forms.Label(); ((System.ComponentModel.ISupportInitialize)(this.TRK_pantrim)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.TRK_tilttrim)).BeginInit(); this.SuspendLayout(); @@ -52,7 +63,8 @@ // this.CMB_interface.FormattingEnabled = true; this.CMB_interface.Items.AddRange(new object[] { - "Maestro"}); + "Maestro", + "ArduTracker"}); this.CMB_interface.Location = new System.Drawing.Point(83, 10); this.CMB_interface.Name = "CMB_interface"; this.CMB_interface.Size = new System.Drawing.Size(121, 21); @@ -95,6 +107,184 @@ this.CMB_serialport.Size = new System.Drawing.Size(121, 21); this.CMB_serialport.TabIndex = 1; // + // TRK_pantrim + // + this.TRK_pantrim.Location = new System.Drawing.Point(153, 81); + this.TRK_pantrim.Maximum = 360; + this.TRK_pantrim.Minimum = -360; + this.TRK_pantrim.Name = "TRK_pantrim"; + this.TRK_pantrim.Size = new System.Drawing.Size(375, 45); + this.TRK_pantrim.TabIndex = 5; + this.TRK_pantrim.TickFrequency = 5; + this.TRK_pantrim.Scroll += new System.EventHandler(this.TRK_pantrim_Scroll); + // + // TXT_panrange + // + this.TXT_panrange.Location = new System.Drawing.Point(83, 81); + this.TXT_panrange.Name = "TXT_panrange"; + this.TXT_panrange.Size = new System.Drawing.Size(64, 20); + this.TXT_panrange.TabIndex = 4; + this.TXT_panrange.Text = "360"; + this.TXT_panrange.TextChanged += new System.EventHandler(this.TXT_panrange_TextChanged); + // + // label3 + // + this.label3.AutoSize = true; + this.label3.Location = new System.Drawing.Point(326, 65); + this.label3.Name = "label3"; + this.label3.Size = new System.Drawing.Size(27, 13); + this.label3.TabIndex = 10; + this.label3.Text = "Trim"; + // + // label4 + // + this.label4.AutoSize = true; + this.label4.Location = new System.Drawing.Point(83, 65); + this.label4.Name = "label4"; + this.label4.Size = new System.Drawing.Size(39, 13); + this.label4.TabIndex = 11; + this.label4.Text = "Range"; + // + // label5 + // + this.label5.AutoSize = true; + this.label5.Location = new System.Drawing.Point(83, 141); + this.label5.Name = "label5"; + this.label5.Size = new System.Drawing.Size(39, 13); + this.label5.TabIndex = 17; + this.label5.Text = "Range"; + // + // label6 + // + this.label6.AutoSize = true; + this.label6.Location = new System.Drawing.Point(326, 141); + this.label6.Name = "label6"; + this.label6.Size = new System.Drawing.Size(27, 13); + this.label6.TabIndex = 16; + this.label6.Text = "Trim"; + // + // TXT_tiltrange + // + this.TXT_tiltrange.Location = new System.Drawing.Point(83, 157); + this.TXT_tiltrange.Name = "TXT_tiltrange"; + this.TXT_tiltrange.Size = new System.Drawing.Size(64, 20); + this.TXT_tiltrange.TabIndex = 6; + this.TXT_tiltrange.Text = "90"; + this.TXT_tiltrange.TextChanged += new System.EventHandler(this.TXT_tiltrange_TextChanged); + // + // TRK_tilttrim + // + this.TRK_tilttrim.Location = new System.Drawing.Point(153, 157); + this.TRK_tilttrim.Maximum = 180; + this.TRK_tilttrim.Minimum = -180; + this.TRK_tilttrim.Name = "TRK_tilttrim"; + this.TRK_tilttrim.Size = new System.Drawing.Size(375, 45); + this.TRK_tilttrim.TabIndex = 7; + this.TRK_tilttrim.TickFrequency = 5; + this.TRK_tilttrim.Scroll += new System.EventHandler(this.TRK_tilttrim_Scroll); + // + // label2 + // + this.label2.AutoSize = true; + this.label2.Location = new System.Drawing.Point(12, 65); + this.label2.Name = "label2"; + this.label2.Size = new System.Drawing.Size(26, 13); + this.label2.TabIndex = 18; + this.label2.Text = "Pan"; + // + // label7 + // + this.label7.AutoSize = true; + this.label7.Location = new System.Drawing.Point(12, 141); + this.label7.Name = "label7"; + this.label7.Size = new System.Drawing.Size(21, 13); + this.label7.TabIndex = 19; + this.label7.Text = "Tilt"; + // + // CHK_revpan + // + this.CHK_revpan.AutoSize = true; + this.CHK_revpan.Location = new System.Drawing.Point(534, 83); + this.CHK_revpan.Name = "CHK_revpan"; + this.CHK_revpan.Size = new System.Drawing.Size(46, 17); + this.CHK_revpan.TabIndex = 20; + this.CHK_revpan.Text = "Rev"; + this.CHK_revpan.UseVisualStyleBackColor = true; + this.CHK_revpan.CheckedChanged += new System.EventHandler(this.CHK_revpan_CheckedChanged); + // + // CHK_revtilt + // + this.CHK_revtilt.AutoSize = true; + this.CHK_revtilt.Location = new System.Drawing.Point(534, 159); + this.CHK_revtilt.Name = "CHK_revtilt"; + this.CHK_revtilt.Size = new System.Drawing.Size(46, 17); + this.CHK_revtilt.TabIndex = 21; + this.CHK_revtilt.Text = "Rev"; + this.CHK_revtilt.UseVisualStyleBackColor = true; + this.CHK_revtilt.CheckedChanged += new System.EventHandler(this.CHK_revtilt_CheckedChanged); + // + // TXT_pwmrangepan + // + this.TXT_pwmrangepan.Location = new System.Drawing.Point(83, 107); + this.TXT_pwmrangepan.Name = "TXT_pwmrangepan"; + this.TXT_pwmrangepan.Size = new System.Drawing.Size(64, 20); + this.TXT_pwmrangepan.TabIndex = 22; + this.TXT_pwmrangepan.Text = "1000"; + // + // TXT_pwmrangetilt + // + this.TXT_pwmrangetilt.Location = new System.Drawing.Point(83, 183); + this.TXT_pwmrangetilt.Name = "TXT_pwmrangetilt"; + this.TXT_pwmrangetilt.Size = new System.Drawing.Size(64, 20); + this.TXT_pwmrangetilt.TabIndex = 23; + this.TXT_pwmrangetilt.Text = "1000"; + // + // label8 + // + this.label8.AutoSize = true; + this.label8.Location = new System.Drawing.Point(43, 110); + this.label8.Name = "label8"; + this.label8.Size = new System.Drawing.Size(34, 13); + this.label8.TabIndex = 24; + this.label8.Text = "PWM"; + // + // label9 + // + this.label9.AutoSize = true; + this.label9.Location = new System.Drawing.Point(43, 186); + this.label9.Name = "label9"; + this.label9.Size = new System.Drawing.Size(34, 13); + this.label9.TabIndex = 25; + this.label9.Text = "PWM"; + // + // label10 + // + this.label10.AutoSize = true; + this.label10.Location = new System.Drawing.Point(45, 160); + this.label10.Name = "label10"; + this.label10.Size = new System.Drawing.Size(34, 13); + this.label10.TabIndex = 27; + this.label10.Text = "Angle"; + // + // label11 + // + this.label11.AutoSize = true; + this.label11.Location = new System.Drawing.Point(45, 84); + this.label11.Name = "label11"; + this.label11.Size = new System.Drawing.Size(34, 13); + this.label11.TabIndex = 26; + this.label11.Text = "Angle"; + // + // label12 + // + this.label12.AutoSize = true; + this.label12.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Bold, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.label12.Location = new System.Drawing.Point(94, 40); + this.label12.Name = "label12"; + this.label12.Size = new System.Drawing.Size(403, 13); + this.label12.TabIndex = 28; + this.label12.Text = "Miss using this interface can cause servo damage, use with caution!!!"; + // // BUT_connect // this.BUT_connect.Location = new System.Drawing.Point(476, 9); @@ -105,105 +295,40 @@ this.BUT_connect.UseVisualStyleBackColor = true; this.BUT_connect.Click += new System.EventHandler(this.BUT_connect_Click); // - // TRK_pantrim + // LBL_pantrim // - this.TRK_pantrim.Location = new System.Drawing.Point(153, 65); - this.TRK_pantrim.Maximum = 90; - this.TRK_pantrim.Minimum = -90; - this.TRK_pantrim.Name = "TRK_pantrim"; - this.TRK_pantrim.Size = new System.Drawing.Size(375, 45); - this.TRK_pantrim.TabIndex = 5; - this.TRK_pantrim.TickFrequency = 5; - this.TRK_pantrim.Scroll += new System.EventHandler(this.TRK_pantrim_Scroll); + this.LBL_pantrim.AutoSize = true; + this.LBL_pantrim.Location = new System.Drawing.Point(326, 113); + this.LBL_pantrim.Name = "LBL_pantrim"; + this.LBL_pantrim.Size = new System.Drawing.Size(34, 13); + this.LBL_pantrim.TabIndex = 29; + this.LBL_pantrim.Text = "Angle"; // - // TXT_panrange + // LBL_tilttrim // - this.TXT_panrange.Location = new System.Drawing.Point(83, 65); - this.TXT_panrange.Name = "TXT_panrange"; - this.TXT_panrange.Size = new System.Drawing.Size(64, 20); - this.TXT_panrange.TabIndex = 4; - this.TXT_panrange.Text = "180"; - this.TXT_panrange.TextChanged += new System.EventHandler(this.TXT_panrange_TextChanged); - // - // label3 - // - this.label3.AutoSize = true; - this.label3.Location = new System.Drawing.Point(326, 49); - this.label3.Name = "label3"; - this.label3.Size = new System.Drawing.Size(27, 13); - this.label3.TabIndex = 10; - this.label3.Text = "Trim"; - // - // label4 - // - this.label4.AutoSize = true; - this.label4.Location = new System.Drawing.Point(83, 49); - this.label4.Name = "label4"; - this.label4.Size = new System.Drawing.Size(56, 13); - this.label4.TabIndex = 11; - this.label4.Text = "Range -/+"; - // - // label5 - // - this.label5.AutoSize = true; - this.label5.Location = new System.Drawing.Point(83, 125); - this.label5.Name = "label5"; - this.label5.Size = new System.Drawing.Size(56, 13); - this.label5.TabIndex = 17; - this.label5.Text = "Range -/+"; - // - // label6 - // - this.label6.AutoSize = true; - this.label6.Location = new System.Drawing.Point(326, 125); - this.label6.Name = "label6"; - this.label6.Size = new System.Drawing.Size(27, 13); - this.label6.TabIndex = 16; - this.label6.Text = "Trim"; - // - // TXT_tiltrange - // - this.TXT_tiltrange.Location = new System.Drawing.Point(83, 141); - this.TXT_tiltrange.Name = "TXT_tiltrange"; - this.TXT_tiltrange.Size = new System.Drawing.Size(64, 20); - this.TXT_tiltrange.TabIndex = 6; - this.TXT_tiltrange.Text = "90"; - this.TXT_tiltrange.TextChanged += new System.EventHandler(this.TXT_tiltrange_TextChanged); - // - // TRK_tilttrim - // - this.TRK_tilttrim.Location = new System.Drawing.Point(153, 141); - this.TRK_tilttrim.Maximum = 45; - this.TRK_tilttrim.Minimum = -45; - this.TRK_tilttrim.Name = "TRK_tilttrim"; - this.TRK_tilttrim.Size = new System.Drawing.Size(375, 45); - this.TRK_tilttrim.TabIndex = 7; - this.TRK_tilttrim.TickFrequency = 5; - this.TRK_tilttrim.Scroll += new System.EventHandler(this.TRK_tilttrim_Scroll); - // - // label2 - // - this.label2.AutoSize = true; - this.label2.Location = new System.Drawing.Point(13, 68); - this.label2.Name = "label2"; - this.label2.Size = new System.Drawing.Size(26, 13); - this.label2.TabIndex = 18; - this.label2.Text = "Pan"; - // - // label7 - // - this.label7.AutoSize = true; - this.label7.Location = new System.Drawing.Point(13, 144); - this.label7.Name = "label7"; - this.label7.Size = new System.Drawing.Size(21, 13); - this.label7.TabIndex = 19; - this.label7.Text = "Tilt"; + this.LBL_tilttrim.AutoSize = true; + this.LBL_tilttrim.Location = new System.Drawing.Point(326, 190); + this.LBL_tilttrim.Name = "LBL_tilttrim"; + this.LBL_tilttrim.Size = new System.Drawing.Size(34, 13); + this.LBL_tilttrim.TabIndex = 30; + this.LBL_tilttrim.Text = "Angle"; // // Tracker // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.ClientSize = new System.Drawing.Size(569, 195); + this.ClientSize = new System.Drawing.Size(587, 212); + this.Controls.Add(this.LBL_tilttrim); + this.Controls.Add(this.LBL_pantrim); + this.Controls.Add(this.label12); + this.Controls.Add(this.label10); + this.Controls.Add(this.label11); + this.Controls.Add(this.label9); + this.Controls.Add(this.label8); + this.Controls.Add(this.TXT_pwmrangetilt); + this.Controls.Add(this.TXT_pwmrangepan); + this.Controls.Add(this.CHK_revtilt); + this.Controls.Add(this.CHK_revpan); this.Controls.Add(this.label7); this.Controls.Add(this.label2); this.Controls.Add(this.label5); @@ -222,6 +347,7 @@ this.Icon = ((System.Drawing.Icon)(resources.GetObject("$this.Icon"))); this.Name = "Tracker"; this.Text = "Tracker"; + this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.Tracker_FormClosing); ((System.ComponentModel.ISupportInitialize)(this.TRK_pantrim)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.TRK_tilttrim)).EndInit(); this.ResumeLayout(false); @@ -246,5 +372,16 @@ private System.Windows.Forms.TrackBar TRK_tilttrim; private System.Windows.Forms.Label label2; private System.Windows.Forms.Label label7; + private System.Windows.Forms.CheckBox CHK_revpan; + private System.Windows.Forms.CheckBox CHK_revtilt; + private System.Windows.Forms.TextBox TXT_pwmrangepan; + private System.Windows.Forms.TextBox TXT_pwmrangetilt; + private System.Windows.Forms.Label label8; + private System.Windows.Forms.Label label9; + private System.Windows.Forms.Label label10; + private System.Windows.Forms.Label label11; + private System.Windows.Forms.Label label12; + private System.Windows.Forms.Label LBL_pantrim; + private System.Windows.Forms.Label LBL_tilttrim; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs index 5c1ee337bc..53bad8d3bb 100644 --- a/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs +++ b/Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs @@ -15,6 +15,12 @@ namespace ArdupilotMega.Antenna static bool threadrun = false; static ITrackerOutput tracker; + enum interfaces + { + Maestro, + ArduTracker + } + public Tracker() { InitializeComponent(); @@ -27,10 +33,63 @@ namespace ArdupilotMega.Antenna { BUT_connect.Text = "Disconnect"; } + + foreach (string value in MainV2.config.Keys) + { + if (value.StartsWith("Tracker_")) + { + var ctls = Controls.Find(value.Replace("Tracker_",""),true); + + foreach (Control ctl in ctls) + { + if (typeof(TextBox) == ctl.GetType() || + typeof(ComboBox) == ctl.GetType()) + { + ctl.Text = MainV2.config[value].ToString(); + } + else if (typeof(TrackBar) == ctl.GetType()) + { + ((TrackBar)ctl).Value = int.Parse(MainV2.config[value].ToString()); + } + else if (typeof(CheckBox) == ctl.GetType()) + { + ((CheckBox)ctl).Checked = bool.Parse(MainV2.config[value].ToString()); + } + } + } + } + + // update other fields from load params + TXT_panrange_TextChanged(null, null); + TXT_tiltrange_TextChanged(null, null); + TRK_pantrim_Scroll(null, null); + TRK_tilttrim_Scroll(null, null); + } + + void saveconfig() + { + foreach (Control ctl in Controls) + { + if (typeof(TextBox) == ctl.GetType() || + typeof(ComboBox) == ctl.GetType()) + { + MainV2.config["Tracker_" + ctl.Name] = ctl.Text; + } + if (typeof(TrackBar) == ctl.GetType()) + { + MainV2.config["Tracker_" + ctl.Name] = ((TrackBar)ctl).Value; + } + if (typeof(CheckBox) == ctl.GetType()) + { + MainV2.config["Tracker_" + ctl.Name] = ((CheckBox)ctl).Checked; + } + } } private void BUT_connect_Click(object sender, EventArgs e) { + saveconfig(); + if (threadrun) { threadrun = false; @@ -44,7 +103,10 @@ namespace ArdupilotMega.Antenna tracker.ComPort.Close(); } - tracker = new ArdupilotMega.Antenna.Maestro(); + if (CMB_interface.Text == "Maestro") + tracker = new ArdupilotMega.Antenna.Maestro(); + if (CMB_interface.Text == "ArduTracker") + tracker = new ArdupilotMega.Antenna.ArduTracker(); try { @@ -66,6 +128,12 @@ namespace ArdupilotMega.Antenna tracker.TiltEndRange = int.Parse(TXT_tiltrange.Text) / 2; tracker.TrimTilt = TRK_tilttrim.Value; + tracker.PanReverse = CHK_revpan.Checked; + tracker.TiltReverse = CHK_revtilt.Checked; + + tracker.PanPWMRange = int.Parse(TXT_pwmrangepan.Text); + tracker.TiltPWMRange = int.Parse(TXT_pwmrangetilt.Text); + } catch (Exception ex) { CustomMessageBox.Show("Bad User input " + ex.Message); return; } @@ -83,6 +151,8 @@ namespace ArdupilotMega.Antenna t12.Start(); } } + + BUT_connect.Text = "Disconnect"; } void mainloop() @@ -104,12 +174,14 @@ namespace ArdupilotMega.Antenna { if (tracker != null) tracker.TrimPan = TRK_pantrim.Value; + LBL_pantrim.Text = TRK_pantrim.Value.ToString(); } private void TRK_tilttrim_Scroll(object sender, EventArgs e) { if (tracker != null) tracker.TrimTilt = TRK_tilttrim.Value; + LBL_tilttrim.Text = TRK_tilttrim.Value.ToString(); } private void TXT_panrange_TextChanged(object sender, EventArgs e) @@ -118,8 +190,8 @@ namespace ArdupilotMega.Antenna int.TryParse(TXT_panrange.Text, out range); - TRK_pantrim.Minimum = range / 2 * -1; - TRK_pantrim.Maximum = range / 2; + TRK_pantrim.Minimum = range / 1 * -1; + TRK_pantrim.Maximum = range / 1; } private void TXT_tiltrange_TextChanged(object sender, EventArgs e) @@ -128,8 +200,23 @@ namespace ArdupilotMega.Antenna int.TryParse(TXT_tiltrange.Text, out range); - TRK_tilttrim.Minimum = range / 2 * -1; - TRK_tilttrim.Maximum = range / 2; + TRK_tilttrim.Minimum = range / 1 * -1; + TRK_tilttrim.Maximum = range / 1; + } + + private void CHK_revpan_CheckedChanged(object sender, EventArgs e) + { + + } + + private void CHK_revtilt_CheckedChanged(object sender, EventArgs e) + { + + } + + private void Tracker_FormClosing(object sender, FormClosingEventArgs e) + { + saveconfig(); } } } diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 80b938dd1c..bd24ec4529 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -213,6 +213,7 @@ + @@ -222,6 +223,18 @@ Tracker.cs + + UserControl + + + BackstageView.cs + + + Component + + + Component + Form @@ -229,7 +242,74 @@ ProgressReporterDialogue.cs + + UserControl + + + ConfigAccelerometerCalibration.cs + + + UserControl + + + ConfigArducopter.cs + + + UserControl + + + ConfigArduplane.cs + + + UserControl + + + ConfigBatteryMonitoring.cs + + + UserControl + + + ConfigFlightModes.cs + + + UserControl + + + ConfigHardwareOptions.cs + + + UserControl + + + ConfigPlanner.cs + + + UserControl + + + ConfigRadioInput.cs + + + UserControl + + + ConfigTradHeli.cs + + + UserControl + + + Configuration.cs + + + UserControl + + + ConfigRawParams.cs + + Form @@ -446,14 +526,49 @@ - Tracker.cs + + BackstageView.cs + ProgressReporterDialogue.cs + + ConfigAccelerometerCalibration.cs + + + ConfigArducopter.cs + + + ConfigArduplane.cs + + + ConfigBatteryMonitoring.cs + + + ConfigFlightModes.cs + + + ConfigHardwareOptions.cs + + + ConfigPlanner.cs + + + ConfigRadioInput.cs + + + ConfigRawParams.cs + + + ConfigTradHeli.cs + + + Configuration.cs + 3DRradio.cs diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index bb5aecd2d0..826ae2117d 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -381,7 +381,12 @@ namespace ArdupilotMega CH6_NAV_I = 20, CH6_LOITER_RATE_P = 22, - CH6_LOITER_RATE_D = 23 + CH6_LOITER_RATE_D = 23, + CH6_YAW_KI = 24, + CH6_ACRO_KP = 25, + CH6_YAW_RATE_KD = 26, + CH6_LOITER_KI = 27, + CH6_LOITER_RATE_KI = 28 } diff --git a/Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs b/Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs index 1bf03f955b..cd85b92445 100644 --- a/Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs +++ b/Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs @@ -34,7 +34,7 @@ namespace ArdupilotMega int BaudRate { get; set; } //bool BreakState { get; set; } int BytesToRead { get; } - //int BytesToWrite { get; } + int BytesToWrite { get; } //bool CDHolding { get; } //bool CtsHolding { get; } int DataBits { get; set; } diff --git a/Tools/ArdupilotMegaPlanner/CommsSerialPort.cs b/Tools/ArdupilotMegaPlanner/CommsSerialPort.cs index 5b08850731..ae5d6ef677 100644 --- a/Tools/ArdupilotMegaPlanner/CommsSerialPort.cs +++ b/Tools/ArdupilotMegaPlanner/CommsSerialPort.cs @@ -3,6 +3,7 @@ using System.Collections.Generic; using System.Text; using System.IO.Ports; using System.IO; +using System.Linq; namespace ArdupilotMega { @@ -18,6 +19,11 @@ namespace ArdupilotMega public void toggleDTR() { + bool open = this.IsOpen; + + if (!open) + this.Open(); + base.DtrEnable = false; base.RtsEnable = false; @@ -27,6 +33,56 @@ namespace ArdupilotMega base.RtsEnable = true; System.Threading.Thread.Sleep(50); + + if (!open) + this.Close(); + } + + public new static string[] GetPortNames() + { + string[] monoDevs = new string[0]; + + if (Directory.Exists("/dev/")) + { + if (Directory.Exists("/dev/serial/by-id/")) + monoDevs = Directory.GetFiles("/dev/serial/by-id/", "*"); + monoDevs = Directory.GetFiles("/dev/", "*ACM*"); + monoDevs = Directory.GetFiles("/dev/", "ttyUSB*"); + } + + string[] ports = System.IO.Ports.SerialPort.GetPortNames() + .Select(p => p.TrimEnd()) + .Select(FixBlueToothPortNameBug) + .ToArray(); + + string[] allPorts = new string[monoDevs.Length + ports.Length]; + + monoDevs.CopyTo(allPorts, 0); + ports.CopyTo(allPorts, monoDevs.Length); + + return allPorts; + } + + + // .NET bug: sometimes bluetooth ports are enumerated with bogus characters + // eg 'COM10' becomes 'COM10c' - one workaround is to remove the non numeric + // char. Annoyingly, sometimes a numeric char is added, which means this + // does not work in all cases. + // See http://connect.microsoft.com/VisualStudio/feedback/details/236183/system-io-ports-serialport-getportnames-error-with-bluetooth + private static string FixBlueToothPortNameBug(string portName) + { + if (!portName.StartsWith("COM")) + return portName; + var newPortName = "COM"; // Start over with "COM" + foreach (var portChar in portName.Substring(3).ToCharArray()) // Remove "COM", put the rest in a character array + { + if (char.IsDigit(portChar)) + newPortName += portChar.ToString(); // Good character, append to portName + // else + //log.WarnFormat("Bad (Non Numeric) character in port name '{0}' - removing", portName); + } + + return newPortName; } } } diff --git a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs index 0331169c87..76492e1e8b 100644 --- a/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs @@ -66,6 +66,8 @@ namespace System.IO.Ports get { return client.Available + rbuffer.Length - rbufferread; } } + public int BytesToWrite { get { return 0; } } + public bool IsOpen { get { try { return client.Client.Connected; } catch { return false; } } } public bool DtrEnable diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index 3556c12f67..f5b5feb3e2 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -60,6 +60,8 @@ namespace System.IO.Ports get { return client.Available + rbuffer.Length - rbufferread; } } + public int BytesToWrite { get {return 0;} } + public bool IsOpen { get { if (client.Client == null) return false; return client.Client.Connected; } } public bool DtrEnable diff --git a/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackStageViewMenuPanel.cs b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackStageViewMenuPanel.cs new file mode 100644 index 0000000000..1a9ba58375 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackStageViewMenuPanel.cs @@ -0,0 +1,33 @@ +using System.Drawing; +using System.Drawing.Drawing2D; +using System.Windows.Forms; + +namespace ArdupilotMega.Controls.BackstageView +{ + public class BackStageViewMenuPanel : Panel + { + internal Color GradColor = Color.White; + internal Color PencilBorderColor = Color.White; + + private const int GradientWidth = 6; + + public BackStageViewMenuPanel() + { + this.SetStyle(ControlStyles.UserPaint, true); + } + + protected override void OnPaintBackground(PaintEventArgs pevent) + { + base.OnPaintBackground(pevent); + + var rc = new Rectangle(ClientSize.Width - GradientWidth, 0, GradientWidth, this.ClientSize.Height); + + using (var brush = new LinearGradientBrush(rc, BackColor, GradColor, LinearGradientMode.Horizontal)) + { + pevent.Graphics.FillRectangle(brush, rc); + } + + pevent.Graphics.DrawLine(new Pen(PencilBorderColor), Width-1,0,Width-1,Height); + } + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.Designer.cs b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.Designer.cs new file mode 100644 index 0000000000..07546e1ac8 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.Designer.cs @@ -0,0 +1,73 @@ +namespace ArdupilotMega.Controls.BackstageView +{ + partial class BackstageView + { + /// + /// Required designer variable. + /// + private System.ComponentModel.IContainer components = null; + + /// + /// Clean up any resources being used. + /// + /// true if managed resources should be disposed; otherwise, false. + protected override void Dispose(bool disposing) + { + if (disposing && (components != null)) + { + components.Dispose(); + } + base.Dispose(disposing); + } + + #region Component Designer generated code + + /// + /// Required method for Designer support - do not modify + /// the contents of this method with the code editor. + /// + private void InitializeComponent() + { + this.pnlPages = new System.Windows.Forms.Panel(); + this.pnlMenu = new BackStageViewMenuPanel(); + this.SuspendLayout(); + // + // pnlPages + // + this.pnlPages.Anchor = ((System.Windows.Forms.AnchorStyles)((((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom) + | System.Windows.Forms.AnchorStyles.Left) + | System.Windows.Forms.AnchorStyles.Right))); + this.pnlPages.Location = new System.Drawing.Point(147, 0); + this.pnlPages.MinimumSize = new System.Drawing.Size(100, 0); + this.pnlPages.Name = "pnlPages"; + this.pnlPages.Size = new System.Drawing.Size(243, 189); + this.pnlPages.TabIndex = 0; + // + // pnlMenu + // + this.pnlMenu.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom) + | System.Windows.Forms.AnchorStyles.Left))); + this.pnlMenu.Location = new System.Drawing.Point(0, 0); + this.pnlMenu.Name = "pnlMenu"; + this.pnlMenu.Size = new System.Drawing.Size(150, 170); + this.pnlMenu.TabIndex = 1; + // + // BackstageView + // + this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); + this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.AutoSize = true; + this.Controls.Add(this.pnlMenu); + this.Controls.Add(this.pnlPages); + this.Name = "BackstageView"; + this.Size = new System.Drawing.Size(393, 192); + this.ResumeLayout(false); + + } + + #endregion + + private System.Windows.Forms.Panel pnlPages; + private BackStageViewMenuPanel pnlMenu; + } +} diff --git a/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.cs b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.cs new file mode 100644 index 0000000000..2970e499b6 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.cs @@ -0,0 +1,225 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Drawing; +using System.Linq; +using System.Windows.Forms; +using ArdupilotMega.Controls.BackstageView; + +namespace ArdupilotMega.Controls.BackstageView +{ + public partial class BackstageView : UserControl + { + private Color _buttonsAreaBgColor = Color.White; + private Color _buttonsAreaPencilColor = Color.DarkGray; + private Color _selectedTextColor = Color.White; + private Color _unSelectedTextColor = Color.Gray; + private Color _highlightColor1 = Color.DarkBlue; + private Color _highlightColor2 = Color.Blue; + + private readonly List _pages= new List(); + private BackstageViewPage _activePage; + private const int ButtonSpacing = 30; + private const int ButtonHeight = 30; + + public BackstageView() + { + InitializeComponent(); + + this.pnlMenu.Height = this.Height; + this.pnlPages.Height = this.Height; + + pnlMenu.BackColor = _buttonsAreaBgColor; + pnlMenu.PencilBorderColor = _buttonsAreaPencilColor; + pnlMenu.GradColor = this.BackColor; + } + + + public override Color BackColor + { + get + { + return base.BackColor; + } + set + { + base.BackColor = value; + UpdateButtons(); + pnlMenu.GradColor = this.BackColor; + } + } + + [Description("Background pencil line color for the content region"), Category("Appearance")] + [DefaultValue(typeof(Color),"DarkGray")] + public Color ButtonsAreaPencilColor + { + get { return _buttonsAreaPencilColor; } + set + { + _buttonsAreaPencilColor = value; + pnlMenu.PencilBorderColor = _buttonsAreaPencilColor; + pnlMenu.Invalidate(); + UpdateButtons(); + Invalidate(); + } + } + + + [Description("Background color for the buttons region"), Category("Appearance")] + [DefaultValue(typeof(Color),"White")] + public Color ButtonsAreaBgColor + { + get { return _buttonsAreaBgColor; } + set + { + _buttonsAreaBgColor = value; + this.pnlMenu.BackColor = _buttonsAreaBgColor; + pnlMenu.Invalidate(); + Invalidate(); + } + } + + [Description("Color for the selector buttons text"), Category("Appearance")] + [DefaultValue(typeof(Color), "White")] + public Color SelectedTextColor + { + get { return _selectedTextColor; } + set + { + _selectedTextColor = value; + UpdateButtons(); + } + } + + [Description("Color for the un selected selector buttons text"), Category("Appearance")] + [DefaultValue(typeof(Color), "Gray")] + public Color UnSelectedTextColor + { + get { return _unSelectedTextColor; } + set + { + _unSelectedTextColor = value; + UpdateButtons(); + Invalidate(); + } + } + + [Description("Color selected button background 1"), Category("Appearance")] + [DefaultValue(typeof(Color), "DarkBlue")] + public Color HighlightColor1 + { + get { return _highlightColor1; } + set + { + _highlightColor1 = value; + UpdateButtons(); + Invalidate(); + } + } + + [Description("Color selected button background 2"), Category("Appearance")] + [DefaultValue(typeof(Color), "Blue")] + public Color HighlightColor2 + { + get { return _highlightColor2; } + set + { + _highlightColor2 = value; + UpdateButtons(); + Invalidate(); + } + } + + + private void UpdateButtons() + { + foreach (var backstageViewButton in pnlMenu.Controls.OfType()) + { + backstageViewButton.HighlightColor2 = _highlightColor2; + backstageViewButton.HighlightColor1 = _highlightColor1; + backstageViewButton.UnSelectedTextColor = _unSelectedTextColor; + backstageViewButton.SelectedTextColor = _selectedTextColor; + backstageViewButton.ContentPageColor = this.BackColor; + backstageViewButton.PencilBorderColor = _buttonsAreaPencilColor; + + backstageViewButton.Invalidate(); + } + } + + public void AddPage(BackstageViewPage page) + { + page.Page.Anchor = AnchorStyles.Bottom | AnchorStyles.Left | AnchorStyles.Right | AnchorStyles.Top; + page.Page.Location = new Point(pnlMenu.Width, 0); + page.Page.Dock = DockStyle.Fill; + + _pages.Add(page); + CreateLinkButton(page); + this.pnlPages.Controls.Add(page.Page); + + if (_activePage == null) + _activePage = page; + + ActivatePage(page); + // Todo: set the link button to be selected looking + } + + private void CreateLinkButton(BackstageViewPage page) + { + var lnkButton = new BackstageViewButton + { + Text = page.LinkText, + Tag = page, + Top = _pages.IndexOf(page) * ButtonSpacing, + Width = this.pnlMenu.Width, + Height = ButtonHeight, + ContentPageColor = this.BackColor, + PencilBorderColor = _buttonsAreaPencilColor, + SelectedTextColor = _selectedTextColor, + UnSelectedTextColor = _unSelectedTextColor, + HighlightColor1 = _highlightColor1, + HighlightColor2 = _highlightColor2 + }; + + pnlMenu.Controls.Add(lnkButton); + lnkButton.Click += this.ButtonClick; + } + + + private void ButtonClick(object sender, EventArgs e) + { + var backstageViewButton = ((BackstageViewButton) sender); + var associatedPage = backstageViewButton.Tag as BackstageViewPage; + this.ActivatePage(associatedPage); + } + + private void ActivatePage(BackstageViewPage associatedPage) + { + // deactivate the old page + _activePage.Page.Visible = false; + var oldButton = this.pnlMenu.Controls.OfType().Single(b => b.Tag == _activePage); + oldButton.IsSelected = false; + + associatedPage.Page.Visible = true; + var newButton = this.pnlMenu.Controls.OfType().Single(b => b.Tag == associatedPage); + newButton.IsSelected = true; + + _activePage = associatedPage; + } + + + public class BackstageViewPage + { + public BackstageViewPage(Control page, string linkText) + { + Page = page; + LinkText = linkText; + } + + public Control Page { get; private set; } + public string LinkText { get; set; } + } + } + + + +} diff --git a/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.resx b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.resx new file mode 100644 index 0000000000..7080a7d118 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.resx @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageViewButton.cs b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageViewButton.cs new file mode 100644 index 0000000000..7f384a5bcd --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageViewButton.cs @@ -0,0 +1,150 @@ +using System; +using System.Drawing; +using System.Drawing.Drawing2D; +using System.Windows.Forms; + +namespace ArdupilotMega.Controls.BackstageView +{ + public class BackstageViewButton : Control + { + private bool _isSelected; + + internal Color ContentPageColor = Color.Gray; + internal Color PencilBorderColor = Color.White; + internal Color SelectedTextColor = Color.White; + internal Color UnSelectedTextColor = Color.Gray; + internal Color HighlightColor1 = Color.DarkBlue; + internal Color HighlightColor2 = Color.Blue; + private bool _isMouseOver; + + //internal Color HighlightColor1 = Color.FromArgb(0x94, 0xc1, 0x1f); + //internal Color HighlightColor2 = Color.FromArgb(0xcd, 0xe2, 0x96); + + public BackstageViewButton() + { + SetStyle(ControlStyles.SupportsTransparentBackColor, true); + SetStyle(ControlStyles.Opaque, true); + SetStyle(ControlStyles.ResizeRedraw, true); + this.BackColor = Color.Transparent; + } + + /// + /// Whether this button should show the selected style + /// + public bool IsSelected + { + get { return _isSelected; } + set + { + if (_isSelected != value) + { + _isSelected = value; + //this.Parent.Refresh(); // <-- brutal, but works + + + InvalidateParentForBackground(); + + this.Invalidate(); + } + } + } + + // Must be a better way to redraw parent control in the area of + // the button + private void InvalidateParentForBackground() + { + var screenrect = this.RectangleToScreen(this.ClientRectangle); + var rectangleToClient = this.Parent.RectangleToClient(screenrect); + this.Parent.Invalidate(rectangleToClient); + } + + + protected override void OnPaint(PaintEventArgs pevent) + { + Graphics g = pevent.Graphics; + + + // Now the little 'arrow' thingy if we are selected and the selected bg grad + if (_isSelected) + { + var rect1 = new Rectangle(0, 0, Width / 2, Height); + var rect2 = new Rectangle(Width / 2, 0, Width, Height); + + g.FillRectangle(new LinearGradientBrush(rect1, HighlightColor1, HighlightColor2, LinearGradientMode.Horizontal), rect1); + g.FillRectangle(new LinearGradientBrush(rect2, HighlightColor2, HighlightColor1, LinearGradientMode.Horizontal), rect2); + + var butPen = new Pen(HighlightColor1); + g.DrawLine(butPen, 0, 0, Width, 0); + g.DrawLine(butPen, 0, Height - 1, Width, Height - 1); + + var arrowBrush = new SolidBrush(this.ContentPageColor); + + var midheight = Height / 2; + var arSize = 8; + + var arrowPoints = new[] + { + new Point(Width, midheight + arSize), + new Point(Width - arSize, midheight), + new Point(Width, midheight - arSize) + }; + + g.DrawString(Text, new Font(FontFamily.GenericSansSerif, 10), new SolidBrush(SelectedTextColor), 20, 6); + + g.FillPolygon(arrowBrush, arrowPoints); + + var pencilBrush = new Pen(this.PencilBorderColor); + + + g.DrawPolygon(pencilBrush, arrowPoints); + + + } + else + { + if (_isMouseOver) + { + var brush = new SolidBrush(Color.FromArgb(10, 0xA0, 0xA0, 0xA0)); + + g.FillRectangle(brush, this.ClientRectangle); + + var butPen = new Pen(PencilBorderColor); + g.DrawLine(butPen, 0, 0, Width, 0); + g.DrawLine(butPen, 0, Height - 1, Width, Height - 1); + } + + g.DrawString(Text, new Font(FontFamily.GenericSansSerif, 10), new SolidBrush(this.UnSelectedTextColor), 20, 6); + } + } + + + protected override void OnMouseEnter(EventArgs e) + { + _isMouseOver = true; + base.OnMouseEnter(e); + InvalidateParentForBackground(); + this.Invalidate(); + } + + protected override void OnMouseLeave(EventArgs e) + { + _isMouseOver = false; + base.OnMouseLeave(e); + InvalidateParentForBackground(); + this.Invalidate(); + + } + + // This IS necessary for transparency + protected override CreateParams CreateParams + { + get + { + const int WS_EX_TRANSPARENT = 0x20; + CreateParams cp = base.CreateParams; + cp.ExStyle |= WS_EX_TRANSPARENT; + return cp; + } + } + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Controls/CustomMessageBox.cs b/Tools/ArdupilotMegaPlanner/Controls/CustomMessageBox.cs index fda4573224..71b2f5f0d2 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/CustomMessageBox.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/CustomMessageBox.cs @@ -59,6 +59,7 @@ namespace System.Windows.Forms Width = textSize.Width + 50, Height = textSize.Height + 100, TopMost = true, + TopLevel = true }; Rectangle screenRectangle = msgBoxFrm.RectangleToScreen(msgBoxFrm.ClientRectangle); diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index c64b47acfc..7b8433162d 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -4,7 +4,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560-2.hex - ArduPlane V2.30 + ArduPlane V2.32 12 @@ -12,7 +12,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560-2.hex - ArduPlane V2.30 HIL + ArduPlane V2.32 HIL #define FLIGHT_MODE_CHANNEL 8 #define FLIGHT_MODE_1 AUTO @@ -47,7 +47,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex - ArduCopter V2.5 Quad + ArduCopter V2.5.3 Quad #define FRAME_CONFIG QUAD_FRAME @@ -58,7 +58,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex - ArduCopter V2.5 Tri + ArduCopter V2.5.3 Tri #define FRAME_CONFIG TRI_FRAME @@ -69,7 +69,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex - ArduCopter V2.5 Hexa X + ArduCopter V2.5.3 Hexa X #define FRAME_CONFIG HEXA_FRAME @@ -80,7 +80,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex - ArduCopter V2.5 Y6 + ArduCopter V2.5.3 Y6 #define FRAME_CONFIG Y6_FRAME @@ -91,7 +91,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex - ArduCopter V2.5 Octa V + ArduCopter V2.5.3 Octa V #define FRAME_CONFIG OCTA_FRAME #define FRAME_ORIENTATION V_FRAME @@ -103,7 +103,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex - ArduCopter V2.5 Octa X + ArduCopter V2.5.3 Octa X #define FRAME_CONFIG OCTA_FRAME @@ -114,7 +114,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560-2.hex - ArduCopter V2.5 Heli (2560 only) + ArduCopter V2.5.3 Heli (2560 only) #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME @@ -162,7 +162,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex - ArduCopter V2.5 Quad Hil + ArduCopter V2.5.3 Quad Hil #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION PLUS_FRAME @@ -178,7 +178,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560-2.hex - ArduCopter V2.5 Heli Hil + ArduCopter V2.5.3 Heli Hil #define HIL_MODE HIL_MODE_ATTITUDE diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index 23aab4e460..c95bbbd9a0 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -141,6 +141,8 @@ this.RLL2SRV_P = new System.Windows.Forms.NumericUpDown(); this.label52 = new System.Windows.Forms.Label(); this.TabAC = new System.Windows.Forms.TabPage(); + this.myLabel4 = new ArdupilotMega.MyLabel(); + this.myLabel3 = new ArdupilotMega.MyLabel(); this.TUNE_LOW = new System.Windows.Forms.NumericUpDown(); this.TUNE_HIGH = new System.Windows.Forms.NumericUpDown(); this.myLabel2 = new ArdupilotMega.MyLabel(); @@ -289,8 +291,6 @@ this.BUT_load = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.BUT_compare = new ArdupilotMega.MyButton(); - this.myLabel3 = new ArdupilotMega.MyLabel(); - this.myLabel4 = new ArdupilotMega.MyLabel(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); this.ConfigTabs.SuspendLayout(); this.TabAP.SuspendLayout(); @@ -437,6 +437,8 @@ this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; this.Params.RowHeadersVisible = false; this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); + this.Params.KeyDown += new System.Windows.Forms.KeyEventHandler(this.Params_KeyDown); + this.Params.KeyPress += new System.Windows.Forms.KeyPressEventHandler(this.Params_KeyPress); // // Command // @@ -1118,6 +1120,18 @@ resources.ApplyResources(this.TabAC, "TabAC"); this.TabAC.Name = "TabAC"; // + // myLabel4 + // + resources.ApplyResources(this.myLabel4, "myLabel4"); + this.myLabel4.Name = "myLabel4"; + this.myLabel4.resize = false; + // + // myLabel3 + // + resources.ApplyResources(this.myLabel3, "myLabel3"); + this.myLabel3.Name = "myLabel3"; + this.myLabel3.resize = false; + // // TUNE_LOW // resources.ApplyResources(this.TUNE_LOW, "TUNE_LOW"); @@ -2159,18 +2173,6 @@ this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); // - // myLabel3 - // - resources.ApplyResources(this.myLabel3, "myLabel3"); - this.myLabel3.Name = "myLabel3"; - this.myLabel3.resize = false; - // - // myLabel4 - // - resources.ApplyResources(this.myLabel4, "myLabel4"); - this.myLabel4.Name = "myLabel4"; - this.myLabel4.resize = false; - // // Configuration // resources.ApplyResources(this, "$this"); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index b7023440b0..853d0d4ca1 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -1190,13 +1190,45 @@ namespace ArdupilotMega.GCSViews BUT_load_Click(BUT_load, null); return true; } + if (Params.Focused) + { + if (keyData >= Keys.A && keyData <= Keys.Z) + { + int row = FindRowIndex(0, keyData.ToString()); + Params.FirstDisplayedScrollingRowIndex = row; + Params.ClearSelection(); + Params[1, row].Selected = true; + } + } return base.ProcessCmdKey(ref msg, keyData); } + int FindRowIndex(int col, string startswith) + { + foreach (DataGridViewRow row in Params.Rows) + { + if (row.Cells[col].Value.ToString().StartsWith(startswith)) + { + return row.Index; + } + } + return 0; + } + private void CMB_ratesensors_SelectedIndexChanged(object sender, EventArgs e) { MainV2.config[((ComboBox)sender).Name] = ((ComboBox)sender).Text; MainV2.cs.ratesensors = byte.Parse(((ComboBox)sender).Text); } + + private void Params_KeyDown(object sender, KeyEventArgs e) + { + + } + + private void Params_KeyPress(object sender, KeyPressEventArgs e) + { + + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index fc46ee040c..651bfd7e74 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -792,7 +792,7 @@ namespace ArdupilotMega.GCSViews try { Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); - swlog = new StreamWriter(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm") + " telem.log"); + swlog = new StreamWriter(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm") + " telem.log"); } catch { CustomMessageBox.Show("Log creation error"); } } @@ -1375,7 +1375,7 @@ namespace ArdupilotMega.GCSViews aviwriter = new AviWriter(); Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); - aviwriter.avi_start(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".avi"); + aviwriter.avi_start(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".avi"); recordHudToAVIToolStripMenuItem.Text = "Recording"; } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs index b11ff009d1..2c4897acef 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs @@ -201,9 +201,11 @@ namespace ArdupilotMega.GCSViews } catch { return; } } - + try + { comPort.Write("\n\n\n"); - + } + catch { return; } while (threadrun) { try diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index f414925ac0..c5b6b1f1a5 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -233,7 +233,7 @@ namespace ArdupilotMega case serialstatus.Createfile: receivedbytes = 0; Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); - logfile = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm") + " " + currentlog + ".log"; + logfile = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm") + " " + currentlog + ".log"; sw = new StreamWriter(logfile); status = serialstatus.Waiting; lock (thisLock) @@ -454,7 +454,7 @@ namespace ArdupilotMega foreach (Data mod in flightdata) { xw.WriteStartElement("trkpt"); - xw.WriteAttributeString("lat",mod.model.Location.latitude.ToString(new System.Globalization.CultureInfo("en-US"))); + xw.WriteAttributeString("lat", mod.model.Location.latitude.ToString(new System.Globalization.CultureInfo("en-US"))); xw.WriteAttributeString("lon", mod.model.Location.longitude.ToString(new System.Globalization.CultureInfo("en-US"))); xw.WriteElementString("ele", mod.model.Location.altitude.ToString(new System.Globalization.CultureInfo("en-US"))); diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index a324140c75..8299c9e585 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -101,6 +101,7 @@ namespace ArdupilotMega public DateTime lastlogread = DateTime.MinValue; public BinaryReader logplaybackfile = null; public BinaryWriter logfile = null; + public BinaryWriter rawlogfile = null; int bps1 = 0; int bps2 = 0; @@ -184,9 +185,6 @@ namespace ArdupilotMega BaseStream.DiscardInBuffer(); - // removed because of apc220 units - //BaseStream.toggleDTR(); - Thread.Sleep(1000); } @@ -1164,6 +1162,8 @@ namespace ArdupilotMega req.start_stop = 1; // start req.req_stream_id = id; // id + // send each one twice. + generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); } @@ -1956,6 +1956,18 @@ namespace ArdupilotMega DateTime start = DateTime.Now; + try + { + // test fabs idea - http://diydrones.com/profiles/blogs/flying-with-joystick?commentId=705844%3AComment%3A818712&xg_source=msg_com_blogpost + if (BaseStream.IsOpen && BaseStream.BytesToWrite > 0) + { + // slow down execution. + Thread.Sleep(1); + return new byte[0]; + } + } + catch (Exception ex) { log.Info(ex.ToString()); } + lock (readlock) { @@ -2012,7 +2024,11 @@ namespace ArdupilotMega System.Threading.Thread.Sleep(1); } if (BaseStream.IsOpen) + { temp[count] = (byte)BaseStream.ReadByte(); + if (rawlogfile != null) + rawlogfile.Write(temp[count]); + } } } catch (Exception e) { log.Info("MAVLink readpacket read error: " + e.Message); break; } @@ -2069,6 +2085,11 @@ namespace ArdupilotMega if (BaseStream.IsOpen) { int read = BaseStream.Read(temp, 6, length - 4); + if (rawlogfile != null) + { + rawlogfile.Write(temp, 0, read); + rawlogfile.BaseStream.Flush(); + } } } //Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead); diff --git a/Tools/ArdupilotMegaPlanner/MagCalib.cs b/Tools/ArdupilotMegaPlanner/MagCalib.cs index 8e88536308..e94a639932 100644 --- a/Tools/ArdupilotMegaPlanner/MagCalib.cs +++ b/Tools/ArdupilotMegaPlanner/MagCalib.cs @@ -21,7 +21,7 @@ namespace ArdupilotMega /// /// Self contained process tlog and save/display offsets /// - public static void ProcessLog() + public static void ProcessLog(int throttleThreshold = 0) { OpenFileDialog openFileDialog1 = new OpenFileDialog(); openFileDialog1.Filter = "*.tlog|*.tlog"; @@ -40,7 +40,7 @@ namespace ArdupilotMega { try { - double[] ans = getOffsets(openFileDialog1.FileName); + double[] ans = getOffsets(openFileDialog1.FileName, throttleThreshold); SaveOffsets(ans); } @@ -53,9 +53,9 @@ namespace ArdupilotMega /// /// Filename /// Offsets - public static double[] getOffsets(string fn) + public static double[] getOffsets(string fn, int throttleThreshold = 0) { - // based of tridge's work + // based off tridge's work string logfile = fn; // old method @@ -76,6 +76,9 @@ namespace ArdupilotMega Hashtable filter = new Hashtable(); + // track data to use + bool useData = false; + log.Info("Start log: " + DateTime.Now); MAVLink mine = new MAVLink(); @@ -95,6 +98,19 @@ namespace ArdupilotMega if (packet == null) continue; + if (packet.GetType() == typeof(MAVLink.__mavlink_vfr_hud_t)) + { + if (((MAVLink.__mavlink_vfr_hud_t)packet).throttle >= throttleThreshold) + { + useData = true; + } + else + { + useData = false; + } + + } + if (packet.GetType() == typeof(MAVLink.__mavlink_sensor_offsets_t)) { offset = new Tuple( @@ -102,7 +118,7 @@ namespace ArdupilotMega ((MAVLink.__mavlink_sensor_offsets_t)packet).mag_ofs_y, ((MAVLink.__mavlink_sensor_offsets_t)packet).mag_ofs_z); } - else if (packet.GetType() == typeof(MAVLink.__mavlink_raw_imu_t)) + else if (packet.GetType() == typeof(MAVLink.__mavlink_raw_imu_t) && useData) { int div = 20; diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 8fc86cf6a3..ecf883be85 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -72,6 +72,7 @@ namespace ArdupilotMega GCSViews.FlightData FlightData; GCSViews.FlightPlanner FlightPlanner; GCSViews.Configuration Configuration; + //GCSViews.ConfigurationView.Configuration Configuration; GCSViews.Simulation Simulation; GCSViews.Firmware Firmware; GCSViews.Terminal Terminal; @@ -117,7 +118,7 @@ namespace ArdupilotMega comPort.BaseStream.BaudRate = 115200; - CMB_serialport.Items.AddRange(GetPortNames()); + CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); CMB_serialport.Items.Add("TCP"); CMB_serialport.Items.Add("UDP"); if (CMB_serialport.Items.Count > 0) @@ -188,14 +189,14 @@ namespace ArdupilotMega if (config["CMB_rateattitude"] != null) MainV2.cs.rateattitude = byte.Parse(config["CMB_rateattitude"].ToString()); - if (config["CMB_rateattitude"] != null) + if (config["rateposition"] != null) MainV2.cs.rateposition = byte.Parse(config["CMB_rateposition"].ToString()); - if (config["CMB_rateattitude"] != null) + if (config["CMB_ratestatus"] != null) MainV2.cs.ratestatus = byte.Parse(config["CMB_ratestatus"].ToString()); - if (config["CMB_rateattitude"] != null) + if (config["CMB_raterc"] != null) MainV2.cs.raterc = byte.Parse(config["CMB_raterc"].ToString()); if (config["CMB_ratesensors"] != null) - MainV2.cs.raterc = byte.Parse(config["CMB_ratesensors"].ToString()); + MainV2.cs.ratesensors = byte.Parse(config["CMB_ratesensors"].ToString()); if (config["speechenable"] != null) MainV2.speechEnable = bool.Parse(config["speechenable"].ToString()); @@ -253,57 +254,6 @@ namespace ArdupilotMega splash.Close(); } - private string[] GetPortNames() - { - string[] monoDevs = new string[0]; - - log.Debug("Getting Comports"); - - if (MONO) - { - if (Directory.Exists("/dev/")) - { - if (Directory.Exists("/dev/serial/by-id/")) - monoDevs = Directory.GetFiles("/dev/serial/by-id/", "*"); - monoDevs = Directory.GetFiles("/dev/", "*ACM*"); - monoDevs = Directory.GetFiles("/dev/", "ttyUSB*"); - } - } - - string[] ports = SerialPort.GetPortNames() - .Select(p => p.TrimEnd()) - .Select(FixBlueToothPortNameBug) - .ToArray(); - - string[] allPorts = new string[monoDevs.Length + ports.Length]; - - monoDevs.CopyTo(allPorts, 0); - ports.CopyTo(allPorts, monoDevs.Length); - - return allPorts; - } - - // .NET bug: sometimes bluetooth ports are enumerated with bogus characters - // eg 'COM10' becomes 'COM10c' - one workaround is to remove the non numeric - // char. Annoyingly, sometimes a numeric char is added, which means this - // does not work in all cases. - // See http://connect.microsoft.com/VisualStudio/feedback/details/236183/system-io-ports-serialport-getportnames-error-with-bluetooth - private string FixBlueToothPortNameBug(string portName) - { - if (!portName.StartsWith("COM")) - return portName; - var newPortName = "COM"; // Start over with "COM" - foreach (var portChar in portName.Substring(3).ToCharArray()) // Remove "COM", put the rest in a character array - { - if (char.IsDigit(portChar)) - newPortName += portChar.ToString(); // Good character, append to portName - else - log.WarnFormat("Bad (Non Numeric) character in port name '{0}' - removing", portName); - } - - return newPortName; - } - internal void ScreenShot() { Rectangle bounds = Screen.GetBounds(Point.Empty); @@ -313,7 +263,7 @@ namespace ArdupilotMega { g.CopyFromScreen(Point.Empty, Point.Empty, bounds.Size); } - string name = "ss" + DateTime.Now.ToString("hhmmss") + ".jpg"; + string name = "ss" + DateTime.Now.ToString("HHmmss") + ".jpg"; bitmap.Save(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + name, System.Drawing.Imaging.ImageFormat.Jpeg); CustomMessageBox.Show("Screenshot saved to " + name); } @@ -324,7 +274,7 @@ namespace ArdupilotMega { string oldport = CMB_serialport.Text; CMB_serialport.Items.Clear(); - CMB_serialport.Items.AddRange(GetPortNames()); + CMB_serialport.Items.AddRange(SerialPort.GetPortNames()); CMB_serialport.Items.Add("TCP"); CMB_serialport.Items.Add("UDP"); if (CMB_serialport.Items.Contains(oldport)) @@ -390,6 +340,7 @@ namespace ArdupilotMega } Configuration = new GCSViews.Configuration(); + //Configuration = new GCSViews.ConfigurationView.Configuration(); UserControl temp = Configuration; @@ -507,6 +458,9 @@ namespace ArdupilotMega if (comPort.logfile != null) comPort.logfile.Close(); + if (comPort.rawlogfile != null) + comPort.rawlogfile.Close(); + comPort.BaseStream.DtrEnable = false; comPort.Close(); } @@ -538,20 +492,33 @@ namespace ArdupilotMega comPort.BaseStream.StopBits = (StopBits)Enum.Parse(typeof(StopBits), "1"); comPort.BaseStream.Parity = (Parity)Enum.Parse(typeof(Parity), "None"); - comPort.BaseStream.DtrEnable = false; - try { + comPort.BaseStream.PortName = CMB_serialport.Text; + + // false here + comPort.BaseStream.DtrEnable = false; + comPort.BaseStream.RtsEnable = false; + + if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true) + comPort.BaseStream.toggleDTR(); + + // if reset on connect is on dtr will be true here + if (comPort.logfile != null) comPort.logfile.Close(); + + if (comPort.rawlogfile != null) + comPort.rawlogfile.Close(); try { Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.Read)); + + comPort.rawlogfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".rlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.Read)); } catch { CustomMessageBox.Show("Failed to create log - wont log this session"); } // soft fail - comPort.BaseStream.PortName = CMB_serialport.Text; comPort.Open(true); if (comPort.param["SYSID_SW_TYPE"] != null) @@ -1928,6 +1895,9 @@ namespace ArdupilotMega comPort.logreadmode = false; if (comPort.logfile != null) comPort.logfile.Close(); + + if (comPort.rawlogfile != null) + comPort.rawlogfile.Close(); } catch { } diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 36e972eff2..73f5cfac07 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -36,9 +36,15 @@ namespace ArdupilotMega List selection = new List(); + Hashtable data = new Hashtable(); + public MavlinkLog() { InitializeComponent(); + + zg1.GraphPane.YAxis.Title.IsVisible = false; + zg1.GraphPane.Title.IsVisible = true; + zg1.GraphPane.Title.Text = "Mavlink Log Graph"; } private void writeKML(string filename) @@ -328,7 +334,6 @@ namespace ArdupilotMega zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream zipStream.Close(); - flightdata.Clear(); } static void AddNamespace(Element element, string prefix, string uri) @@ -377,7 +382,7 @@ namespace ArdupilotMega CurrentState cs = new CurrentState(); - float oldlatlngalt = 0; + float oldlatlngsum = 0; int appui = 0; @@ -408,15 +413,15 @@ namespace ArdupilotMega } catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting. - if ((float)(cs.lat + cs.lng) != oldlatlngalt + if ((float)(cs.lat + cs.lng) != oldlatlngsum && cs.lat != 0 && cs.lng != 0) { - Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng) + "!=" + oldlatlngalt); + Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng) + "!=" + oldlatlngsum); CurrentState cs2 = (CurrentState)cs.Clone(); flightdata.Add(cs2); - oldlatlngalt = (cs.lat + cs.lng); + oldlatlngsum = (cs.lat + cs.lng); } } @@ -426,14 +431,52 @@ namespace ArdupilotMega Application.DoEvents(); + writeGPX(logfile); writeKML(logfile + ".kml"); + flightdata.Clear(); + progressBar1.Value = 100; } } } + private void writeGPX(string filename) + { + System.Xml.XmlTextWriter xw = new System.Xml.XmlTextWriter(Path.GetDirectoryName(filename) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(filename) + ".gpx", Encoding.ASCII); + + xw.WriteStartElement("gpx"); + + xw.WriteStartElement("trk"); + + xw.WriteStartElement("trkseg"); + + foreach (CurrentState cs in flightdata) + { + xw.WriteStartElement("trkpt"); + xw.WriteAttributeString("lat", cs.lat.ToString(new System.Globalization.CultureInfo("en-US"))); + xw.WriteAttributeString("lon", cs.lng.ToString(new System.Globalization.CultureInfo("en-US"))); + + xw.WriteElementString("ele", cs.alt.ToString(new System.Globalization.CultureInfo("en-US"))); + xw.WriteElementString("time", cs.datetime.ToString("yyyy-MM-ddTHH:mm:sszzzzzz")); + xw.WriteElementString("course", (cs.yaw).ToString(new System.Globalization.CultureInfo("en-US"))); + + xw.WriteElementString("roll", cs.roll.ToString(new System.Globalization.CultureInfo("en-US"))); + xw.WriteElementString("pitch", cs.pitch.ToString(new System.Globalization.CultureInfo("en-US"))); + //xw.WriteElementString("speed", mod.model.Orientation.); + //xw.WriteElementString("fix", cs.altitude); + + xw.WriteEndElement(); + } + + xw.WriteEndElement(); + xw.WriteEndElement(); + xw.WriteEndElement(); + + xw.Close(); + } + public static Color HexStringToColor(string hexColor) { string hc = (hexColor); @@ -549,7 +592,7 @@ namespace ArdupilotMega zg1.GraphPane.CurveList.Clear(); - GetLogFileData(zg1, openFileDialog1.FileName, fields); + //GetLogFileData(zg1, openFileDialog1.FileName, fields); try { @@ -587,19 +630,10 @@ namespace ArdupilotMega Random rand = new Random(); - int step = 0; - - // setup display and arrays + // setup arrays for (int a = 0; a < lookforfields.Count; a++) { lists[a] = new PointPairList(); - - LineItem myCurve; - - int colorvalue = ColourValues[step % ColourValues.Length]; - step++; - - myCurve = zg1.GraphPane.AddCurve(lookforfields[a].Replace("__mavlink_", ""), lists[a], Color.FromArgb(unchecked( colorvalue + (int)0xff000000)), SymbolType.None); } { @@ -694,6 +728,35 @@ namespace ArdupilotMega progressBar1.Value = 100; } + + int step = 0; + + zg1.GraphPane.AddY2Axis("PWM"); + zg1.GraphPane.AddY2Axis("Angle"); + + //zg1.GraphPane.XAxis.Title.Text = "Seconds"; + + // setup display and arrays + for (int a = 0; a < lookforfields.Count; a++) + { + LineItem myCurve; + + int colorvalue = ColourValues[step % ColourValues.Length]; + step++; + + myCurve = zg1.GraphPane.AddCurve(lookforfields[a].Replace("__mavlink_", ""), lists[a], Color.FromArgb(unchecked(colorvalue + (int)0xff000000)), SymbolType.None); + + double xMin, xMax, yMin, yMax; + + myCurve.GetRange(out xMin, out xMax, out yMin, out yMax, true, false, zg1.GraphPane); + + if (yMin > 900 && yMax < 2100) + { + myCurve.IsY2Axis = true; + myCurve.YAxisIndex = 0; + zg1.GraphPane.Y2Axis.IsVisible = true; + } + } } private List GetLogFileValidFields(string logfile) @@ -701,23 +764,36 @@ namespace ArdupilotMega Form selectform = SelectDataToGraphForm(); Hashtable seenIt = new Hashtable(); + + selection = new List(); + + List options = new List(); + + this.data.Clear(); + + colorStep = 0; { - MAVLink mine = new MAVLink(); - mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); - mine.logreadmode = true; + MAVLink MavlinkInterface = new MAVLink(); + MavlinkInterface.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + MavlinkInterface.logreadmode = true; - mine.packets.Initialize(); // clear + MavlinkInterface.packets.Initialize(); // clear - while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) + // to get first packet time + MavlinkInterface.readPacket(); + + DateTime startlogtime = MavlinkInterface.lastlogread; + + while (MavlinkInterface.logplaybackfile.BaseStream.Position < MavlinkInterface.logplaybackfile.BaseStream.Length) { - progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f); - this.Refresh(); + progressBar1.Value = (int)((float)MavlinkInterface.logplaybackfile.BaseStream.Position / (float)MavlinkInterface.logplaybackfile.BaseStream.Length * 100.0f); + progressBar1.Refresh(); - byte[] packet = mine.readPacket(); + byte[] packet = MavlinkInterface.readPacket(); - object data = mine.DebugPacket(packet, false); + object data = MavlinkInterface.DebugPacket(packet, false); Type test = data.GetType(); @@ -735,18 +811,67 @@ namespace ArdupilotMega { if (!seenIt.ContainsKey(field.DeclaringType.Name + "." + field.Name)) { - AddDataOption(selectform, field.Name + " " + field.DeclaringType.Name); seenIt[field.DeclaringType.Name + "." + field.Name] = 1; + //AddDataOption(selectform, field.Name + " " + field.DeclaringType.Name); + options.Add(field.DeclaringType.Name + "." + field.Name); + } + + if (!this.data.ContainsKey(field.Name + " " + field.DeclaringType.Name)) + this.data[field.Name + " " + field.DeclaringType.Name] = new PointPairList(); + + PointPairList list = ((PointPairList)this.data[field.Name + " " + field.DeclaringType.Name]); + + object value = fieldValue; + // seconds scale + double time = (MavlinkInterface.lastlogread - startlogtime).TotalMilliseconds / 1000.0; + + if (value.GetType() == typeof(Single)) + { + list.Add(time, (Single)field.GetValue(data)); + } + else if (value.GetType() == typeof(short)) + { + list.Add(time, (short)field.GetValue(data)); + } + else if (value.GetType() == typeof(ushort)) + { + list.Add(time, (ushort)field.GetValue(data)); + } + else if (value.GetType() == typeof(byte)) + { + list.Add(time, (byte)field.GetValue(data)); + } + else if (value.GetType() == typeof(Int32)) + { + list.Add(time, (Int32)field.GetValue(data)); } } } } - mine.logreadmode = false; - mine.logplaybackfile.Close(); - mine.logplaybackfile = null; + MavlinkInterface.logreadmode = false; + MavlinkInterface.logplaybackfile.Close(); + MavlinkInterface.logplaybackfile = null; - selectform.ShowDialog(); + try + { + + addMagField(ref options); + + } + catch (Exception ex) { log.Info(ex.ToString()); } + + // custom sort based on packet name + //options.Sort(delegate(string c1, string c2) { return String.Compare(c1.Substring(0,c1.IndexOf('.')),c2.Substring(0,c2.IndexOf('.')));}); + + // this needs sorting + foreach (string item in options) + { + var items = item.Split('.'); + AddDataOption(selectform, items[1] + " " + items[0]); + } + + selectform.Show(); progressBar1.Value = 100; @@ -755,6 +880,33 @@ namespace ArdupilotMega return selection; } + void addMagField(ref List options) + { + string field = "mag_field Custom"; + + options.Add("Custom.mag_field"); + + this.data[field] = new PointPairList(); + + PointPairList list = ((PointPairList)this.data[field]); + + PointPairList listx = ((PointPairList)this.data["xmag __mavlink_raw_imu_t"]); + PointPairList listy = ((PointPairList)this.data["ymag __mavlink_raw_imu_t"]); + PointPairList listz = ((PointPairList)this.data["zmag __mavlink_raw_imu_t"]); + + //(float)Math.Sqrt(Math.Pow(mx, 2) + Math.Pow(my, 2) + Math.Pow(mz, 2)); + + for (int a = 0; a < listx.Count; a++) + { + + double ans = Math.Sqrt(Math.Pow(listx[a].Y, 2) + Math.Pow(listy[a].Y, 2) + Math.Pow(listz[a].Y, 2)); + + //Console.WriteLine("{0} {1} {2} {3}", ans, listx[a].Y, listy[a].Y, listz[a].Y); + + list.Add(listx[a].X, ans); + } + } + private void AddDataOption(Form selectform, string Name) { @@ -784,16 +936,69 @@ namespace ArdupilotMega } } + int colorStep = 0; + void chk_box_CheckedChanged(object sender, EventArgs e) { if (((CheckBox)sender).Checked) { selection.Add(((CheckBox)sender).Name); + + LineItem myCurve; + + int colorvalue = ColourValues[colorStep % ColourValues.Length]; + colorStep++; + + myCurve = zg1.GraphPane.AddCurve(((CheckBox)sender).Name.Replace("__mavlink_", ""), (PointPairList)data[((CheckBox)sender).Name], Color.FromArgb(unchecked(colorvalue + (int)0xff000000)), SymbolType.None); + + myCurve.Tag = ((CheckBox)sender).Name; + + if (myCurve.Tag.ToString() == "roll __mavlink_attitude_t" || + myCurve.Tag.ToString() == "pitch __mavlink_attitude_t" || + myCurve.Tag.ToString() == "yaw __mavlink_attitude_t") + { + PointPairList ppl = new PointPairList((PointPairList)data[((CheckBox)sender).Name]); + for (int a = 0; a < ppl.Count; a++) + { + ppl[a].Y = ppl[a].Y * (180.0 / Math.PI); + } + + myCurve.Points = ppl; + } + + double xMin, xMax, yMin, yMax; + + myCurve.GetRange(out xMin, out xMax, out yMin, out yMax, true, false, zg1.GraphPane); + + if (yMin > 900 && yMax < 2100 && yMin < 2100) + { + myCurve.IsY2Axis = true; + myCurve.YAxisIndex = 0; + zg1.GraphPane.Y2Axis.IsVisible = true; + } } else { selection.Remove(((CheckBox)sender).Name); + foreach (var item in zg1.GraphPane.CurveList) + { + if (item.Tag.ToString() == ((CheckBox)sender).Name) + { + zg1.GraphPane.CurveList.Remove(item); + break; + } + } } + + try + { + // fix new line types + ThemeManager.ApplyThemeTo(this); + + zg1.Invalidate(); + zg1.AxisChange(); + } + catch { } } int x = 10; @@ -806,7 +1011,8 @@ namespace ArdupilotMega Name = "select", Width = 50, Height = 500, - Text = "Graph This" + Text = "Graph This", + TopLevel = true }; x = 10; diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.resx b/Tools/ArdupilotMegaPlanner/MavlinkLog.resx index 58503a2863..9406a8df5f 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.resx +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.resx @@ -123,7 +123,7 @@ - 45, 12 + 143, 12 116, 23 @@ -133,7 +133,7 @@ 8 - Create KML + Create KML + GPX BUT_redokml @@ -154,7 +154,7 @@ 10, 42 - 432, 26 + 622, 26 9 @@ -178,7 +178,7 @@ NoControl - 167, 12 + 265, 12 116, 23 @@ -208,7 +208,7 @@ NoControl - 289, 12 + 387, 12 116, 23 @@ -238,7 +238,7 @@ 10, 74 - 434, 293 + 624, 293 12 @@ -262,7 +262,7 @@ 6, 13 - 456, 379 + 646, 379 diff --git a/Tools/ArdupilotMegaPlanner/PIDTunning.cs b/Tools/ArdupilotMegaPlanner/PIDTunning.cs new file mode 100644 index 0000000000..866fe583bb --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/PIDTunning.cs @@ -0,0 +1,46 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace ArdupilotMega +{ + class PIDTunning + { + + public static void twiddle(double[] initialgains, Func run, double tol = 0.001) + { + int n_params = 3; + double err= 0; + double[] dparams = initialgains; //{1.0f,1.0f,1.0f}; + double[] paramss = {0.0f,0.0f,0.0f}; + double best_error = run(paramss); + int n = 0; + + while (dparams.Sum() > tol) { + for (int i = 0; i < n_params; i++){ + paramss[i] += dparams[i]; + err = run(paramss); + if (err < best_error){ + best_error = err; + dparams[i] *= 1.1; + } + else { + paramss[i] -= 2.0 * dparams[i]; + err = run(paramss); + if (err < best_error){ + best_error = err; + dparams[i] *= 1.1; + } + else { + paramss[i] += dparams[i]; + dparams[i] *= 0.9; + } + } + n += 1; + Console.WriteLine("Twiddle #" + n + " " + paramss.ToString() + " -> " + best_error); + } + } + } + } +} diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index e99d7dd1d3..f695ace3a6 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.1.55")] +[assembly: AssemblyFileVersion("1.1.59")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs index 41b6ab6aad..0adca95f80 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs @@ -30,7 +30,7 @@ { this.components = new System.ComponentModel.Container(); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup)); - this.tabControl1 = new System.Windows.Forms.TabControl(); + this.Tabs = new System.Windows.Forms.TabControl(); this.tabRadioIn = new System.Windows.Forms.TabPage(); this.groupBoxElevons = new System.Windows.Forms.GroupBox(); this.CHK_mixmode = new System.Windows.Forms.CheckBox(); @@ -82,6 +82,7 @@ this.CMB_fmode1 = new System.Windows.Forms.ComboBox(); this.BUT_SaveModes = new ArdupilotMega.MyButton(); this.tabHardware = new System.Windows.Forms.TabPage(); + this.BUT_MagCalibration = new ArdupilotMega.MyButton(); this.label27 = new System.Windows.Forms.Label(); this.CMB_sonartype = new System.Windows.Forms.ComboBox(); this.CHK_enableoptflow = new System.Windows.Forms.CheckBox(); @@ -115,6 +116,9 @@ this.TXT_battcapacity = new System.Windows.Forms.TextBox(); this.CMB_batmontype = new System.Windows.Forms.ComboBox(); this.pictureBox5 = new System.Windows.Forms.PictureBox(); + this.tabArduplane = new System.Windows.Forms.TabPage(); + this.label48 = new System.Windows.Forms.Label(); + this.BUT_levelplane = new ArdupilotMega.MyButton(); this.tabArducopter = new System.Windows.Forms.TabPage(); this.label28 = new System.Windows.Forms.Label(); this.label16 = new System.Windows.Forms.Label(); @@ -179,8 +183,7 @@ this.tabReset = new System.Windows.Forms.TabPage(); this.BUT_reset = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); - this.BUT_MagCalibration = new ArdupilotMega.MyButton(); - this.tabControl1.SuspendLayout(); + this.Tabs.SuspendLayout(); this.tabRadioIn.SuspendLayout(); this.groupBoxElevons.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); @@ -193,6 +196,7 @@ this.tabBattery.SuspendLayout(); this.groupBox4.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).BeginInit(); + this.tabArduplane.SuspendLayout(); this.tabArducopter.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit(); @@ -208,18 +212,19 @@ this.tabReset.SuspendLayout(); this.SuspendLayout(); // - // tabControl1 + // Tabs // - this.tabControl1.Controls.Add(this.tabRadioIn); - this.tabControl1.Controls.Add(this.tabModes); - this.tabControl1.Controls.Add(this.tabHardware); - this.tabControl1.Controls.Add(this.tabBattery); - this.tabControl1.Controls.Add(this.tabArducopter); - this.tabControl1.Controls.Add(this.tabHeli); - resources.ApplyResources(this.tabControl1, "tabControl1"); - this.tabControl1.Name = "tabControl1"; - this.tabControl1.SelectedIndex = 0; - this.tabControl1.SelectedIndexChanged += new System.EventHandler(this.tabControl1_SelectedIndexChanged); + this.Tabs.Controls.Add(this.tabRadioIn); + this.Tabs.Controls.Add(this.tabModes); + this.Tabs.Controls.Add(this.tabHardware); + this.Tabs.Controls.Add(this.tabBattery); + this.Tabs.Controls.Add(this.tabArduplane); + this.Tabs.Controls.Add(this.tabArducopter); + this.Tabs.Controls.Add(this.tabHeli); + resources.ApplyResources(this.Tabs, "Tabs"); + this.Tabs.Name = "Tabs"; + this.Tabs.SelectedIndex = 0; + this.Tabs.SelectedIndexChanged += new System.EventHandler(this.tabControl1_SelectedIndexChanged); // // tabRadioIn // @@ -657,7 +662,7 @@ // // tabHardware // - this.tabHardware.BackColor = System.Drawing.SystemColors.Control; + this.tabHardware.BackColor = System.Drawing.Color.Transparent; this.tabHardware.Controls.Add(this.BUT_MagCalibration); this.tabHardware.Controls.Add(this.label27); this.tabHardware.Controls.Add(this.CMB_sonartype); @@ -675,6 +680,13 @@ resources.ApplyResources(this.tabHardware, "tabHardware"); this.tabHardware.Name = "tabHardware"; // + // BUT_MagCalibration + // + resources.ApplyResources(this.BUT_MagCalibration, "BUT_MagCalibration"); + this.BUT_MagCalibration.Name = "BUT_MagCalibration"; + this.BUT_MagCalibration.UseVisualStyleBackColor = true; + this.BUT_MagCalibration.Click += new System.EventHandler(this.BUT_MagCalibration_Click); + // // label27 // resources.ApplyResources(this.label27, "label27"); @@ -924,6 +936,26 @@ this.pictureBox5.Name = "pictureBox5"; this.pictureBox5.TabStop = false; // + // tabArduplane + // + this.tabArduplane.Controls.Add(this.label48); + this.tabArduplane.Controls.Add(this.BUT_levelplane); + resources.ApplyResources(this.tabArduplane, "tabArduplane"); + this.tabArduplane.Name = "tabArduplane"; + this.tabArduplane.UseVisualStyleBackColor = true; + // + // label48 + // + resources.ApplyResources(this.label48, "label48"); + this.label48.Name = "label48"; + // + // BUT_levelplane + // + resources.ApplyResources(this.BUT_levelplane, "BUT_levelplane"); + this.BUT_levelplane.Name = "BUT_levelplane"; + this.BUT_levelplane.UseVisualStyleBackColor = true; + this.BUT_levelplane.Click += new System.EventHandler(this.BUT_levelplane_Click); + // // tabArducopter // this.tabArducopter.Controls.Add(this.label28); @@ -1584,23 +1616,16 @@ this.BUT_reset.UseVisualStyleBackColor = true; this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click); // - // BUT_MagCalibration - // - resources.ApplyResources(this.BUT_MagCalibration, "BUT_MagCalibration"); - this.BUT_MagCalibration.Name = "BUT_MagCalibration"; - this.BUT_MagCalibration.UseVisualStyleBackColor = true; - this.BUT_MagCalibration.Click += new System.EventHandler(this.BUT_MagCalibration_Click); - // // Setup // resources.ApplyResources(this, "$this"); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.Controls.Add(this.tabControl1); + this.Controls.Add(this.Tabs); this.FormBorderStyle = System.Windows.Forms.FormBorderStyle.SizableToolWindow; this.Name = "Setup"; this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.Setup_FormClosing); this.Load += new System.EventHandler(this.Setup_Load); - this.tabControl1.ResumeLayout(false); + this.Tabs.ResumeLayout(false); this.tabRadioIn.ResumeLayout(false); this.tabRadioIn.PerformLayout(); this.groupBoxElevons.ResumeLayout(false); @@ -1619,6 +1644,8 @@ this.groupBox4.ResumeLayout(false); this.groupBox4.PerformLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox5)).EndInit(); + this.tabArduplane.ResumeLayout(false); + this.tabArduplane.PerformLayout(); this.tabArducopter.ResumeLayout(false); this.tabArducopter.PerformLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).EndInit(); @@ -1644,7 +1671,7 @@ #endregion - private System.Windows.Forms.TabControl tabControl1; + private System.Windows.Forms.TabControl Tabs; private System.Windows.Forms.TabPage tabRadioIn; private HorizontalProgressBar2 BAR8; private HorizontalProgressBar2 BAR7; @@ -1794,6 +1821,9 @@ private System.Windows.Forms.RadioButton H1_ENABLE; private System.Windows.Forms.RadioButton CCPM; private MyButton BUT_MagCalibration; + private System.Windows.Forms.TabPage tabArduplane; + private System.Windows.Forms.Label label48; + private MyButton BUT_levelplane; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 0b71ca9ea9..1bb21aee79 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -100,7 +100,7 @@ namespace ArdupilotMega.Setup fmodelist[no].BackColor = Color.Green; - if (tabControl1.SelectedTab == tabHeli) + if (Tabs.SelectedTab == tabHeli) { if (MainV2.comPort.param["HSV_MAN"] == null || MainV2.comPort.param["HSV_MAN"].ToString() == "0") return; @@ -312,7 +312,7 @@ namespace ArdupilotMega.Setup int monosux = 0; monosux *= 5; - if (tabControl1.SelectedTab == tabRadioIn) + if (Tabs.SelectedTab == tabRadioIn) { startup = true; @@ -340,7 +340,7 @@ namespace ArdupilotMega.Setup startup = false; } - if (tabControl1.SelectedTab == tabModes) + if (Tabs.SelectedTab == tabModes) { if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) // APM { @@ -416,7 +416,7 @@ namespace ArdupilotMega.Setup } } - if (tabControl1.SelectedTab == tabHardware) + if (Tabs.SelectedTab == tabHardware) { startup = true; @@ -442,7 +442,7 @@ namespace ArdupilotMega.Setup startup = false; } - if (tabControl1.SelectedTab == tabBattery) + if (Tabs.SelectedTab == tabBattery) { startup = true; bool not_supported = false; @@ -501,7 +501,7 @@ namespace ArdupilotMega.Setup startup = false; } - if (tabControl1.SelectedTab == tabArducopter) + if (Tabs.SelectedTab == tabArducopter) { if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) { @@ -510,7 +510,7 @@ namespace ArdupilotMega.Setup } } - if (tabControl1.SelectedTab == tabHeli) + if (Tabs.SelectedTab == tabHeli) { if (MainV2.comPort.param["GYR_ENABLE"] == null) { @@ -1437,7 +1437,7 @@ namespace ArdupilotMega.Setup timer.Stop(); timer.Dispose(); - tabControl1.SelectedIndex = 0; + Tabs.SelectedIndex = 0; // mono runs validation on all controls on exit. try and skip it startup = true; @@ -1697,7 +1697,34 @@ namespace ArdupilotMega.Setup } else { - MagCalib.ProcessLog(); + string minthro = "30"; + Common.InputBox("Min Throttle", "Use only data above this throttle percent.", ref minthro); + + int ans = 0; + int.TryParse(minthro, out ans); + + MagCalib.ProcessLog(ans); + } + } + + private void BUT_levelplane_Click(object sender, EventArgs e) + { + try + { + MainV2.comPort.setParam("MANUAL_LEVEL",1); + +#if MAVLINK10 + int fixme; // needs to be accel only + MainV2.comPort.doCommand(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION,1,1,1,1,1,1,1); +#else + MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC); +#endif + + BUT_levelac2.Text = "Complete"; + } + catch + { + CustomMessageBox.Show("Failed to level : AP 2.32+ is required"); } } } diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx index 16be1cd25e..0524d1cf2e 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx @@ -310,7 +310,7 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs 0 @@ -682,23 +682,11 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs 1 - - 405, 25 - - - 75, 23 - - - 33 - - - Calibration - BUT_MagCalibration @@ -711,21 +699,6 @@ 0 - - NoControl - - - 510, 57 - - - 150, 20 - - - 32 - - - in Degrees eg 2° 3' W is -2.3 - label27 @@ -738,24 +711,6 @@ 1 - - XL-EZ0 - - - LV-EZ0 - - - XL-EZL0 - - - 308, 134 - - - 121, 21 - - - 31 - CMB_sonartype @@ -768,21 +723,6 @@ 2 - - NoControl - - - 162, 297 - - - 134, 19 - - - 30 - - - Enable Optical Flow - CHK_enableoptflow @@ -795,21 +735,6 @@ 3 - - Zoom - - - NoControl - - - 78, 271 - - - 75, 75 - - - 29 - pictureBox2 @@ -822,24 +747,6 @@ 4 - - True - - - NoControl - - - 390, 80 - - - 104, 13 - - - 28 - - - Declination WebSite - linkLabelmagdec @@ -852,21 +759,6 @@ 5 - - NoControl - - - 305, 57 - - - 72, 16 - - - 23 - - - Declination - label100 @@ -879,18 +771,6 @@ 6 - - 383, 57 - - - 121, 20 - - - 20 - - - Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3 - TXT_declination @@ -903,21 +783,6 @@ 7 - - NoControl - - - 162, 214 - - - 103, 17 - - - 24 - - - Enable Airspeed - CHK_enableairspeed @@ -930,21 +795,6 @@ 8 - - NoControl - - - 159, 136 - - - 90, 17 - - - 25 - - - Enable Sonar - CHK_enablesonar @@ -957,21 +807,6 @@ 9 - - NoControl - - - 162, 56 - - - 105, 17 - - - 27 - - - Enable Compass - CHK_enablecompass @@ -984,21 +819,6 @@ 10 - - Zoom - - - NoControl - - - 78, 188 - - - 75, 75 - - - 3 - pictureBox4 @@ -1011,21 +831,6 @@ 11 - - Zoom - - - NoControl - - - 78, 106 - - - 75, 75 - - - 2 - pictureBox3 @@ -1038,27 +843,6 @@ 12 - - Zoom - - - - - - NoControl - - - - - - 78, 25 - - - 75, 75 - - - 0 - pictureBox1 @@ -1093,7 +877,7 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs 2 @@ -1228,11 +1012,95 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs 3 + + True + + + NoControl + + + 228, 170 + + + 212, 13 + + + 11 + + + Level your plane to set default accel offsets + + + label48 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArduplane + + + 0 + + + NoControl + + + 285, 199 + + + 75, 23 + + + 10 + + + Level + + + BUT_levelplane + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + + + tabArduplane + + + 1 + + + 4, 22 + + + 3, 3, 3, 3 + + + 666, 393 + + + 7 + + + ArduPlane + + + tabArduplane + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + Tabs + + + 4 + label28 @@ -1324,10 +1192,10 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs - 4 + 5 groupBox5 @@ -1792,33 +1660,33 @@ System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - tabControl1 + Tabs - 5 + 6 - + Fill - + 0, 0 - + 674, 419 - + 93 - - tabControl1 + + Tabs - + System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + $this - + 0 @@ -3180,6 +3048,390 @@ 28 + + 405, 25 + + + 75, 23 + + + 33 + + + Calibration + + + BUT_MagCalibration + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + + + tabHardware + + + 0 + + + NoControl + + + 510, 57 + + + 150, 20 + + + 32 + + + in Degrees eg 2° 3' W is -2.3 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 1 + + + XL-EZ0 + + + LV-EZ0 + + + XL-EZL0 + + + 308, 134 + + + 121, 21 + + + 31 + + + CMB_sonartype + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 2 + + + NoControl + + + 162, 297 + + + 134, 19 + + + 30 + + + Enable Optical Flow + + + CHK_enableoptflow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 3 + + + Zoom + + + NoControl + + + 78, 271 + + + 75, 75 + + + 29 + + + pictureBox2 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 4 + + + True + + + NoControl + + + 390, 80 + + + 104, 13 + + + 28 + + + Declination WebSite + + + linkLabelmagdec + + + System.Windows.Forms.LinkLabel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 5 + + + NoControl + + + 305, 57 + + + 72, 16 + + + 23 + + + Declination + + + label100 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 6 + + + 383, 57 + + + 121, 20 + + + 20 + + + Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3 + + + TXT_declination + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 7 + + + NoControl + + + 162, 214 + + + 103, 17 + + + 24 + + + Enable Airspeed + + + CHK_enableairspeed + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 8 + + + NoControl + + + 159, 136 + + + 90, 17 + + + 25 + + + Enable Sonar + + + CHK_enablesonar + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 9 + + + NoControl + + + 162, 56 + + + 105, 17 + + + 27 + + + Enable Compass + + + CHK_enablecompass + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 10 + + + Zoom + + + NoControl + + + 78, 188 + + + 75, 75 + + + 3 + + + pictureBox4 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 11 + + + Zoom + + + NoControl + + + 78, 106 + + + 75, 75 + + + 2 + + + pictureBox3 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 12 + + + Zoom + + + + + + NoControl + + + + + + 78, 25 + + + 75, 75 + + + 0 + + + pictureBox1 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 13 + label31 diff --git a/Tools/ArdupilotMegaPlanner/ThemeManager.cs b/Tools/ArdupilotMegaPlanner/ThemeManager.cs index 47bf8b5c24..859cd8e7d4 100644 --- a/Tools/ArdupilotMegaPlanner/ThemeManager.cs +++ b/Tools/ArdupilotMegaPlanner/ThemeManager.cs @@ -1,6 +1,7 @@ using System; using System.Drawing; using System.Windows.Forms; +using ArdupilotMega.Controls.BackstageView; using log4net; namespace ArdupilotMega @@ -123,12 +124,17 @@ namespace ArdupilotMega zg1.GraphPane.XAxis.MinorTic.Color = Color.White; zg1.GraphPane.YAxis.MajorTic.Color = Color.White; zg1.GraphPane.YAxis.MinorTic.Color = Color.White; + zg1.GraphPane.Y2Axis.MajorTic.Color = Color.White; + zg1.GraphPane.Y2Axis.MinorTic.Color = Color.White; zg1.GraphPane.XAxis.MajorGrid.Color = Color.White; zg1.GraphPane.YAxis.MajorGrid.Color = Color.White; + zg1.GraphPane.Y2Axis.MajorGrid.Color = Color.White; zg1.GraphPane.YAxis.Scale.FontSpec.FontColor = Color.White; zg1.GraphPane.YAxis.Title.FontSpec.FontColor = Color.White; + zg1.GraphPane.Y2Axis.Title.FontSpec.FontColor = Color.White; + zg1.GraphPane.Y2Axis.Scale.FontSpec.FontColor = Color.White; zg1.GraphPane.XAxis.Scale.FontSpec.FontColor = Color.White; zg1.GraphPane.XAxis.Title.FontSpec.FontColor = Color.White; @@ -220,6 +226,18 @@ namespace ArdupilotMega LNK.VisitedLinkColor = TextColor; } + else if (ctl.GetType() == typeof(BackstageView)) + { + var bsv = ctl as BackstageView; + + bsv.BackColor = BGColor; + bsv.ButtonsAreaBgColor = ControlBGColor; + bsv.HighlightColor2 = Color.FromArgb(0x94, 0xc1, 0x1f); + bsv.HighlightColor1 = Color.FromArgb(0x40, 0x57, 0x04); + bsv.SelectedTextColor = Color.White; + bsv.UnSelectedTextColor = Color.Gray; + bsv.ButtonsAreaPencilColor = Color.DarkGray; + } else if (ctl.GetType() == typeof(HorizontalProgressBar2) || ctl.GetType() == typeof(VerticalProgressBar2)) { ((HorizontalProgressBar2)ctl).BackgroundColor = ControlBGColor; diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb index 10a92a6c02..3a1d3667b7 100644 Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml index 3964602e0a..973da2116f 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml @@ -93,6 +93,190 @@ FlightMode Thr Cruise + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/m3u/networklink.kml b/Tools/ArdupilotMegaPlanner/bin/Release/m3u/networklink.kml index 0f28d211d2..7504af4016 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/m3u/networklink.kml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/m3u/networklink.kml @@ -11,8 +11,8 @@ http://127.0.0.1:56781/network.kml onInterval - 1 - 1 + 0.3 + 0.3 diff --git a/Tools/ArdupilotMegaPlanner/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/dataflashlog.xml index 3964602e0a..973da2116f 100644 --- a/Tools/ArdupilotMegaPlanner/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/dataflashlog.xml @@ -93,6 +93,190 @@ FlightMode Thr Cruise + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + + + Err + P + I + D + Output + Gain + diff --git a/Tools/ArdupilotMegaPlanner/georefimage.cs b/Tools/ArdupilotMegaPlanner/georefimage.cs index a6dc599094..bf877efaec 100644 --- a/Tools/ArdupilotMegaPlanner/georefimage.cs +++ b/Tools/ArdupilotMegaPlanner/georefimage.cs @@ -26,7 +26,7 @@ namespace ArdupilotMega private TextBox TXT_outputlog; private MyButton BUT_estoffset; - int latpos = 5, lngpos = 4, altpos = 7; + int latpos = 4, lngpos = 5, altpos = 7; internal Georefimage() { InitializeComponent(); @@ -117,8 +117,8 @@ namespace ArdupilotMega // line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string - string[] vals = new string[] { "GPS", (new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0) - cs.datetime).TotalMilliseconds.ToString(), "1", - "8",cs.lat.ToString(),cs.lng.ToString(),"0.0",cs.alt.ToString(),cs.alt.ToString(),"0.0",cs.groundcourse.ToString()}; + string[] vals = new string[] { "GPS", (cs.datetime - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Local)).TotalMilliseconds.ToString(), "1", + cs.satcount.ToString(),cs.lat.ToString(),cs.lng.ToString(),"0.0",cs.alt.ToString(),cs.alt.ToString(),"0.0",cs.groundcourse.ToString()}; if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos] && oldvalues[lngpos] == vals[lngpos] @@ -182,6 +182,8 @@ namespace ArdupilotMega Document kml = new Document(); + StreamWriter sw4 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "loglocation.csv"); + StreamWriter sw3 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.kml"); StreamWriter sw2 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.txt"); @@ -239,25 +241,35 @@ namespace ArdupilotMega first++; } - //Console.Write("ph " + dt + " log " + crap + " \r"); + Console.Write("ph " + dt + " log " + crap + " \r"); - if (dt.Equals(crap)) + sw4.WriteLine("ph " + file + " " + dt + " log " + crap); + + if (dt.ToString("yyyy-MM-ddTHH:mm:ss") == crap.ToString("yyyy-MM-ddTHH:mm:ss")) { TXT_outputlog.AppendText("MATCH Photo " + Path.GetFileNameWithoutExtension(file) + " " + dt + "\r\n"); matchs++; + SharpKml.Dom.Timestamp tstamp = new SharpKml.Dom.Timestamp(); + + tstamp.When = dt; + kml.AddFeature( new Placemark() { + Time = tstamp , Name = Path.GetFileNameWithoutExtension(file), Geometry = new SharpKml.Dom.Point() { - Coordinate = new Vector(double.Parse(arr[lngpos]), double.Parse(arr[latpos]), double.Parse(arr[altpos])) + Coordinate = new Vector(double.Parse(arr[latpos]), double.Parse(arr[lngpos]), double.Parse(arr[altpos])) } + } ); + + sw2.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[lngpos] + " " + arr[latpos] + " " + arr[altpos]); sw.WriteLine(Path.GetFileNameWithoutExtension(file) + "\t" + crap.ToString("yyyy:MM:dd HH:mm:ss") + "\t" + arr[lngpos] + "\t" + arr[latpos] + "\t" + arr[altpos]); sw.Flush(); @@ -277,6 +289,7 @@ namespace ArdupilotMega sw3.Write(serializer.Xml); sw3.Close(); + sw4.Close(); sw2.Close(); sw.Close(); @@ -345,7 +358,7 @@ namespace ArdupilotMega this.TXT_offsetseconds.Name = "TXT_offsetseconds"; this.TXT_offsetseconds.Size = new System.Drawing.Size(100, 20); this.TXT_offsetseconds.TabIndex = 4; - this.TXT_offsetseconds.Text = "0"; + this.TXT_offsetseconds.Text = "-86158"; // // BUT_doit // @@ -389,7 +402,7 @@ namespace ArdupilotMega this.BUT_estoffset.UseVisualStyleBackColor = true; this.BUT_estoffset.Click += new System.EventHandler(this.BUT_estoffset_Click); // - // georefimage + // Georefimage // this.ClientSize = new System.Drawing.Size(453, 299); this.Controls.Add(this.BUT_estoffset); diff --git a/Tools/ArdupilotMegaPlanner/m3u/networklink.kml b/Tools/ArdupilotMegaPlanner/m3u/networklink.kml index 0f28d211d2..7504af4016 100644 --- a/Tools/ArdupilotMegaPlanner/m3u/networklink.kml +++ b/Tools/ArdupilotMegaPlanner/m3u/networklink.kml @@ -11,8 +11,8 @@ http://127.0.0.1:56781/network.kml onInterval - 1 - 1 + 0.3 + 0.3 diff --git a/Tools/VARTest/VARTest.pde b/Tools/VARTest/VARTest.pde index e8b07b862c..dac394a010 100644 --- a/Tools/VARTest/VARTest.pde +++ b/Tools/VARTest/VARTest.pde @@ -28,10 +28,7 @@ #include #include #include - -#if 0 #include -#endif static Parameters g; diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm index 45742f3a72..c687de355a 100644 --- a/Tools/autotest/ArduCopter.parm +++ b/Tools/autotest/ArduCopter.parm @@ -1,7 +1,6 @@ FRAME 0 MAG_ENABLE 1 LOG_BITMASK 4095 -COMPASS_DEC 0.211 RC1_MAX 2000.000000 RC1_MIN 1000.000000 RC1_TRIM 1500.000000 diff --git a/Tools/autotest/ArduPlane.parm b/Tools/autotest/ArduPlane.parm index 7a95108056..bad1c82b82 100644 --- a/Tools/autotest/ArduPlane.parm +++ b/Tools/autotest/ArduPlane.parm @@ -9,7 +9,6 @@ ARSPD_FBW_MAX 30 ARSPD_FBW_MIN 10 KFF_RDDRMIX 0.5 KFF_PTCHCOMP 0.3 -COMPASS_DEC 0.211 THR_MAX 100 HDNG2RLL_D 0.5 HDNG2RLL_I 0.01 diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py index af1d786853..08aaaa8f40 100644 --- a/Tools/autotest/pysim/aircraft.py +++ b/Tools/autotest/pysim/aircraft.py @@ -21,14 +21,20 @@ class Aircraft(object): self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down self.position = Vector3(0, 0, 0) # m North, East, Down - self.last_velocity = self.velocity.copy() self.mass = 0.0 self.update_frequency = 50 # in Hz self.gravity = 9.8 # m/s/s - self.accelerometer = Vector3(0, 0, self.gravity) + self.accelerometer = Vector3(0, 0, -self.gravity) self.wind = util.Wind('0,0,0') + def on_ground(self, position=None): + '''return true if we are on the ground''' + if position is None: + position = self.position + return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height + + def update_position(self, delta_time): '''update lat/lon/alt from position''' @@ -41,8 +47,11 @@ class Aircraft(object): self.altitude = self.home_altitude - self.position.z - # work out what the accelerometer would see - self.accelerometer = ((self.velocity - self.last_velocity)/delta_time) + Vector3(0,0,self.gravity) -# self.accelerometer = Vector3(0,0,-self.gravity) - self.accelerometer = self.dcm.transposed() * self.accelerometer - self.last_velocity = self.velocity.copy() + velocity_body = self.dcm.transposed() * self.velocity + + # force the acceleration to mostly be from gravity. We should be using 100% accel_body, + # but right now that flies very badly as the AHRS system can't do centripetal correction + # for multicopters. This is a compromise until we get that sorted out + accel_true = self.accel_body + accel_fake = self.dcm.transposed() * Vector3(0, 0, -self.gravity) + self.accelerometer = (accel_true * 0.5) + (accel_fake * 0.5) diff --git a/Tools/autotest/pysim/multicopter.py b/Tools/autotest/pysim/multicopter.py index 1eca2c62a1..3447189271 100755 --- a/Tools/autotest/pysim/multicopter.py +++ b/Tools/autotest/pysim/multicopter.py @@ -143,6 +143,15 @@ class MultiCopter(Aircraft): # add in some wind accel_earth += self.wind.accel(self.velocity) + # if we're on the ground, then our vertical acceleration is limited + # to zero. This effectively adds the force of the ground on the aircraft + if self.on_ground() and accel_earth.z > 0: + accel_earth.z = 0 + + # work out acceleration as seen by the accelerometers. It sees the kinematic + # acceleration (ie. real movement), plus gravity + self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity)) + # new velocity vector self.velocity += accel_earth * delta_time @@ -151,8 +160,8 @@ class MultiCopter(Aircraft): self.position += self.velocity * delta_time # constrain height to the ground - if (-self.position.z) + self.home_altitude < self.ground_level + self.frame_height: - if (-old_position.z) + self.home_altitude > self.ground_level + self.frame_height: + if self.on_ground(): + if not self.on_ground(old_position): print("Hit ground at %f m/s" % (self.velocity.z)) self.velocity = Vector3(0, 0, 0) diff --git a/Tools/autotest/pysim/rotmat.py b/Tools/autotest/pysim/rotmat.py index f108c8818d..c7ecc6ae65 100644 --- a/Tools/autotest/pysim/rotmat.py +++ b/Tools/autotest/pysim/rotmat.py @@ -29,19 +29,19 @@ class Vector3: '''a vector''' def __init__(self, x=None, y=None, z=None): if x != None and y != None and z != None: - self.x = x - self.y = y - self.z = z + self.x = float(x) + self.y = float(y) + self.z = float(z) elif x != None and len(x) == 3: - self.x = x[0] - self.y = x[1] - self.z = x[2] + self.x = float(x[0]) + self.y = float(x[1]) + self.z = float(x[2]) elif x != None: raise ValueError('bad initialiser') else: - self.x = 0 - self.y = 0 - self.z = 0 + self.x = float(0) + self.y = float(0) + self.z = float(0) def __repr__(self): return 'Vector3(%.2f, %.2f, %.2f)' % (self.x, diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index 3425521504..803e5f1e2c 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/Tools/autotest/pysim/sim_multicopter.py @@ -46,7 +46,7 @@ def sim_send(m, a): buf = struct.pack('<16dI', a.latitude, a.longitude, a.altitude, degrees(yaw), a.velocity.x, a.velocity.y, - -a.accelerometer.x, -a.accelerometer.y, -a.accelerometer.z, + a.accelerometer.x, a.accelerometer.y, a.accelerometer.z, degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z), degrees(roll), degrees(pitch), degrees(yaw), math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 7bfea34fd8..36a467e302 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -44,6 +44,10 @@ class APM_RC_Class virtual void enable_out(uint8_t) = 0; virtual void disable_out(uint8_t) = 0; + virtual void Force_Out0_Out1(void) = 0; + virtual void Force_Out2_Out3(void) = 0; + virtual void Force_Out6_Out7(void) = 0; + protected: uint16_t _map_speed(uint16_t speed_hz) { return 2000000UL / speed_hz; } diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 29a694ae08..7a925e6351 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -228,7 +228,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, float *result) // 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]) / (float)count[i]; + result[i] = sum[i] / (float)count[i]; } // return number of microseconds since last call diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 323d1bb338..b6fa2e1f45 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -495,25 +495,27 @@ AP_AHRS_DCM::euler_angles(void) // average error_roll_pitch since last call float AP_AHRS_DCM::get_error_rp(void) { - float ret; if (_error_rp_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_rp_last; } - ret = _error_rp_sum / _error_rp_count; + _error_rp_last = _error_rp_sum / _error_rp_count; _error_rp_sum = 0; _error_rp_count = 0; - return ret; + return _error_rp_last; } // average error_yaw since last call float AP_AHRS_DCM::get_error_yaw(void) { - float ret; if (_error_yaw_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_yaw_last; } - ret = _error_yaw_sum / _error_yaw_count; + _error_yaw_last = _error_yaw_sum / _error_yaw_count; _error_yaw_sum = 0; _error_yaw_count = 0; - return ret; + return _error_yaw_last; } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index ee5b3403ac..5952380c7e 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -75,8 +75,10 @@ private: uint16_t _renorm_val_count; float _error_rp_sum; uint16_t _error_rp_count; + float _error_rp_last; float _error_yaw_sum; uint16_t _error_yaw_count; + float _error_yaw_last; // time in millis when we last got a GPS heading uint32_t _gps_last_update; diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp index 03fe02869d..5148aa878f 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp @@ -383,30 +383,34 @@ void AP_AHRS_Quaternion::update(void) } -// average error in roll/pitch since last call +/* reporting of Quaternion state for MAVLink */ + +// average error_roll_pitch since last call float AP_AHRS_Quaternion::get_error_rp(void) { - float ret; if (_error_rp_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_rp_last; } - ret = _error_rp_sum / _error_rp_count; + _error_rp_last = _error_rp_sum / _error_rp_count; _error_rp_sum = 0; _error_rp_count = 0; - return ret; + return _error_rp_last; } -// average error in yaw since last call +// average error_yaw since last call float AP_AHRS_Quaternion::get_error_yaw(void) { - float ret; if (_error_yaw_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_yaw_last; } - ret = _error_yaw_sum / _error_yaw_count; + _error_yaw_last = _error_yaw_sum / _error_yaw_count; _error_yaw_sum = 0; _error_yaw_count = 0; - return ret; + return _error_yaw_last; } // reset attitude system diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.h b/libraries/AP_AHRS/AP_AHRS_Quaternion.h index a5b6583903..91c41e2ea4 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.h +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.h @@ -87,8 +87,10 @@ private: // estimate of error float _error_rp_sum; uint16_t _error_rp_count; + float _error_rp_last; float _error_yaw_sum; uint16_t _error_yaw_count; + float _error_yaw_last; }; #endif diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index a07833e7fb..c60e022cbe 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -38,7 +38,7 @@ class AP_Baro_BMP085 : public AP_Baro int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; uint16_t ac4, ac5, ac6; - AverageFilterInt16_Size4 _temp_filter; + AverageFilterInt32_Size4 _temp_filter; uint32_t _retry_time; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index ad84d9d03a..ef1181bdf5 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -7,6 +7,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { AP_GROUPINFO("DEC", 2, Compass, _declination), AP_GROUPINFO("LEARN", 3, Compass, _learn), // true if learning calibration AP_GROUPINFO("USE", 4, Compass, _use_for_yaw), // true if used for DCM yaw + AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination), AP_GROUPEND }; @@ -21,6 +22,7 @@ Compass::Compass(void) : _use_for_yaw(1), _null_enable(false), _null_init_done(false), + _auto_declination(1), _orientation(ROTATION_NONE) { } @@ -57,23 +59,17 @@ Compass::get_offsets() return _offset; } -bool -Compass::set_initial_location(long latitude, long longitude, bool force) +void +Compass::set_initial_location(long latitude, long longitude) { - // If the user has choosen to use auto-declination regardless of the planner value - // OR - // If the declination failed to load from the EEPROM (ie. not set by user) - if(force || !_declination.load()) - { + // if automatic declination is configured, then compute + // the declination based on the initial GPS fix + if (_auto_declination) { // Set the declination based on the lat/lng from GPS - _declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000))); - - // Reset null offsets null_offsets_disable(); + _declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000))); null_offsets_enable(); - return true; } - return false; } void @@ -182,38 +178,101 @@ Compass::calculate(const Matrix3f &dcm_matrix) #endif } + +/* + this offset nulling algorithm is inspired by this paper from Bill Premerlani + + http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf + + The base algorithm works well, but is quite sensitive to + noise. After long discussions with Bill, the following changes were + made: + + 1) we keep a history buffer that effectively divides the mag + vectors into a set of N streams. The algorithm is run on the + streams separately + + 2) within each stream we only calculate a change when the mag + vector has changed by a significant amount. + + This gives us the property that we learn quickly if there is no + noise, but still learn correctly (and slowly) in the face of lots of + noise. + */ void -Compass::null_offsets(const Matrix3f &dcm_matrix) +Compass::null_offsets(void) { if (_null_enable == false || _learn == 0) { // auto-calibration is disabled return; } - // Update our estimate of the offsets in the magnetometer - Vector3f calc; - Matrix3f dcm_new_from_last; - float weight; + // this gain is set so we converge on the offsets in about 5 + // minutes with a 10Hz compass + const float gain = 0.01; + const float max_change = 10.0; + const float min_diff = 50.0; + Vector3f ofs; - Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z); - - if(_null_init_done) { - dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is. + ofs = _offset.get(); - weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z); - if (weight > .001) { - calc = mag_body_new + _mag_body_last; // Eq 11 from Bill P's paper - calc -= dcm_new_from_last * _mag_body_last; - calc -= dcm_new_from_last.transposed() * mag_body_new; - if(weight > 0.5) weight = 0.5; - calc = calc * (weight); - _offset.set(_offset.get() - calc); - } - } else { + if (!_null_init_done) { + // first time through _null_init_done = true; + for (uint8_t i=0; i<_mag_history_size; i++) { + // fill the history buffer with the current mag vector, + // with the offset removed + _mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); + } + _mag_history_index = 0; + return; } - _mag_body_last = mag_body_new - calc; - _last_dcm_matrix = dcm_matrix; + + Vector3f b1, b2, diff; + float length; + + // get a past element + b1 = Vector3f(_mag_history[_mag_history_index].x, + _mag_history[_mag_history_index].y, + _mag_history[_mag_history_index].z); + // the history buffer doesn't have the offsets + b1 += ofs; + + // get the current vector + b2 = Vector3f(mag_x, mag_y, mag_z); + + // calculate the delta for this sample + diff = b2 - b1; + length = diff.length(); + if (length < min_diff) { + // the mag vector hasn't changed enough - we don't get + // enough information from this vector to use it. + // Note that we don't put the current vector into the mag + // history here. We want to wait for a larger rotation to + // build up before calculating an offset change, as accuracy + // of the offset change is highly dependent on the size of the + // rotation. + _mag_history_index = (_mag_history_index + 1) % _mag_history_size; + return; + } + + // put the vector in the history + _mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); + _mag_history_index = (_mag_history_index + 1) % _mag_history_size; + + // equation 6 of Bills paper + diff = diff * (gain * (b2.length() - b1.length()) / length); + + // limit the change from any one reading. This is to prevent + // single crazy readings from throwing off the offsets for a long + // time + length = diff.length(); + if (length > max_change) { + diff *= max_change / length; + } + + // set the new offsets + _offset.set(_offset.get() - diff); } diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 770a520d60..8adc3ede13 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -23,7 +23,7 @@ public: float heading; ///< compass heading in radians float heading_x; ///< compass vector X magnitude float heading_y; ///< compass vector Y magnitude - uint32_t last_update; ///< millis() time of last update + uint32_t last_update; ///< micros() time of last update bool healthy; ///< true if last read OK /// Constructor @@ -81,13 +81,12 @@ public: /// virtual Vector3f &get_offsets(); - /// Sets the initial location used to get declination - Returns true if declination set + /// Sets the initial location used to get declination /// /// @param latitude GPS Latitude. /// @param longitude GPS Longitude. - /// @param force Force the compass declination update. /// - bool set_initial_location(long latitude, long longitude, bool force); + void set_initial_location(long latitude, long longitude); /// Program new offset values. /// @@ -97,11 +96,9 @@ public: /// void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } - /// Perform automatic offset updates using the results of the DCM matrix. + /// Perform automatic offset updates /// - /// @param dcm_matrix The DCM result matrix. - /// - void null_offsets(const Matrix3f &dcm_matrix); + void null_offsets(void); /// Enable/Start automatic offset updates @@ -131,10 +128,14 @@ protected: AP_Float _declination; AP_Int8 _learn; /// @@ -20,49 +20,97 @@ #include #include -static const int8_t dec_tbl[37][73] PROGMEM = \ -{{150,145,140,135,130,125,120,115,110,105,100,95,90,85,80,75,70,65,60,55,50,45,40,35,30,25,20,15,10,5,0,-4,-9,-14,-19,-24,-29,-34,-39,-44,-49,-54,-59,-64,-69,-74,-79,-84,-89,-94,-99,104,109,114,119,124,129,134,139,144,149,154,159,164,169,174,179,175,170,165,160,155,150}, \ -{143,137,131,126,120,115,110,105,100,95,90,85,80,75,71,66,62,57,53,48,44,39,35,31,27,22,18,14,9,5,1,-3,-7,-11,-16,-20,-25,-29,-34,-38,-43,-47,-52,-57,-61,-66,-71,-76,-81,-86,-91,-96,101,107,112,117,123,128,134,140,146,151,157,163,169,175,178,172,166,160,154,148,143}, \ -{130,124,118,112,107,101,96,92,87,82,78,74,70,65,61,57,54,50,46,42,38,34,31,27,23,19,16,12,8,4,1,-2,-6,-10,-14,-18,-22,-26,-30,-34,-38,-43,-47,-51,-56,-61,-65,-70,-75,-79,-84,-89,-94,100,105,111,116,122,128,135,141,148,155,162,170,177,174,166,159,151,144,137,130}, \ -{111,104,99,94,89,85,81,77,73,70,66,63,60,56,53,50,46,43,40,36,33,30,26,23,20,16,13,10,6,3,0,-3,-6,-9,-13,-16,-20,-24,-28,-32,-36,-40,-44,-48,-52,-57,-61,-65,-70,-74,-79,-84,-88,-93,-98,103,109,115,121,128,135,143,152,162,172,176,165,154,144,134,125,118,111}, \ -{85,81,77,74,71,68,65,63,60,58,56,53,51,49,46,43,41,38,35,32,29,26,23,19,16,13,10,7,4,1,-1,-3,-6,-9,-13,-16,-19,-23,-26,-30,-34,-38,-42,-46,-50,-54,-58,-62,-66,-70,-74,-78,-83,-87,-91,-95,100,105,110,117,124,133,144,159,178,160,141,125,112,103,96,90,85}, \ -{62,60,58,57,55,54,52,51,50,48,47,46,44,42,41,39,36,34,31,28,25,22,19,16,13,10,7,4,2,0,-3,-5,-8,-10,-13,-16,-19,-22,-26,-29,-33,-37,-41,-45,-49,-53,-56,-60,-64,-67,-70,-74,-77,-80,-83,-86,-89,-91,-94,-97,101,105,111,130,109,84,77,74,71,68,66,64,62}, \ -{46,46,45,44,44,43,42,42,41,41,40,39,38,37,36,35,33,31,28,26,23,20,16,13,10,7,4,1,-1,-3,-5,-7,-9,-12,-14,-16,-19,-22,-26,-29,-33,-36,-40,-44,-48,-51,-55,-58,-61,-64,-66,-68,-71,-72,-74,-74,-75,-74,-72,-68,-61,-48,-25,2,22,33,40,43,45,46,47,46,46}, \ -{36,36,36,36,36,35,35,35,35,34,34,34,34,33,32,31,30,28,26,23,20,17,14,10,6,3,0,-2,-4,-7,-9,-10,-12,-14,-15,-17,-20,-23,-26,-29,-32,-36,-40,-43,-47,-50,-53,-56,-58,-60,-62,-63,-64,-64,-63,-62,-59,-55,-49,-41,-30,-17,-4,6,15,22,27,31,33,34,35,36,36}, \ -{30,30,30,30,30,30,30,29,29,29,29,29,29,29,29,28,27,26,24,21,18,15,11,7,3,0,-3,-6,-9,-11,-12,-14,-15,-16,-17,-19,-21,-23,-26,-29,-32,-35,-39,-42,-45,-48,-51,-53,-55,-56,-57,-57,-56,-55,-53,-49,-44,-38,-31,-23,-14,-6,0,7,13,17,21,24,26,27,29,29,30}, \ -{25,25,26,26,26,25,25,25,25,25,25,25,25,26,25,25,24,23,21,19,16,12,8,4,0,-3,-7,-10,-13,-15,-16,-17,-18,-19,-20,-21,-22,-23,-25,-28,-31,-34,-37,-40,-43,-46,-48,-49,-50,-51,-51,-50,-48,-45,-42,-37,-32,-26,-19,-13,-7,-1,3,7,11,14,17,19,21,23,24,25,25}, \ -{21,22,22,22,22,22,22,22,22,22,22,22,22,22,22,22,21,20,18,16,13,9,5,1,-3,-7,-11,-14,-17,-18,-20,-21,-21,-22,-22,-22,-23,-23,-25,-27,-29,-32,-35,-37,-40,-42,-44,-45,-45,-45,-44,-42,-40,-36,-32,-27,-22,-17,-12,-7,-3,0,3,7,9,12,14,16,18,19,20,21,21}, \ -{18,19,19,19,19,19,19,19,19,19,19,19,19,19,19,19,18,17,16,14,10,7,2,-1,-6,-10,-14,-17,-19,-21,-22,-23,-24,-24,-24,-24,-23,-23,-23,-24,-26,-28,-30,-33,-35,-37,-38,-39,-39,-38,-36,-34,-31,-28,-24,-19,-15,-10,-6,-3,0,1,4,6,8,10,12,14,15,16,17,18,18}, \ -{16,16,17,17,17,17,17,17,17,17,17,16,16,16,16,16,16,15,13,11,8,4,0,-4,-9,-13,-16,-19,-21,-23,-24,-25,-25,-25,-25,-24,-23,-21,-20,-20,-21,-22,-24,-26,-28,-30,-31,-32,-31,-30,-29,-27,-24,-21,-17,-13,-9,-6,-3,-1,0,2,4,5,7,9,10,12,13,14,15,16,16}, \ -{14,14,14,15,15,15,15,15,15,15,14,14,14,14,14,14,13,12,11,9,5,2,-2,-6,-11,-15,-18,-21,-23,-24,-25,-25,-25,-25,-24,-22,-21,-18,-16,-15,-15,-15,-17,-19,-21,-22,-24,-24,-24,-23,-22,-20,-18,-15,-12,-9,-5,-3,-1,0,1,2,4,5,6,8,9,10,11,12,13,14,14}, \ -{12,13,13,13,13,13,13,13,13,13,13,13,12,12,12,12,11,10,9,6,3,0,-4,-8,-12,-16,-19,-21,-23,-24,-24,-24,-24,-23,-22,-20,-17,-15,-12,-10,-9,-9,-10,-12,-13,-15,-17,-17,-18,-17,-16,-15,-13,-11,-8,-5,-3,-1,0,1,1,2,3,4,6,7,8,9,10,11,12,12,12}, \ -{11,11,11,11,11,12,12,12,12,12,11,11,11,11,11,10,10,9,7,5,2,-1,-5,-9,-13,-17,-20,-22,-23,-23,-23,-23,-22,-20,-18,-16,-14,-11,-9,-6,-5,-4,-5,-6,-8,-9,-11,-12,-12,-12,-12,-11,-9,-8,-6,-3,-1,0,0,1,1,2,3,4,5,6,7,8,9,10,11,11,11}, \ -{10,10,10,10,10,10,10,10,10,10,10,10,10,10,9,9,9,7,6,3,0,-3,-6,-10,-14,-17,-20,-21,-22,-22,-22,-21,-19,-17,-15,-13,-10,-8,-6,-4,-2,-2,-2,-2,-4,-5,-7,-8,-8,-9,-8,-8,-7,-5,-4,-2,0,0,1,1,1,2,2,3,4,5,6,7,8,9,10,10,10}, -{9,9,9,9,9,9,9,10,10,9,9,9,9,9,9,8,8,6,5,2,0,-4,-7,-11,-15,-17,-19,-21,-21,-21,-20,-18,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,0,-1,-2,-4,-5,-5,-6,-6,-5,-5,-4,-3,-1,0,0,1,1,1,1,2,3,3,5,6,7,8,8,9,9,9}, \ -{9,9,9,9,9,9,9,9,9,9,9,9,8,8,8,8,7,5,4,1,-1,-5,-8,-12,-15,-17,-19,-20,-20,-19,-18,-16,-14,-11,-9,-7,-5,-4,-2,-1,0,0,1,1,0,0,-2,-3,-3,-4,-4,-4,-3,-3,-2,-1,0,0,0,0,0,1,1,2,3,4,5,6,7,8,8,9,9}, \ -{9,9,9,8,8,8,9,9,9,9,9,8,8,8,8,7,6,5,3,0,-2,-5,-9,-12,-15,-17,-18,-19,-19,-18,-16,-14,-12,-9,-7,-5,-4,-2,-1,0,0,1,1,1,1,0,0,-1,-2,-2,-3,-3,-2,-2,-1,-1,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8,9}, \ -{8,8,8,8,8,8,9,9,9,9,9,9,8,8,8,7,6,4,2,0,-3,-6,-9,-12,-15,-17,-18,-18,-17,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,2,2,1,0,0,-1,-1,-1,-2,-2,-1,-1,0,0,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8}, \ -{8,8,8,8,9,9,9,9,9,9,9,9,9,8,8,7,5,3,1,-1,-4,-7,-10,-13,-15,-16,-17,-17,-16,-15,-13,-11,-9,-6,-5,-3,-2,0,0,0,1,2,2,2,2,1,1,0,0,0,-1,-1,-1,-1,-1,0,0,0,0,-1,-1,-1,-1,-1,0,0,1,3,4,5,7,7,8}, \ -{8,8,9,9,9,9,10,10,10,10,10,10,10,9,8,7,5,3,0,-2,-5,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,3,3,2,2,1,0,0,0,0,0,0,0,0,0,0,-1,-1,-2,-2,-2,-2,-2,-1,0,0,1,3,4,6,7,8}, \ -{7,8,9,9,9,10,10,11,11,11,11,11,10,10,9,7,5,3,0,-2,-6,-9,-11,-13,-15,-16,-16,-15,-14,-13,-11,-9,-7,-5,-3,-2,0,0,1,1,2,3,3,3,3,2,2,1,1,0,0,0,0,0,0,0,-1,-1,-2,-3,-3,-4,-4,-4,-3,-2,-1,0,1,3,5,6,7}, \ -{6,8,9,9,10,11,11,12,12,12,12,12,11,11,9,7,5,2,0,-3,-7,-10,-12,-14,-15,-16,-15,-15,-13,-12,-10,-8,-7,-5,-3,-1,0,0,1,2,2,3,3,4,3,3,3,2,2,1,1,1,0,0,0,0,-1,-2,-3,-4,-4,-5,-5,-5,-5,-4,-2,-1,0,2,3,5,6}, \ -{6,7,8,10,11,12,12,13,13,14,14,13,13,11,10,8,5,2,0,-4,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-5,-3,-1,0,0,1,2,3,3,4,4,4,4,4,3,3,3,2,2,1,1,0,0,-1,-2,-3,-5,-6,-7,-7,-7,-6,-5,-4,-3,-1,0,2,4,6}, \ -{5,7,8,10,11,12,13,14,15,15,15,14,14,12,11,8,5,2,-1,-5,-9,-12,-14,-16,-17,-17,-16,-15,-14,-12,-11,-9,-7,-5,-3,-1,0,0,1,2,3,4,4,5,5,5,5,5,5,4,4,3,3,2,1,0,-1,-2,-4,-6,-7,-8,-8,-8,-8,-7,-6,-4,-2,0,1,3,5}, \ -{4,6,8,10,12,13,14,15,16,16,16,16,15,13,11,9,5,2,-2,-6,-10,-13,-16,-17,-18,-18,-17,-16,-15,-13,-11,-9,-7,-5,-4,-2,0,0,1,3,3,4,5,6,6,7,7,7,7,7,6,5,4,3,2,0,-1,-3,-5,-7,-8,-9,-10,-10,-10,-9,-7,-5,-4,-1,0,2,4}, \ -{4,6,8,10,12,14,15,16,17,18,18,17,16,15,12,9,5,1,-3,-8,-12,-15,-18,-19,-20,-20,-19,-18,-16,-15,-13,-11,-8,-6,-4,-2,-1,0,1,3,4,5,6,7,8,9,9,9,9,9,9,8,7,5,3,1,-1,-3,-6,-8,-10,-11,-12,-12,-11,-10,-9,-7,-5,-2,0,1,4}, \ -{4,6,8,11,13,15,16,18,19,19,19,19,18,16,13,10,5,0,-5,-10,-15,-18,-21,-22,-23,-22,-22,-20,-18,-17,-14,-12,-10,-8,-5,-3,-1,0,1,3,5,6,8,9,10,11,12,12,13,12,12,11,9,7,5,2,0,-3,-6,-9,-11,-12,-13,-13,-12,-11,-10,-8,-6,-3,-1,1,4}, \ -{3,6,9,11,14,16,17,19,20,21,21,21,19,17,14,10,4,-1,-8,-14,-19,-22,-25,-26,-26,-26,-25,-23,-21,-19,-17,-14,-12,-9,-7,-4,-2,0,1,3,5,7,9,11,13,14,15,16,16,16,16,15,13,10,7,4,0,-3,-7,-10,-12,-14,-15,-14,-14,-12,-11,-9,-6,-4,-1,1,3}, \ -{4,6,9,12,14,17,19,21,22,23,23,23,21,19,15,9,2,-5,-13,-20,-25,-28,-30,-31,-31,-30,-29,-27,-25,-22,-20,-17,-14,-11,-9,-6,-3,0,1,4,6,9,11,13,15,17,19,20,21,21,21,20,18,15,11,6,2,-2,-7,-11,-13,-15,-16,-16,-15,-13,-11,-9,-7,-4,-1,1,4}, \ -{4,7,10,13,15,18,20,22,24,25,25,25,23,20,15,7,-2,-12,-22,-29,-34,-37,-38,-38,-37,-36,-34,-31,-29,-26,-23,-20,-17,-13,-10,-7,-4,-1,2,5,8,11,13,16,18,21,23,24,26,26,26,26,24,21,17,12,5,0,-6,-10,-14,-16,-16,-16,-15,-14,-12,-10,-7,-4,-1,1,4}, \ -{4,7,10,13,16,19,22,24,26,27,27,26,24,19,11,-1,-15,-28,-37,-43,-46,-47,-47,-45,-44,-41,-39,-36,-32,-29,-26,-22,-19,-15,-11,-8,-4,-1,2,5,9,12,15,19,22,24,27,29,31,33,33,33,32,30,26,21,14,6,0,-6,-11,-14,-15,-16,-15,-14,-12,-9,-7,-4,-1,1,4}, \ -{6,9,12,15,18,21,23,25,27,28,27,24,17,4,-14,-34,-49,-56,-60,-60,-60,-58,-56,-53,-50,-47,-43,-40,-36,-32,-28,-25,-21,-17,-13,-9,-5,-1,2,6,10,14,17,21,24,28,31,34,37,39,41,42,43,43,41,38,33,25,17,8,0,-4,-8,-10,-10,-10,-8,-7,-4,-2,0,3,6}, \ -{22,24,26,28,30,32,33,31,23,-18,-81,-96,-99,-98,-95,-93,-89,-86,-82,-78,-74,-70,-66,-62,-57,-53,-49,-44,-40,-36,-32,-27,-23,-19,-14,-10,-6,-1,2,6,10,15,19,23,27,31,35,38,42,45,49,52,55,57,60,61,63,63,62,61,57,53,47,40,33,28,23,21,19,19,19,20,22}, -{168,173,178,176,171,166,161,156,151,146,141,136,131,126,121,116,111,106,101,-96,-91,-86,-81,-76,-71,-66,-61,-56,-51,-46,-41,-36,-31,-26,-21,-16,-11,-6,-1,3,8,13,18,23,28,33,38,43,48,53,58,63,68,73,78,83,88,93,98,103,108,113,118,123,128,133,138,143,148,153,158,163,168}}; + +// 1 byte - 4 bits for value + 1 bit for sign + 3 bits for repeats => 8 bits +struct row_value{ + + // Offset has a max value of 15 + uint8_t abs_offset:4; + + // Sign of the offset, 0 = negative, 1 = positive + uint8_t offset_sign:1; + + // The highest repeat is 7 + uint8_t repeats:3; +}; + +// 730 bytes +static const uint8_t exceptions[10][73] PROGMEM = \ +{ \ +{150,145,140,135,130,125,120,115,110,105,100,95,90,85,80,75,70,65,60,55,50,45,40,35,30,25,20,15,10,5,0,4,9,14,19,24,29,34,39,44,49,54,59,64,69,74,79,84,89,94,99,104,109,114,119,124,129,134,139,144,149,154,159,164,169,174,179,175,170,165,160,155,150}, \ +{143,137,131,126,120,115,110,105,100,95,90,85,80,75,71,66,62,57,53,48,44,39,35,31,27,22,18,14,9,5,1,3,7,11,16,20,25,29,34,38,43,47,52,57,61,66,71,76,81,86,91,96,101,107,112,117,123,128,134,140,146,151,157,163,169,175,178,172,166,160,154,148,143}, \ +{130,124,118,112,107,101,96,92,87,82,78,74,70,65,61,57,54,50,46,42,38,34,31,27,23,19,16,12,8,4,1,2,6,10,14,18,22,26,30,34,38,43,47,51,56,61,65,70,75,79,84,89,94,100,105,111,116,122,128,135,141,148,155,162,170,177,174,166,159,151,144,137,130}, \ +{111,104,99,94,89,85,81,77,73,70,66,63,60,56,53,50,46,43,40,36,33,30,26,23,20,16,13,10,6,3,0,3,6,9,13,16,20,24,28,32,36,40,44,48,52,57,61,65,70,74,79,84,88,93,98,103,109,115,121,128,135,143,152,162,172,176,165,154,144,134,125,118,111}, \ +{85,81,77,74,71,68,65,63,60,58,56,53,51,49,46,43,41,38,35,32,29,26,23,19,16,13,10,7,4,1,1,3,6,9,13,16,19,23,26,30,34,38,42,46,50,54,58,62,66,70,74,78,83,87,91,95,100,105,110,117,124,133,144,159,178,160,141,125,112,103,96,90,85}, \ +{62,60,58,57,55,54,52,51,50,48,47,46,44,42,41,39,36,34,31,28,25,22,19,16,13,10,7,4,2,0,3,5,8,10,13,16,19,22,26,29,33,37,41,45,49,53,56,60,64,67,70,74,77,80,83,86,89,91,94,97,101,105,111,130,109,84,77,74,71,68,66,64,62}, \ +{46,46,45,44,44,43,42,42,41,41,40,39,38,37,36,35,33,31,28,26,23,20,16,13,10,7,4,1,1,3,5,7,9,12,14,16,19,22,26,29,33,36,40,44,48,51,55,58,61,64,66,68,71,72,74,74,75,74,72,68,61,48,25,2,22,33,40,43,45,46,47,46,46}, \ +{6,9,12,15,18,21,23,25,27,28,27,24,17,4,14,34,49,56,60,60,60,58,56,53,50,47,43,40,36,32,28,25,21,17,13,9,5,1,2,6,10,14,17,21,24,28,31,34,37,39,41,42,43,43,41,38,33,25,17,8,0,4,8,10,10,10,8,7,4,2,0,3,6}, \ +{22,24,26,28,30,32,33,31,23,18,81,96,99,98,95,93,89,86,82,78,74,70,66,62,57,53,49,44,40,36,32,27,23,19,14,10,6,1,2,6,10,15,19,23,27,31,35,38,42,45,49,52,55,57,60,61,63,63,62,61,57,53,47,40,33,28,23,21,19,19,19,20,22}, \ +{168,173,178,176,171,166,161,156,151,146,141,136,131,126,121,116,111,106,101,96,91,86,81,76,71,66,61,56,51,46,41,36,31,26,21,16,11,6,1,3,8,13,18,23,28,33,38,43,48,53,58,63,68,73,78,83,88,93,98,103,108,113,118,123,128,133,138,143,148,153,158,163,168} \ +}; + +// 100 bytes +static const uint8_t exception_signs[10][10] PROGMEM = \ +{ \ +{0,0,0,1,255,255,224,0,0,0}, \ +{0,0,0,1,255,255,240,0,0,0}, \ +{0,0,0,1,255,255,248,0,0,0}, \ +{0,0,0,1,255,255,254,0,0,0}, \ +{0,0,0,3,255,255,255,0,0,0}, \ +{0,0,0,3,255,255,255,240,0,0}, \ +{0,0,0,15,255,255,255,254,0,0}, \ +{0,3,255,255,252,0,0,7,252,0}, \ +{0,127,255,255,252,0,0,0,0,0}, \ +{0,0,31,255,254,0,0,0,0,0} \ +}; + +// 76 bytes +static const uint8_t declination_keys[2][37] PROGMEM = \ +{ \ +// Row start values +{36,30,25,21,18,16,14,12,11,10,9,9,9,8,8,8,7,6,6,5,4,4,4,3,4,4,4}, \ +// Row length values +{39,38,33,35,37,35,37,36,39,34,41,42,42,28,39,40,43,51,50,39,37,34,44,51,49,48,55} \ +}; + +// 1056 total values @ 1 byte each = 1056 bytes +static const row_value declination_values[] PROGMEM = \ +{ \ +{0,0,4},{1,1,0},{0,0,2},{1,1,0},{0,0,2},{1,1,3},{2,1,1},{3,1,3},{4,1,1},{3,1,1},{2,1,1},{3,1,0},{2,1,0},{1,1,0},{2,1,1},{1,1,0},{2,1,0},{3,1,4},{4,1,1},{3,1,0},{4,1,0},{3,1,2},{2,1,2},{1,1,1},{0,0,0},{1,0,1},{3,0,0},{4,0,0},{6,0,0},{8,0,0},{11,0,0},{13,0,1},{10,0,0},{9,0,0},{7,0,0},{5,0,0},{4,0,0},{2,0,0},{1,0,2}, \ +{0,0,6},{1,1,0},{0,0,6},{1,1,2},{2,1,0},{3,1,2},{4,1,2},{3,1,3},{2,1,0},{1,1,0},{2,1,0},{1,1,2},{2,1,2},{3,1,3},{4,1,0},{3,1,3},{2,1,1},{1,1,1},{0,0,0},{1,0,1},{2,0,0},{4,0,0},{5,0,0},{6,0,0},{7,0,0},{8,0,0},{9,0,0},{8,0,0},{6,0,0},{7,0,0},{6,0,0},{4,0,1},{3,0,0},{2,0,0},{1,0,0},{2,0,0},{0,0,0},{1,0,0}, \ +{0,0,1},{1,0,0},{0,0,1},{1,1,0},{0,0,6},{1,0,0},{1,1,0},{0,0,0},{1,1,1},{2,1,1},{3,1,0},{4,1,3},{3,1,0},{4,1,0},{3,1,1},{2,1,0},{1,1,7},{2,1,0},{3,1,6},{2,1,0},{1,1,2},{0,0,0},{1,0,0},{2,0,0},{3,0,1},{5,0,1},{6,0,0},{7,0,0},{6,0,2},{4,0,2},{3,0,1},{2,0,2},{1,0,1}, \ +{0,0,0},{1,0,0},{0,0,7},{0,0,5},{1,1,1},{2,1,1},{3,1,0},{4,1,5},{3,1,1},{1,1,0},{2,1,0},{1,1,0},{0,0,0},{1,1,0},{0,0,1},{1,1,0},{0,0,0},{2,1,2},{3,1,1},{2,1,0},{3,1,0},{2,1,1},{1,1,0},{0,0,1},{1,0,0},{2,0,1},{4,0,1},{5,0,4},{4,0,0},{3,0,1},{4,0,0},{2,0,0},{3,0,0},{2,0,2},{1,0,2}, \ +{0,0,0},{1,0,0},{0,0,7},{0,0,5},{1,1,2},{2,1,0},{4,1,0},{3,1,0},{5,1,0},{3,1,0},{5,1,0},{4,1,1},{3,1,0},{2,1,1},{1,1,2},{0,0,2},{1,0,0},{0,0,1},{1,1,0},{2,1,2},{3,1,0},{2,1,1},{1,1,1},{0,0,0},{1,0,0},{2,0,1},{3,0,1},{4,0,0},{5,0,0},{4,0,0},{5,0,0},{4,0,0},{3,0,1},{1,0,0},{3,0,0},{2,0,4},{1,0,3}, \ +{0,0,1},{1,0,0},{0,0,7},{1,1,0},{0,0,4},{1,1,0},{2,1,1},{3,1,0},{4,1,2},{5,1,0},{4,1,0},{3,1,1},{2,1,1},{1,1,1},{0,0,2},{1,0,1},{2,0,0},{1,0,0},{0,0,0},{1,1,1},{2,1,3},{1,1,1},{1,0,2},{2,0,0},{3,0,1},{4,0,2},{3,0,1},{2,0,0},{1,0,0},{2,0,1},{1,0,0},{2,0,1},{1,0,0},{2,0,0},{1,0,3}, \ +{0,0,2},{1,0,0},{0,0,5},{1,1,0},{0,0,4},{1,1,2},{2,1,0},{4,1,0},{3,1,0},{4,1,1},{5,1,0},{4,1,0},{3,1,1},{2,1,0},{1,1,1},{0,0,2},{1,0,0},{2,0,0},{1,0,0},{3,0,0},{2,0,0},{1,0,0},{0,0,1},{2,1,2},{1,1,0},{2,1,0},{0,0,1},{1,0,1},{2,0,1},{3,0,2},{4,0,0},{2,0,1},{1,0,2},{2,0,0},{1,0,1},{2,0,0},{1,0,5}, \ +{0,0,0},{1,0,0},{0,0,7},{0,0,1},{1,1,0},{0,0,2},{1,1,2},{3,1,2},{4,1,3},{3,1,0},{2,1,1},{1,1,0},{0,0,2},{1,0,1},{2,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,0},{1,0,0},{0,0,0},{1,1,0},{2,1,0},{1,1,0},{2,1,1},{0,0,0},{1,1,0},{1,0,2},{2,0,1},{3,0,1},{2,0,1},{1,0,1},{0,0,0},{1,0,2},{2,0,0},{1,0,5}, \ +{0,0,4},{1,0,0},{0,0,3},{1,1,0},{0,0,3},{1,1,0},{0,0,0},{1,1,0},{2,1,1},{3,1,1},{4,1,3},{3,1,0},{2,1,0},{1,1,0},{0,0,2},{1,0,0},{2,0,3},{3,0,0},{2,0,0},{3,0,0},{1,0,1},{1,1,1},{2,1,0},{1,1,0},{2,1,0},{1,1,0},{0,0,2},{1,0,0},{2,0,0},{1,0,0},{2,0,0},{3,0,0},{2,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,0},{1,0,7},{1,0,1}, \ +{0,0,7},{0,0,5},{1,1,0},{0,0,1},{2,1,0},{1,1,0},{3,1,3},{4,1,1},{3,1,1},{1,1,1},{0,0,1},{1,0,0},{2,0,3},{3,0,0},{2,0,3},{0,0,2},{2,1,0},{1,1,0},{2,1,0},{1,1,0},{0,0,0},{1,1,0},{1,0,0},{0,0,0},{1,0,0},{2,0,0},{1,0,0},{2,0,1},{0,0,0},{1,0,0},{0,0,1},{1,0,0},{0,0,0},{1,0,7}, \ +{0,0,6},{1,0,0},{0,0,0},{1,1,0},{0,0,4},{1,1,0},{0,0,0},{2,1,0},{1,1,0},{3,1,0},{2,1,0},{4,1,0},{3,1,0},{4,1,1},{2,1,2},{0,0,1},{1,0,0},{2,0,7},{2,0,0},{1,0,1},{0,0,1},{1,1,1},{2,1,0},{1,1,0},{0,0,0},{1,1,0},{0,0,0},{1,0,0},{0,0,0},{1,0,1},{2,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,2},{1,0,1},{0,0,0},{2,0,0},{1,0,2},{0,0,0},{1,0,0}, \ +{0,0,7},{0,0,3},{1,1,0},{0,0,2},{1,1,0},{2,1,0},{1,1,0},{3,1,0},{2,1,0},{4,1,0},{3,1,0},{4,1,0},{3,1,0},{2,1,1},{1,1,0},{0,0,0},{1,0,1},{2,0,1},{3,0,0},{2,0,2},{1,0,0},{2,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,0},{1,1,0},{0,0,0},{2,1,0},{1,1,0},{0,0,0},{1,1,0},{0,0,1},{1,0,0},{0,0,0},{1,0,2},{0,0,3},{1,0,0},{0,0,0},{1,0,6},{0,0,0},{1,0,0}, \ +{0,0,2},{1,1,0},{0,0,1},{1,0,0},{0,0,3},{1,1,0},{0,0,2},{1,1,2},{2,1,0},{3,1,0},{2,1,0},{3,1,0},{4,1,0},{3,1,1},{2,1,0},{1,1,1},{0,0,0},{1,0,0},{2,0,2},{3,0,0},{2,0,1},{1,0,0},{2,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,2},{1,1,0},{0,0,0},{1,1,1},{0,0,0},{1,1,0},{0,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,5},{1,0,7},{0,0,0},{1,0,0}, \ +{0,0,5},{1,0,0},{0,0,4},{1,1,0},{0,0,1},{1,1,1},{2,1,2},{3,1,4},{2,1,0},{1,1,0},{0,0,0},{1,0,1},{2,0,6},{1,0,1},{0,0,0},{1,0,1},{0,0,2},{1,1,1},{0,0,0},{1,1,0},{0,0,1},{1,1,0},{0,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,7},{1,0,7}, \ +{0,0,3},{1,0,0},{0,0,7},{1,1,0},{0,0,0},{1,1,0},{2,1,3},{3,1,3},{2,1,0},{1,1,1},{0,0,0},{1,0,1},{2,0,2},{3,0,0},{1,0,0},{2,0,0},{1,0,0},{2,0,0},{0,0,1},{1,0,1},{0,0,2},{1,1,0},{0,0,0},{1,1,0},{0,0,1},{1,1,0},{0,0,3},{1,0,0},{0,0,2},{1,1,0},{0,0,3},{1,0,0},{0,0,0},{1,0,0},{2,0,0},{1,0,1},{2,0,0},{0,0,0},{1,0,0}, \ +{0,0,1},{1,0,0},{0,0,2},{1,0,0},{0,0,5},{1,1,2},{2,1,1},{3,1,0},{2,1,0},{3,1,2},{2,1,1},{1,1,0},{0,0,1},{1,0,0},{2,0,0},{1,0,0},{2,0,4},{1,0,1},{0,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,0},{1,1,0},{0,0,0},{1,1,1},{0,0,7},{0,0,0},{1,1,0},{0,0,0},{1,1,0},{0,0,3},{1,0,1},{0,0,0},{1,0,0},{2,0,0},{1,0,0},{2,0,0},{1,0,0},{1,0,0}, \ +{0,0,0},{1,0,1},{0,0,1},{1,0,0},{0,0,0},{1,0,0},{0,0,3},{1,1,0},{0,0,0},{1,1,0},{2,1,2},{3,1,0},{2,1,0},{4,1,0},{3,1,0},{2,1,2},{1,1,0},{0,0,0},{1,0,2},{2,0,4},{1,0,0},{2,0,0},{0,0,0},{1,0,0},{0,0,0},{1,0,1},{0,0,2},{1,1,0},{0,0,0},{1,1,0},{0,0,0},{1,1,0},{0,0,5},{1,1,0},{0,0,0},{1,1,1},{0,0,0},{1,1,0},{0,0,1},{1,0,4},{2,0,1},{1,0,0},{1,0,0}, \ +{0,0,0},{2,0,0},{1,0,0},{0,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,3},{1,1,0},{0,0,0},{2,1,2},{3,1,0},{2,1,0},{3,1,0},{4,1,0},{3,1,0},{2,1,1},{1,1,1},{1,0,0},{0,0,0},{2,0,0},{1,0,0},{2,0,1},{1,0,0},{2,0,2},{1,0,0},{0,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,0},{1,0,0},{1,1,0},{0,0,1},{1,1,0},{0,0,0},{1,1,0},{0,0,1},{1,1,0},{0,0,2},{1,1,3},{0,0,0},{1,1,0},{0,0,2},{1,0,0},{2,0,0},{1,0,1},{2,0,0},{1,0,0},{2,0,0},{1,0,0}, \ +{0,0,0},{1,0,1},{2,0,0},{1,0,1},{0,0,0},{1,0,0},{0,0,0},{1,0,0},{0,0,0},{1,1,0},{0,0,0},{2,1,0},{1,1,0},{2,1,0},{3,1,1},{2,1,0},{4,1,1},{3,1,0},{2,1,1},{1,1,0},{0,0,1},{1,0,0},{2,0,0},{1,0,0},{2,0,2},{1,0,0},{2,0,1},{1,0,0},{0,0,0},{1,0,2},{0,0,0},{1,0,0},{0,0,3},{1,1,0},{0,0,1},{1,1,0},{0,0,0},{1,1,0},{0,0,0},{1,1,0},{0,0,0},{1,1,2},{2,1,0},{1,1,1},{0,0,1},{1,0,3},{2,0,0},{1,0,0},{2,0,1},{2,0,0}, \ +{0,0,0},{2,0,0},{1,0,0},{2,0,0},{1,0,4},{0,0,1},{1,1,0},{0,0,0},{2,1,0},{1,1,0},{3,1,3},{4,1,1},{3,1,0},{2,1,1},{1,1,0},{0,0,0},{1,0,2},{2,0,0},{1,0,0},{2,0,4},{1,0,0},{0,0,0},{1,0,3},{0,0,0},{1,0,0},{0,0,4},{1,1,0},{0,0,0},{1,1,0},{0,0,0},{1,1,4},{2,1,1},{1,1,1},{0,0,2},{1,0,1},{2,0,2},{1,0,0},{2,0,0},{2,0,0}, \ +{0,0,0},{2,0,3},{1,0,3},{0,0,2},{1,1,0},{2,1,2},{4,1,0},{3,1,0},{4,1,2},{3,1,1},{1,1,1},{0,0,0},{1,0,2},{2,0,4},{1,0,0},{2,0,1},{0,0,0},{1,0,0},{2,0,0},{0,0,0},{1,0,2},{0,0,0},{1,0,0},{0,0,3},{1,1,4},{2,1,0},{1,1,0},{2,1,2},{1,1,2},{0,0,1},{1,0,0},{2,0,1},{1,0,0},{3,0,0},{1,0,0},{2,0,0},{2,0,0}, \ +{0,0,0},{2,0,4},{1,0,3},{0,0,0},{1,1,2},{3,1,1},{4,1,2},{5,1,0},{4,1,0},{3,1,1},{1,1,1},{0,0,0},{1,0,1},{2,0,0},{1,0,0},{2,0,1},{3,0,0},{2,0,2},{1,0,2},{2,0,0},{1,0,5},{0,0,4},{1,1,1},{2,1,4},{3,1,0},{2,1,1},{1,1,1},{0,0,0},{1,0,2},{2,0,1},{3,0,0},{2,0,0},{1,0,0},{3,0,0}, \ +{0,0,0},{2,0,1},{3,0,0},{2,0,1},{1,0,0},{2,0,0},{1,0,0},{0,0,2},{1,1,0},{2,1,0},{3,1,1},{5,1,4},{3,1,1},{1,1,1},{1,0,0},{0,0,0},{2,0,1},{1,0,0},{3,0,0},{2,0,2},{3,0,0},{2,0,1},{1,0,1},{2,0,1},{1,0,0},{2,0,0},{1,0,3},{0,0,0},{1,0,0},{1,1,0},{0,0,0},{1,1,0},{2,1,2},{3,1,0},{2,1,0},{3,1,2},{2,1,0},{1,1,1},{0,0,0},{1,0,2},{2,0,1},{3,0,0},{2,0,1},{3,0,0}, \ +{0,0,0},{3,0,1},{2,0,0},{3,0,0},{2,0,0},{1,0,0},{2,0,0},{1,0,1},{0,0,1},{2,1,1},{3,1,0},{4,1,0},{6,1,0},{5,1,0},{7,1,0},{6,1,0},{5,1,0},{3,1,1},{1,1,0},{0,0,1},{1,0,0},{2,0,3},{3,0,0},{2,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,1},{1,0,0},{2,0,5},{1,0,2},{0,0,2},{1,1,0},{2,1,0},{3,1,2},{4,1,0},{3,1,0},{4,1,0},{3,1,0},{2,1,1},{1,1,0},{1,0,0},{0,0,0},{2,0,0},{1,0,0},{2,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,0},{2,0,0}, \ +{0,0,0},{2,0,0},{3,0,1},{2,0,0},{3,0,0},{2,0,1},{1,0,1},{0,0,1},{2,1,1},{4,1,0},{6,1,0},{7,1,1},{8,1,0},{7,1,0},{5,1,0},{3,1,0},{2,1,0},{1,1,0},{0,0,0},{1,0,1},{2,0,1},{3,0,0},{2,0,0},{3,0,2},{2,0,0},{3,0,2},{1,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,4},{1,0,1},{0,0,1},{1,1,0},{2,1,0},{3,1,0},{4,1,0},{5,1,0},{4,1,1},{5,1,0},{4,1,0},{2,1,1},{1,1,0},{0,0,0},{1,0,0},{2,0,3},{3,0,1},{2,0,0},{3,0,0}, \ +{0,0,0},{3,0,2},{2,0,0},{3,0,0},{2,0,2},{1,0,0},{0,0,1},{2,1,0},{3,1,0},{5,1,0},{8,1,0},{9,1,0},{10,1,1},{7,1,0},{5,1,0},{3,1,0},{1,1,0},{0,0,0},{1,0,1},{2,0,0},{3,0,0},{2,0,0},{3,0,3},{4,0,0},{3,0,7},{2,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,0},{1,0,0},{2,0,0},{0,0,2},{2,1,0},{3,1,0},{4,1,0},{5,1,0},{7,1,0},{5,1,0},{6,1,0},{4,1,1},{2,1,0},{0,0,1},{1,0,1},{2,0,1},{3,0,2},{2,0,0},{3,0,0}, \ +{0,0,0},{3,0,5},{2,0,1},{1,0,0},{0,0,0},{1,1,0},{2,1,0},{5,1,0},{8,1,0},{12,1,0},{14,1,0},{13,1,0},{9,1,0},{6,1,0},{3,1,0},{1,1,0},{0,0,0},{2,0,0},{1,0,0},{3,0,0},{2,0,0},{3,0,0},{4,0,0},{3,0,1},{4,0,0},{3,0,0},{4,0,1},{3,0,0},{4,0,0},{3,0,2},{4,0,0},{3,0,1},{4,0,0},{3,0,0},{2,0,0},{3,0,0},{2,0,2},{0,0,1},{1,1,0},{2,1,0},{4,1,0},{5,1,0},{7,1,0},{8,1,0},{6,1,1},{5,1,0},{3,1,0},{1,1,1},{1,0,1},{2,0,0},{3,0,0},{2,0,0},{3,0,1},{2,0,0},{3,0,0}, \ +}; + +#define PGM_UINT8(p) (uint8_t)pgm_read_byte_far(p) float AP_Declination::get_declination(float lat, float lon) { - int8_t decSW, decSE, decNW, decNE, lonmin, latmin; + int16_t decSW, decSE, decNW, decNE, lonmin, latmin; uint8_t latmin_index,lonmin_index; float decmin, decmax; @@ -76,13 +124,88 @@ AP_Declination::get_declination(float lat, float lon) latmin_index= (90+latmin)/5; lonmin_index= (180+lonmin)/5; - decSW = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index][lonmin_index]); - decSE = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index][lonmin_index+1]); - decNE = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index+1][lonmin_index+1]); - decNW = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index+1][lonmin_index]); + decSW = get_lookup_value(latmin_index, lonmin_index); + decSE = get_lookup_value(latmin_index, lonmin_index+1); + decNE = get_lookup_value(latmin_index+1, lonmin_index+1); + decNW = get_lookup_value(latmin_index+1, lonmin_index); /* approximate declination within the grid using bilinear interpolation */ decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW; decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW; return (lat - latmin) / 5 * (decmax - decmin) + decmin; } + +int16_t +AP_Declination::get_lookup_value(uint8_t x, uint8_t y) +{ + // return value + int16_t val = 0; + + // These are exception indicies + if(x <= 6 || x >= 34) + { + // If the x index is in the upper range we need to translate it + // to match the 10 indicies in the exceptions lookup table + if(x >= 34) x -= 27; + + // Read the unsigned value from the array + val = PGM_UINT8(&exceptions[x][y]); + + // Read the 8 bit compressed sign values + uint8_t sign = PGM_UINT8(&exception_signs[x][y/8]); + + // Check the sign bit for this index + if(sign & (0x80 >> y%8)) + val = -val; + + return val; + } + + // Because the values were removed from the start of the + // original array (0-6) to the exception array, all the indicies + // in this main lookup need to be shifted left 7 + // EX: User enters 7 -> 7 is the first row in this array so it needs to be zero + if(x >= 7) x -= 7; + + // If we are looking for the first value we can just use the + // row start value from declination_keys + if(y == 0) return PGM_UINT8(&declination_keys[0][x]); + + // Init vars + row_value stval; + int16_t offset = 0; + + // These will never exceed the second dimension length of 73 + uint8_t current_virtual_index = 0, r; + + // This could be the length of the array or less (1075 or less) + uint16_t start_index = 0, i; + + // Init value to row start + val = PGM_UINT8(&declination_keys[0][x]); + + // Find the first element in the 1D array + // that corresponds with the target row + for(i = 0; i < x; i++){ + start_index += PGM_UINT8(&declination_keys[1][i]); + } + + // Traverse the row until we find our value + for(i = start_index; i < (start_index + PGM_UINT8(&declination_keys[1][x])) && current_virtual_index <= y; i++){ + + // Pull out the row_value struct + memcpy_P((void*) &stval, (const prog_char *)&declination_values[i], sizeof(struct row_value)); + + // Pull the first offset and determine sign + offset = stval.abs_offset; + offset = (stval.offset_sign == 1) ? -offset : offset; + + // Add offset for each repeat + // This will at least run once for zero repeat + for(r = 0; r <= stval.repeats && current_virtual_index <= y; r++){ + val += offset; + current_virtual_index++; + } + } + return val; +} diff --git a/libraries/AP_Declination/AP_Declination.h b/libraries/AP_Declination/AP_Declination.h index 26e9abb283..76cfe0af24 100644 --- a/libraries/AP_Declination/AP_Declination.h +++ b/libraries/AP_Declination/AP_Declination.h @@ -16,6 +16,8 @@ class AP_Declination { public: static float get_declination(float lat, float lon); +private: + static int16_t get_lookup_value(uint8_t x, uint8_t y); }; #endif // AP_Declination_h diff --git a/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde b/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde index a77a24298a..cc74e2de25 100644 --- a/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde +++ b/libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde @@ -5,19 +5,132 @@ #include #include #include +#include +#include +#include + +#ifdef DESKTOP_BUILD +// all of this is needed to build with SITL +#include +#include +#include +#include +#include +#include +#include +#include +#include +Arduino_Mega_ISR_Registry isr_registry; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; +#endif FastSerialPort(Serial, 0); +static const int16_t dec_tbl[37][73] = \ +{ \ +{150,145,140,135,130,125,120,115,110,105,100,95 ,90 ,85 ,80 ,75 ,70 ,65 ,60 ,55 ,50 ,45 ,40 ,35 ,30 ,25 ,20 ,15 ,10 ,5 ,0 ,-4 ,-9 ,-14,-19,-24,-29,-34,-39,-44,-49,-54,-59,-64,-69,-74,-79,-84,-89,-94,-99,104,109,114,119,124,129,134,139,144,149,154,159,164,169,174,179,175,170,165,160,155,150}, \ +{143,137,131,126,120,115,110,105,100,95 ,90 ,85 ,80 ,75 ,71 ,66 ,62 ,57 ,53 ,48 ,44 ,39 ,35 ,31 ,27 ,22 ,18 ,14 ,9 ,5 ,1 ,-3 ,-7 ,-11,-16,-20,-25,-29,-34,-38,-43,-47,-52,-57,-61,-66,-71,-76,-81,-86,-91,-96,101,107,112,117,123,128,134,140,146,151,157,163,169,175,178,172,166,160,154,148,143}, \ +{130,124,118,112,107,101,96 ,92 ,87 ,82 ,78 ,74 ,70 ,65 ,61 ,57 ,54 ,50 ,46 ,42 ,38 ,34 ,31 ,27 ,23 ,19 ,16 ,12 ,8 ,4 ,1 ,-2 ,-6 ,-10,-14,-18,-22,-26,-30,-34,-38,-43,-47,-51,-56,-61,-65,-70,-75,-79,-84,-89,-94,100,105,111,116,122,128,135,141,148,155,162,170,177,174,166,159,151,144,137,130}, \ +{111,104,99 ,94 ,89 ,85 ,81 ,77 ,73 ,70 ,66 ,63 ,60 ,56 ,53 ,50 ,46 ,43 ,40 ,36 ,33 ,30 ,26 ,23 ,20 ,16 ,13 ,10 ,6 ,3 ,0 ,-3 ,-6 ,-9 ,-13,-16,-20,-24,-28,-32,-36,-40,-44,-48,-52,-57,-61,-65,-70,-74,-79,-84,-88,-93,-98,103,109,115,121,128,135,143,152,162,172,176,165,154,144,134,125,118,111}, \ +{85 ,81 ,77 ,74 ,71 ,68 ,65 ,63 ,60 ,58 ,56 ,53 ,51 ,49 ,46 ,43 ,41 ,38 ,35 ,32 ,29 ,26 ,23 ,19 ,16 ,13 ,10 ,7 ,4 ,1 ,-1 ,-3 ,-6 ,-9 ,-13,-16,-19,-23,-26,-30,-34,-38,-42,-46,-50,-54,-58,-62,-66,-70,-74,-78,-83,-87,-91,-95,100,105,110,117,124,133,144,159,178,160,141,125,112,103,96 ,90 ,85 }, \ +{62 ,60 ,58 ,57 ,55 ,54 ,52 ,51 ,50 ,48 ,47 ,46 ,44 ,42 ,41 ,39 ,36 ,34 ,31 ,28 ,25 ,22 ,19 ,16 ,13 ,10 ,7 ,4 ,2 ,0 ,-3 ,-5 ,-8 ,-10,-13,-16,-19,-22,-26,-29,-33,-37,-41,-45,-49,-53,-56,-60,-64,-67,-70,-74,-77,-80,-83,-86,-89,-91,-94,-97,101,105,111,130,109,84 ,77 ,74 ,71 ,68 ,66 ,64 ,62 }, \ +{46 ,46 ,45 ,44 ,44 ,43 ,42 ,42 ,41 ,41 ,40 ,39 ,38 ,37 ,36 ,35 ,33 ,31 ,28 ,26 ,23 ,20 ,16 ,13 ,10 ,7 ,4 ,1 ,-1 ,-3 ,-5 ,-7 ,-9 ,-12,-14,-16,-19,-22,-26,-29,-33,-36,-40,-44,-48,-51,-55,-58,-61,-64,-66,-68,-71,-72,-74,-74,-75,-74,-72,-68,-61,-48,-25,2 ,22 ,33 ,40 ,43 ,45 ,46 ,47 ,46 ,46 }, \ +{36 ,36 ,36 ,36 ,36 ,35 ,35 ,35 ,35 ,34 ,34 ,34 ,34 ,33 ,32 ,31 ,30 ,28 ,26 ,23 ,20 ,17 ,14 ,10 ,6 ,3 ,0 ,-2 ,-4 ,-7 ,-9 ,-10,-12,-14,-15,-17,-20,-23,-26,-29,-32,-36,-40,-43,-47,-50,-53,-56,-58,-60,-62,-63,-64,-64,-63,-62,-59,-55,-49,-41,-30,-17,-4 ,6 ,15 ,22 ,27 ,31 ,33 ,34 ,35 ,36 ,36 }, \ +{30 ,30 ,30 ,30 ,30 ,30 ,30 ,29 ,29 ,29 ,29 ,29 ,29 ,29 ,29 ,28 ,27 ,26 ,24 ,21 ,18 ,15 ,11 ,7 ,3 ,0 ,-3 ,-6 ,-9 ,-11,-12,-14,-15,-16,-17,-19,-21,-23,-26,-29,-32,-35,-39,-42,-45,-48,-51,-53,-55,-56,-57,-57,-56,-55,-53,-49,-44,-38,-31,-23,-14,-6 ,0 ,7 ,13 ,17 ,21 ,24 ,26 ,27 ,29 ,29 ,30 }, \ +{25 ,25 ,26 ,26 ,26 ,25 ,25 ,25 ,25 ,25 ,25 ,25 ,25 ,26 ,25 ,25 ,24 ,23 ,21 ,19 ,16 ,12 ,8 ,4 ,0 ,-3 ,-7 ,-10,-13,-15,-16,-17,-18,-19,-20,-21,-22,-23,-25,-28,-31,-34,-37,-40,-43,-46,-48,-49,-50,-51,-51,-50,-48,-45,-42,-37,-32,-26,-19,-13,-7 ,-1 ,3 ,7 ,11 ,14 ,17 ,19 ,21 ,23 ,24 ,25 ,25 }, \ +{21 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,22 ,21 ,20 ,18 ,16 ,13 ,9 ,5 ,1 ,-3 ,-7 ,-11,-14,-17,-18,-20,-21,-21,-22,-22,-22,-23,-23,-25,-27,-29,-32,-35,-37,-40,-42,-44,-45,-45,-45,-44,-42,-40,-36,-32,-27,-22,-17,-12,-7 ,-3 ,0 ,3 ,7 ,9 ,12 ,14 ,16 ,18 ,19 ,20 ,21 ,21 }, \ +{18 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,19 ,18 ,17 ,16 ,14 ,10 ,7 ,2 ,-1 ,-6 ,-10,-14,-17,-19,-21,-22,-23,-24,-24,-24,-24,-23,-23,-23,-24,-26,-28,-30,-33,-35,-37,-38,-39,-39,-38,-36,-34,-31,-28,-24,-19,-15,-10,-6 ,-3 ,0 ,1 ,4 ,6 ,8 ,10 ,12 ,14 ,15 ,16 ,17 ,18 ,18 }, \ +{16 ,16 ,17 ,17 ,17 ,17 ,17 ,17 ,17 ,17 ,17 ,16 ,16 ,16 ,16 ,16 ,16 ,15 ,13 ,11 ,8 ,4 ,0 ,-4 ,-9 ,-13,-16,-19,-21,-23,-24,-25,-25,-25,-25,-24,-23,-21,-20,-20,-21,-22,-24,-26,-28,-30,-31,-32,-31,-30,-29,-27,-24,-21,-17,-13,-9 ,-6 ,-3 ,-1 ,0 ,2 ,4 ,5 ,7 ,9 ,10 ,12 ,13 ,14 ,15 ,16 ,16 }, \ +{14 ,14 ,14 ,15 ,15 ,15 ,15 ,15 ,15 ,15 ,14 ,14 ,14 ,14 ,14 ,14 ,13 ,12 ,11 ,9 ,5 ,2 ,-2 ,-6 ,-11,-15,-18,-21,-23,-24,-25,-25,-25,-25,-24,-22,-21,-18,-16,-15,-15,-15,-17,-19,-21,-22,-24,-24,-24,-23,-22,-20,-18,-15,-12,-9 ,-5 ,-3 ,-1 ,0 ,1 ,2 ,4 ,5 ,6 ,8 ,9 ,10 ,11 ,12 ,13 ,14 ,14 }, \ +{12 ,13 ,13 ,13 ,13 ,13 ,13 ,13 ,13 ,13 ,13 ,13 ,12 ,12 ,12 ,12 ,11 ,10 ,9 ,6 ,3 ,0 ,-4 ,-8 ,-12,-16,-19,-21,-23,-24,-24,-24,-24,-23,-22,-20,-17,-15,-12,-10,-9 ,-9 ,-10,-12,-13,-15,-17,-17,-18,-17,-16,-15,-13,-11,-8 ,-5 ,-3 ,-1 ,0 ,1 ,1 ,2 ,3 ,4 ,6 ,7 ,8 ,9 ,10 ,11 ,12 ,12 ,12 }, \ +{11 ,11 ,11 ,11 ,11 ,12 ,12 ,12 ,12 ,12 ,11 ,11 ,11 ,11 ,11 ,10 ,10 ,9 ,7 ,5 ,2 ,-1 ,-5 ,-9 ,-13,-17,-20,-22,-23,-23,-23,-23,-22,-20,-18,-16,-14,-11,-9 ,-6 ,-5 ,-4 ,-5 ,-6 ,-8 ,-9 ,-11,-12,-12,-12,-12,-11,-9 ,-8 ,-6 ,-3 ,-1 ,0 ,0 ,1 ,1 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,9 ,10 ,11 ,11 ,11 }, \ +{10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,9 ,9 ,9 ,7 ,6 ,3 ,0 ,-3 ,-6 ,-10,-14,-17,-20,-21,-22,-22,-22,-21,-19,-17,-15,-13,-10,-8 ,-6 ,-4 ,-2 ,-2 ,-2 ,-2 ,-4 ,-5 ,-7 ,-8 ,-8 ,-9 ,-8 ,-8 ,-7 ,-5 ,-4 ,-2 ,0 ,0 ,1 ,1 ,1 ,2 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,9 ,10 ,10 ,10 }, \ +{9 ,9 ,9 ,9 ,9 ,9 ,9 ,10 ,10 ,9 ,9 ,9 ,9 ,9 ,9 ,8 ,8 ,6 ,5 ,2 ,0 ,-4 ,-7 ,-11,-15,-17,-19,-21,-21,-21,-20,-18,-16,-14,-12,-10,-8 ,-6 ,-4 ,-2 ,-1 ,0 ,0 ,0 ,-1 ,-2 ,-4 ,-5 ,-5 ,-6 ,-6 ,-5 ,-5 ,-4 ,-3 ,-1 ,0 ,0 ,1 ,1 ,1 ,1 ,2 ,3 ,3 ,5 ,6 ,7 ,8 ,8 ,9 ,9 ,9 }, \ +{9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,8 ,8 ,8 ,8 ,7 ,5 ,4 ,1 ,-1 ,-5 ,-8 ,-12,-15,-17,-19,-20,-20,-19,-18,-16,-14,-11,-9 ,-7 ,-5 ,-4 ,-2 ,-1 ,0 ,0 ,1 ,1 ,0 ,0 ,-2 ,-3 ,-3 ,-4 ,-4 ,-4 ,-3 ,-3 ,-2 ,-1 ,0 ,0 ,0 ,0 ,0 ,1 ,1 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,8 ,9 ,9 }, \ +{9 ,9 ,9 ,8 ,8 ,8 ,9 ,9 ,9 ,9 ,9 ,8 ,8 ,8 ,8 ,7 ,6 ,5 ,3 ,0 ,-2 ,-5 ,-9 ,-12,-15,-17,-18,-19,-19,-18,-16,-14,-12,-9 ,-7 ,-5 ,-4 ,-2 ,-1 ,0 ,0 ,1 ,1 ,1 ,1 ,0 ,0 ,-1 ,-2 ,-2 ,-3 ,-3 ,-2 ,-2 ,-1 ,-1 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,1 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,8 ,9 }, \ +{8 ,8 ,8 ,8 ,8 ,8 ,9 ,9 ,9 ,9 ,9 ,9 ,8 ,8 ,8 ,7 ,6 ,4 ,2 ,0 ,-3 ,-6 ,-9 ,-12,-15,-17,-18,-18,-17,-16,-14,-12,-10,-8 ,-6 ,-4 ,-2 ,-1 ,0 ,0 ,1 ,2 ,2 ,2 ,2 ,1 ,0 ,0 ,-1 ,-1 ,-1 ,-2 ,-2 ,-1 ,-1 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,1 ,2 ,3 ,4 ,5 ,6 ,7 ,8 ,8 }, \ +{8 ,8 ,8 ,8 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,9 ,8 ,8 ,7 ,5 ,3 ,1 ,-1 ,-4 ,-7 ,-10,-13,-15,-16,-17,-17,-16,-15,-13,-11,-9 ,-6 ,-5 ,-3 ,-2 ,0 ,0 ,0 ,1 ,2 ,2 ,2 ,2 ,1 ,1 ,0 ,0 ,0 ,-1 ,-1 ,-1 ,-1 ,-1 ,0 ,0 ,0 ,0 ,-1 ,-1 ,-1 ,-1 ,-1 ,0 ,0 ,1 ,3 ,4 ,5 ,7 ,7 ,8 }, \ +{8 ,8 ,9 ,9 ,9 ,9 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,9 ,8 ,7 ,5 ,3 ,0 ,-2 ,-5 ,-8 ,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8 ,-6 ,-4 ,-2 ,-1 ,0 ,0 ,1 ,2 ,2 ,3 ,3 ,2 ,2 ,1 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,-1 ,-1 ,-2 ,-2 ,-2 ,-2 ,-2 ,-1 ,0 ,0 ,1 ,3 ,4 ,6 ,7 ,8 }, \ +{7 ,8 ,9 ,9 ,9 ,10 ,10 ,11 ,11 ,11 ,11 ,11 ,10 ,10 ,9 ,7 ,5 ,3 ,0 ,-2 ,-6 ,-9 ,-11,-13,-15,-16,-16,-15,-14,-13,-11,-9 ,-7 ,-5 ,-3 ,-2 ,0 ,0 ,1 ,1 ,2 ,3 ,3 ,3 ,3 ,2 ,2 ,1 ,1 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,-1 ,-1 ,-2 ,-3 ,-3 ,-4 ,-4 ,-4 ,-3 ,-2 ,-1 ,0 ,1 ,3 ,5 ,6 ,7 }, \ +{6 ,8 ,9 ,9 ,10 ,11 ,11 ,12 ,12 ,12 ,12 ,12 ,11 ,11 ,9 ,7 ,5 ,2 ,0 ,-3 ,-7 ,-10,-12,-14,-15,-16,-15,-15,-13,-12,-10,-8 ,-7 ,-5 ,-3 ,-1 ,0 ,0 ,1 ,2 ,2 ,3 ,3 ,4 ,3 ,3 ,3 ,2 ,2 ,1 ,1 ,1 ,0 ,0 ,0 ,0 ,-1 ,-2 ,-3 ,-4 ,-4 ,-5 ,-5 ,-5 ,-5 ,-4 ,-2 ,-1 ,0 ,2 ,3 ,5 ,6 }, \ +{6 ,7 ,8 ,10 ,11 ,12 ,12 ,13 ,13 ,14 ,14 ,13 ,13 ,11 ,10 ,8 ,5 ,2 ,0 ,-4 ,-8 ,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8 ,-6 ,-5 ,-3 ,-1 ,0 ,0 ,1 ,2 ,3 ,3 ,4 ,4 ,4 ,4 ,4 ,3 ,3 ,3 ,2 ,2 ,1 ,1 ,0 ,0 ,-1 ,-2 ,-3 ,-5 ,-6 ,-7 ,-7 ,-7 ,-6 ,-5 ,-4 ,-3 ,-1 ,0 ,2 ,4 ,6 }, \ +{5 ,7 ,8 ,10 ,11 ,12 ,13 ,14 ,15 ,15 ,15 ,14 ,14 ,12 ,11 ,8 ,5 ,2 ,-1 ,-5 ,-9 ,-12,-14,-16,-17,-17,-16,-15,-14,-12,-11,-9 ,-7 ,-5 ,-3 ,-1 ,0 ,0 ,1 ,2 ,3 ,4 ,4 ,5 ,5 ,5 ,5 ,5 ,5 ,4 ,4 ,3 ,3 ,2 ,1 ,0 ,-1 ,-2 ,-4 ,-6 ,-7 ,-8 ,-8 ,-8 ,-8 ,-7 ,-6 ,-4 ,-2 ,0 ,1 ,3 ,5 }, \ +{4 ,6 ,8 ,10 ,12 ,13 ,14 ,15 ,16 ,16 ,16 ,16 ,15 ,13 ,11 ,9 ,5 ,2 ,-2 ,-6 ,-10,-13,-16,-17,-18,-18,-17,-16,-15,-13,-11,-9 ,-7 ,-5 ,-4 ,-2 ,0 ,0 ,1 ,3 ,3 ,4 ,5 ,6 ,6 ,7 ,7 ,7 ,7 ,7 ,6 ,5 ,4 ,3 ,2 ,0 ,-1 ,-3 ,-5 ,-7 ,-8 ,-9 ,-10,-10,-10,-9 ,-7 ,-5 ,-4 ,-1 ,0 ,2 ,4 }, \ +{4 ,6 ,8 ,10 ,12 ,14 ,15 ,16 ,17 ,18 ,18 ,17 ,16 ,15 ,12 ,9 ,5 ,1 ,-3 ,-8 ,-12,-15,-18,-19,-20,-20,-19,-18,-16,-15,-13,-11,-8 ,-6 ,-4 ,-2 ,-1 ,0 ,1 ,3 ,4 ,5 ,6 ,7 ,8 ,9 ,9 ,9 ,9 ,9 ,9 ,8 ,7 ,5 ,3 ,1 ,-1 ,-3 ,-6 ,-8 ,-10,-11,-12,-12,-11,-10,-9 ,-7 ,-5 ,-2 ,0 ,1 ,4 }, \ +{4 ,6 ,8 ,11 ,13 ,15 ,16 ,18 ,19 ,19 ,19 ,19 ,18 ,16 ,13 ,10 ,5 ,0 ,-5 ,-10,-15,-18,-21,-22,-23,-22,-22,-20,-18,-17,-14,-12,-10,-8 ,-5 ,-3 ,-1 ,0 ,1 ,3 ,5 ,6 ,8 ,9 ,10 ,11 ,12 ,12 ,13 ,12 ,12 ,11 ,9 ,7 ,5 ,2 ,0 ,-3 ,-6 ,-9 ,-11,-12,-13,-13,-12,-11,-10,-8 ,-6 ,-3 ,-1 ,1 ,4 }, \ +{3 ,6 ,9 ,11 ,14 ,16 ,17 ,19 ,20 ,21 ,21 ,21 ,19 ,17 ,14 ,10 ,4 ,-1 ,-8 ,-14,-19,-22,-25,-26,-26,-26,-25,-23,-21,-19,-17,-14,-12,-9 ,-7 ,-4 ,-2 ,0 ,1 ,3 ,5 ,7 ,9 ,11 ,13 ,14 ,15 ,16 ,16 ,16 ,16 ,15 ,13 ,10 ,7 ,4 ,0 ,-3 ,-7 ,-10,-12,-14,-15,-14,-14,-12,-11,-9 ,-6 ,-4 ,-1 ,1 ,3 }, \ +{4 ,6 ,9 ,12 ,14 ,17 ,19 ,21 ,22 ,23 ,23 ,23 ,21 ,19 ,15 ,9 ,2 ,-5 ,-13,-20,-25,-28,-30,-31,-31,-30,-29,-27,-25,-22,-20,-17,-14,-11,-9 ,-6 ,-3 ,0 ,1 ,4 ,6 ,9 ,11 ,13 ,15 ,17 ,19 ,20 ,21 ,21 ,21 ,20 ,18 ,15 ,11 ,6 ,2 ,-2 ,-7 ,-11,-13,-15,-16,-16,-15,-13,-11,-9 ,-7 ,-4 ,-1 ,1 ,4 }, \ +{4 ,7 ,10 ,13 ,15 ,18 ,20 ,22 ,24 ,25 ,25 ,25 ,23 ,20 ,15 ,7 ,-2 ,-12,-22,-29,-34,-37,-38,-38,-37,-36,-34,-31,-29,-26,-23,-20,-17,-13,-10,-7 ,-4 ,-1 ,2 ,5 ,8 ,11 ,13 ,16 ,18 ,21 ,23 ,24 ,26 ,26 ,26 ,26 ,24 ,21 ,17 ,12 ,5 ,0 ,-6 ,-10,-14,-16,-16,-16,-15,-14,-12,-10,-7 ,-4 ,-1 ,1 ,4 }, \ +{4 ,7 ,10 ,13 ,16 ,19 ,22 ,24 ,26 ,27 ,27 ,26 ,24 ,19 ,11 ,-1 ,-15,-28,-37,-43,-46,-47,-47,-45,-44,-41,-39,-36,-32,-29,-26,-22,-19,-15,-11,-8 ,-4 ,-1 ,2 ,5 ,9 ,12 ,15 ,19 ,22 ,24 ,27 ,29 ,31 ,33 ,33 ,33 ,32 ,30 ,26 ,21 ,14 ,6 ,0 ,-6 ,-11,-14,-15,-16,-15,-14,-12,-9 ,-7 ,-4 ,-1 ,1 ,4 }, \ +{6 ,9 ,12 ,15 ,18 ,21 ,23 ,25 ,27 ,28 ,27 ,24 ,17 ,4 ,-14,-34,-49,-56,-60,-60,-60,-58,-56,-53,-50,-47,-43,-40,-36,-32,-28,-25,-21,-17,-13,-9 ,-5 ,-1 ,2 ,6 ,10 ,14 ,17 ,21 ,24 ,28 ,31 ,34 ,37 ,39 ,41 ,42 ,43 ,43 ,41 ,38 ,33 ,25 ,17 ,8 ,0 ,-4 ,-8 ,-10,-10,-10,-8 ,-7 ,-4 ,-2 ,0 ,3 ,6 }, \ +{22 ,24 ,26 ,28 ,30 ,32 ,33 ,31 ,23 ,-18,-81,-96,-99,-98,-95,-93,-89,-86,-82,-78,-74,-70,-66,-62,-57,-53,-49,-44,-40,-36,-32,-27,-23,-19,-14,-10,-6 ,-1 ,2 ,6 ,10 ,15 ,19 ,23 ,27 ,31 ,35 ,38 ,42 ,45 ,49 ,52 ,55 ,57 ,60 ,61 ,63 ,63 ,62 ,61 ,57 ,53 ,47 ,40 ,33 ,28 ,23 ,21 ,19 ,19 ,19 ,20 ,22 }, \ +{168,173,178,176,171,166,161,156,151,146,141,136,131,126,121,116,111,106,101,-96,-91,-86,-81,-76,-71,-66,-61,-56,-51,-46,-41,-36,-31,-26,-21,-16,-11,-6 ,-1 ,3 ,8 ,13 ,18 ,23 ,28 ,33 ,38 ,43 ,48 ,53 ,58 ,63 ,68 ,73 ,78 ,83 ,88 ,93 ,98 ,103,108,113,118,123,128,133,138,143,148,153,158,163,168}, \ +}; + +static float get_declination(float lat, float lon) +{ + int16_t decSW, decSE, decNW, decNE, lonmin, latmin; + float decmin, decmax; + uint8_t latmin_index, lonmin_index; + + // Validate input values + lat = constrain(lat, -90, 90); + lon = constrain(lon, -180, 180); + + latmin = floor(lat/5)*5; + lonmin = floor(lon/5)*5; + + latmin_index= (90+latmin)/5; + lonmin_index= (180+lonmin)/5; + + decSW = dec_tbl[latmin_index][lonmin_index]; + decSE = dec_tbl[latmin_index][lonmin_index+1]; + decNE = dec_tbl[latmin_index+1][lonmin_index+1]; + decNW = dec_tbl[latmin_index+1][lonmin_index]; + + decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW; + decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW; + return (lat - latmin) / 5 * (decmax - decmin) + decmin; +} + void setup(void) { - float declination; + float declination, declination_test; + uint16_t pass = 0, fail = 0; + uint32_t total_time=0; Serial.begin(115200); + Serial.print("Beginning Test. Please wait...\n"); - declination = AP_Declination::get_declination(43.064191, -87.997498); - Serial.printf("declination: %f\n", declination); + for(int16_t i = -90; i <= 90; i+=5) + { + for(int16_t j = -180; j <= 180; j+=5) + { + uint32_t t1 = micros(); + declination = AP_Declination::get_declination(i, j); + total_time += micros() - t1; + declination_test = get_declination(i, j); + if(declination == declination_test) + { + //Serial.printf("Pass: %i, %i : %f, %f\n", i, j, declination, declination_test); + pass++; + } + else + { + Serial.printf("FAIL: %i, %i : %f, %f\n", i, j, declination, declination_test); + fail++; + } + } + } + Serial.print("Ending Test.\n\n"); + Serial.printf("Total Pass: %i\n", pass); + Serial.printf("Total Fail: %i\n", fail); + Serial.printf("Average time per call: %.1f usec\n", + total_time/(float)(pass+fail)); } void loop(void) { } + diff --git a/libraries/AP_Declination/examples/AP_Declination_test/Makefile b/libraries/AP_Declination/examples/AP_Declination_test/Makefile index d1f40fd90f..fcdc8ff8fe 100644 --- a/libraries/AP_Declination/examples/AP_Declination_test/Makefile +++ b/libraries/AP_Declination/examples/AP_Declination_test/Makefile @@ -1 +1,4 @@ include ../../../AP_Common/Arduino.mk + +sitl: + make -f ../../../../libraries/Desktop/Desktop.mk diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 1c7cb4825c..2bbf2369f9 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -28,9 +28,8 @@ AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; #endif -#if 0 #include -#endif + static float rad_diff(float rad1, float rad2) { diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 2d27a1f8ad..23dc35f862 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -25,10 +25,7 @@ AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; #endif -#if AUTOMATIC_DECLINATION == ENABLED -// this is in an #if to avoid the static data #include // ArduPilot Mega Declination Helper Library -#endif // standard rotation matrices (these are the originals from the old code) diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 236a4263ff..8ad2ffd15c 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -163,6 +163,15 @@ Vector3 Matrix3::operator *(const Vector3 &v) const c.x * v.x + c.y * v.y + c.z * v.z); } +// multiplication of transpose by a vector +template +Vector3 Matrix3::mul_transpose(const Vector3 &v) const +{ + return Vector3(a.x * v.x + b.x * v.y + c.x * v.z, + a.y * v.x + b.y * v.y + c.y * v.z, + a.z * v.x + b.z * v.y + c.z * v.z); +} + // multiplication by another Matrix3 template Matrix3 Matrix3::operator *(const Matrix3 &m) const @@ -186,4 +195,5 @@ template void Matrix3::rotate(const Vector3 &g); template void Matrix3::from_euler(float roll, float pitch, float yaw); template void Matrix3::to_euler(float *roll, float *pitch, float *yaw); template Vector3 Matrix3::operator *(const Vector3 &v) const; +template Vector3 Matrix3::mul_transpose(const Vector3 &v) const; template Matrix3 Matrix3::operator *(const Matrix3 &m) const; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index f11b6153bf..202d294fe2 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -92,6 +92,9 @@ public: // multiplication by a vector Vector3 operator *(const Vector3 &v) const; + // multiplication of transpose by a vector + Vector3 mul_transpose(const Vector3 &v) const; + // multiplication by another Matrix3 Matrix3 operator *(const Matrix3 &m) const; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index fda2472619..b4fb52ddd9 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -75,7 +75,7 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) } // check the sensor is functioning - if( retry < 3 ) { + while( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) return true; retry++; diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index 110ab55173..5e8324651b 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -7,7 +7,6 @@ #include #include // ArduPilot Mega Vector/Matrix math Library #include // Arduino SPI library -#include // ArduCopter DCM Library #include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library //////////////////////////////////////////////////////////////////////////////// diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 205522898d..9ee53f4440 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -223,7 +223,12 @@ static void sitl_simulator_output(void) static void timer_handler(int signum) { static uint32_t last_update_count; + static bool running; + if (running) { + return; + } + running = true; cli(); #ifndef __CYGWIN__ @@ -261,11 +266,13 @@ static void timer_handler(int signum) if (update_count == 0) { sitl_update_gps(0, 0, 0, 0, 0, false); sei(); + running = false; return; } if (update_count == last_update_count) { sei(); + running = false; return; } last_update_count = update_count; @@ -280,6 +287,7 @@ static void timer_handler(int signum) sitl_update_barometer(sim_state.altitude); sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); sei(); + running = false; } diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h index 6a21d6f0f9..348d433ff9 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -8,6 +8,7 @@ #include #include +#include "util.h" static const float vibration_level = 0.2; static const float drift_speed = 0.2; // degrees/second/minute @@ -18,13 +19,6 @@ static const float noise_offset[8]= { 0, 0, 0, 0, 0, 0, 0 static const float drift_rate[8] = { 1.0, 1.0, 1.0, 0, 0, 0, 0, 0 }; static const float base_noise = 2; -// generate a random float between -1 and 1 -static double rand_float(void) -{ - float ret = ((unsigned)random()) % 2000000; - return (ret - 1.0e6) / 1.0e6; -} - static inline float gyro_drift(uint8_t chan) { if (drift_rate[chan] * drift_speed == 0.0) { diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp index 8d1d319a37..39912e8959 100644 --- a/libraries/Desktop/support/sitl_compass.cpp +++ b/libraries/Desktop/support/sitl_compass.cpp @@ -30,6 +30,8 @@ // using an APM1 with 5883L compass #define MAG_FIELD_STRENGTH 818 +#define MAG_NOISE 5 + /* given a magnetic heading, and roll, pitch, yaw values, calculate consistent magnetometer components @@ -53,7 +55,7 @@ static Vector3f heading_to_mag(float roll, float pitch, float yaw) // convert the earth frame magnetic vector to body frame, and // apply the offsets m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z); - return m; + return m + (rand_vec3f() * MAG_NOISE); } diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp index 7656d36bf1..6a956533e3 100644 --- a/libraries/Desktop/support/util.cpp +++ b/libraries/Desktop/support/util.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include "desktop.h" #include "util.h" @@ -58,3 +59,15 @@ void convert_body_frame(double rollDeg, double pitchDeg, *q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta); *r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot; } + +// generate a random Vector3f of size 1 +Vector3f rand_vec3f(void) +{ + Vector3f v = Vector3f(rand_float(), + rand_float(), + rand_float()); + if (v.length() != 0.0) { + v.normalize(); + } + return v; +} diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 7d55f827c6..7778adda14 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -12,3 +12,10 @@ void runInterrupt(uint8_t inum); void convert_body_frame(double rollDeg, double pitchDeg, double rollRate, double pitchRate, double yawRate, double *p, double *q, double *r); + +// generate a random float between -1 and 1 +#define rand_float() (((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6) + +#ifdef VECTOR3_H +Vector3f rand_vec3f(void); +#endif diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp index efc8c896e6..67f71483c6 100644 --- a/libraries/FastSerial/FastSerial.cpp +++ b/libraries/FastSerial/FastSerial.cpp @@ -212,6 +212,13 @@ size_t FastSerial::write(uint8_t c) // wait for room in the tx buffer i = (_txBuffer->head + 1) & _txBuffer->mask; + + // if the port is set into non-blocking mode, then drop the byte + // if there isn't enough room for it in the transmit buffer + if (_nonblocking_writes && i == _txBuffer->tail) { + return 0; + } + while (i == _txBuffer->tail) ; diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h index 5fcaa95c75..b64ff1fb2d 100644 --- a/libraries/FastSerial/FastSerial.h +++ b/libraries/FastSerial/FastSerial.h @@ -160,6 +160,11 @@ public: return (1< is the type of data being filtered. // 2nd parameter is a larger data type used during summation to prevent overflows +// 3rd parameter is the number of elements in the filter template class AverageFilter : public FilterWithBuffer { diff --git a/libraries/Filter/FilterWithBuffer.h b/libraries/Filter/FilterWithBuffer.h index e86ff6b81f..a39f5e109e 100644 --- a/libraries/Filter/FilterWithBuffer.h +++ b/libraries/Filter/FilterWithBuffer.h @@ -55,6 +55,19 @@ typedef FilterWithBuffer FilterWithBufferUInt16_Size5; typedef FilterWithBuffer FilterWithBufferUInt16_Size6; typedef FilterWithBuffer FilterWithBufferUInt16_Size7; +typedef FilterWithBuffer FilterWithBufferInt32_Size2; +typedef FilterWithBuffer FilterWithBufferInt32_Size3; +typedef FilterWithBuffer FilterWithBufferInt32_Size4; +typedef FilterWithBuffer FilterWithBufferInt32_Size5; +typedef FilterWithBuffer FilterWithBufferInt32_Size6; +typedef FilterWithBuffer FilterWithBufferInt32_Size7; +typedef FilterWithBuffer FilterWithBufferUInt32_Size2; +typedef FilterWithBuffer FilterWithBufferUInt32_Size3; +typedef FilterWithBuffer FilterWithBufferUInt32_Size4; +typedef FilterWithBuffer FilterWithBufferUInt32_Size5; +typedef FilterWithBuffer FilterWithBufferUInt32_Size6; +typedef FilterWithBuffer FilterWithBufferUInt32_Size7; + // Constructor template FilterWithBuffer::FilterWithBuffer() : diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h new file mode 100644 index 0000000000..e719a08f67 --- /dev/null +++ b/libraries/Filter/LowPassFilter.h @@ -0,0 +1,89 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// @file LowPassFilter.h +/// @brief A class to implement a low pass filter without losing precision even for int types +/// the downside being that it's a little slower as it internally uses a float +/// and it consumes an extra 4 bytes of memory to hold the constant gain + +#ifndef LowPassFilter_h +#define LowPassFilter_h + +#include +#include + +// 1st parameter is the type of data being filtered. +template +class LowPassFilter : public Filter +{ + public: + // constructor + LowPassFilter(float gain); + + // apply - Add a new raw value to the filter, retrieve the filtered result + virtual T apply(T sample); + + // reset - clear the filter - next sample added will become the new base value + virtual void reset() { _base_value_set = false; }; + + // reset - clear the filter and provide the new base value + virtual void reset( T new_base_value ) { _base_value = new_base_value; _base_value_set = true;}; + + private: + float _gain; // gain value (like 0.02) applied to each new value + bool _base_value_set; // true if the base value has been set + float _base_value; // the number of samples in the filter, maxes out at size of the filter +}; + +// Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size) +typedef LowPassFilter LowPassFilterInt8; +typedef LowPassFilter LowPassFilterUInt8; + +typedef LowPassFilter LowPassFilterInt16; +typedef LowPassFilter LowPassFilterUInt16; + +typedef LowPassFilter LowPassFilterInt32; +typedef LowPassFilter LowPassFilterUInt32; + +typedef LowPassFilter LowPassFilterFloat; + +// Constructor ////////////////////////////////////////////////////////////// + +template +LowPassFilter::LowPassFilter(float gain) : + Filter(), + _gain(gain), + _base_value_set(false) +{ + // bounds checking on _gain + if( _gain > 1.0) { + _gain = 1.0; + }else if( _gain < 0.0 ) { + _gain = 0.0; + } +}; + +// Public Methods ////////////////////////////////////////////////////////////// + +template +T LowPassFilter::apply(T sample) +{ + // initailise _base_value if required + if( ! _base_value_set ) { + _base_value = sample; + _base_value_set = true; + } + + // do the filtering + _base_value = _gain * (float)sample + (1.0 - _gain) * _base_value; + + // return the value. Should be no need to check limits + return (T)_base_value; +} + +#endif \ No newline at end of file diff --git a/libraries/Filter/ModeFilter.h b/libraries/Filter/ModeFilter.h index bdb56a4246..4335ad8ae7 100644 --- a/libraries/Filter/ModeFilter.h +++ b/libraries/Filter/ModeFilter.h @@ -9,8 +9,6 @@ /// @file ModeFilter.h /// @brief A class to apply a mode filter which is basically picking the median value from the last x samples /// the filter size (i.e buffer size) should always be an odd number -/// -/// DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION #ifndef ModeFilter_h #define ModeFilter_h diff --git a/libraries/Filter/examples/Filter/Filter.pde b/libraries/Filter/examples/Filter/Filter.pde index 295389f377..3ee1094479 100644 --- a/libraries/Filter/examples/Filter/Filter.pde +++ b/libraries/Filter/examples/Filter/Filter.pde @@ -21,6 +21,8 @@ int16_t rangevalue[] = {31000, 31000, 50, 55, 60, 55, 10, 0, 31000}; ModeFilterInt16_Size5 mfilter(2); // buffer of 5 values, result will be from buffer element 2 (ie. the 3rd element which is the middle) //AverageFilterInt16_Size5 mfilter; // buffer of 5 values. result will be average of these 5 +AverageFilterUInt16_Size4 _temp_filter; + // Function to print contents of a filter // we need to ues FilterWithBuffer class because we want to access the individual elements void printFilter(FilterWithBufferInt16_Size5& filter) @@ -44,13 +46,38 @@ void setup() delay(500); } +// Read Raw Temperature values +void ReadTemp() +{ + static uint8_t next_num = 0; + static int32_t RawTemp = 0; + uint8_t buf[2]; + uint16_t _temp_sensor; + + next_num++; + buf[0] = next_num; //next_num; + buf[1] = 0xFF; + + _temp_sensor = buf[0]; + _temp_sensor = (_temp_sensor << 8) | buf[1]; + + RawTemp = _temp_filter.apply(_temp_sensor); + + Serial.printf("RT: %ld\n",RawTemp); +} + //Main loop where the action takes place void loop() { uint8_t i = 0; int16_t filtered_value; - while( i < 9 ) { + int16_t j; + + for(j=0; j<0xFF; j++ ) { + ReadTemp(); + } + /*while( i < 9 ) { // output to user Serial.printf("applying: %d\n",(int)rangevalue[i]); @@ -68,8 +95,8 @@ void loop() Serial.printf("The filtered value is: %d\n\n",(int)filtered_value); i++; - } - delay(100000); + }*/ + delay(10000); } diff --git a/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde new file mode 100644 index 0000000000..eff3d7bdee --- /dev/null +++ b/libraries/Filter/examples/LowPassFilter/LowPassFilter.pde @@ -0,0 +1,60 @@ +/* + Example sketch to demonstrate use of LowPassFilter library. + Code by Randy Mackay. DIYDrones.com +*/ + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // Filter library +#include // LowPassFilter class (inherits from Filter class) + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +FastSerialPort0(Serial); // FTDI/console + +// create a global instance of the class instead of local to avoid memory fragmentation +LowPassFilterInt16 low_pass_filter(0.02); // simple low pass filter which applies 2% of new data to old data + +// setup routine +void setup() +{ + // Open up a serial connection + Serial.begin(115200); + + // introduction + Serial.printf("ArduPilot LowPassFilter test ver 1.0\n\n"); + + // Wait for the serial connection + delay(500); +} + +//Main loop where the action takes place +void loop() +{ + int16_t i; + int16_t new_value; + int16_t filtered_value; + + // reset value to 100. If not reset the filter will start at the first value entered + low_pass_filter.reset(100); + + for( i=0; i<210; i++ ) { + + // new data value + new_value = 105; + + // output to user + Serial.printf("applying: %d",(int)new_value); + + // apply new value and retrieved filtered result + filtered_value = low_pass_filter.apply(new_value); + + // display results + Serial.printf("\toutput: %d\n\n",(int)filtered_value); + } + delay(10000); +} + + diff --git a/libraries/Filter/examples/LowPassFilter/Makefile b/libraries/Filter/examples/LowPassFilter/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/Filter/examples/LowPassFilter/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 3e5c53c1d9..d86853ff9e 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {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, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 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} +#define MAVLINK_MESSAGE_LENGTHS {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, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 10, 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} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {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, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 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} +#define MAVLINK_MESSAGE_CRCS {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, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 213, 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} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {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}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {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}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {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}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, {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}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -146,6 +146,7 @@ enum FENCE_BREACH #include "./mavlink_msg_ahrs.h" #include "./mavlink_msg_simstate.h" #include "./mavlink_msg_hwstatus.h" +#include "./mavlink_msg_radio.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_radio.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_radio.h new file mode 100644 index 0000000000..65d04a7f13 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_radio.h @@ -0,0 +1,254 @@ +// MESSAGE RADIO PACKING + +#define MAVLINK_MSG_ID_RADIO 166 + +typedef struct __mavlink_radio_t +{ + uint8_t rssi; ///< local signal strength + uint8_t remrssi; ///< remote signal strength + uint16_t rxerrors; ///< receive errors + uint16_t serrors; ///< serial errors + uint16_t fixed; ///< count of error corrected packets + uint16_t txbuf; ///< number of bytes available in transmit buffer +} mavlink_radio_t; + +#define MAVLINK_MSG_ID_RADIO_LEN 10 +#define MAVLINK_MSG_ID_166_LEN 10 + + + +#define MAVLINK_MESSAGE_INFO_RADIO { \ + "RADIO", \ + 6, \ + { { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_radio_t, rssi) }, \ + { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_radio_t, remrssi) }, \ + { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_t, rxerrors) }, \ + { "serrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_radio_t, serrors) }, \ + { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_radio_t, fixed) }, \ + { "txbuf", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_radio_t, txbuf) }, \ + } \ +} + + +/** + * @brief Pack a radio message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param rxerrors receive errors + * @param serrors serial errors + * @param fixed count of error corrected packets + * @param txbuf number of bytes available in transmit buffer + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, rssi); + _mav_put_uint8_t(buf, 1, remrssi); + _mav_put_uint16_t(buf, 2, rxerrors); + _mav_put_uint16_t(buf, 4, serrors); + _mav_put_uint16_t(buf, 6, fixed); + _mav_put_uint16_t(buf, 8, txbuf); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_radio_t packet; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.rxerrors = rxerrors; + packet.serrors = serrors; + packet.fixed = fixed; + packet.txbuf = txbuf; + + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO; + return mavlink_finalize_message(msg, system_id, component_id, 10); +} + +/** + * @brief Pack a radio message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param rxerrors receive errors + * @param serrors serial errors + * @param fixed count of error corrected packets + * @param txbuf number of bytes available in transmit buffer + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t rssi,uint8_t remrssi,uint16_t rxerrors,uint16_t serrors,uint16_t fixed,uint16_t txbuf) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, rssi); + _mav_put_uint8_t(buf, 1, remrssi); + _mav_put_uint16_t(buf, 2, rxerrors); + _mav_put_uint16_t(buf, 4, serrors); + _mav_put_uint16_t(buf, 6, fixed); + _mav_put_uint16_t(buf, 8, txbuf); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_radio_t packet; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.rxerrors = rxerrors; + packet.serrors = serrors; + packet.fixed = fixed; + packet.txbuf = txbuf; + + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10); +} + +/** + * @brief Encode a radio struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->rxerrors, radio->serrors, radio->fixed, radio->txbuf); +} + +/** + * @brief Send a radio message + * @param chan MAVLink channel to send the message + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param rxerrors receive errors + * @param serrors serial errors + * @param fixed count of error corrected packets + * @param txbuf number of bytes available in transmit buffer + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, rssi); + _mav_put_uint8_t(buf, 1, remrssi); + _mav_put_uint16_t(buf, 2, rxerrors); + _mav_put_uint16_t(buf, 4, serrors); + _mav_put_uint16_t(buf, 6, fixed); + _mav_put_uint16_t(buf, 8, txbuf); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 10); +#else + mavlink_radio_t packet; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.rxerrors = rxerrors; + packet.serrors = serrors; + packet.fixed = fixed; + packet.txbuf = txbuf; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 10); +#endif +} + +#endif + +// MESSAGE RADIO UNPACKING + + +/** + * @brief Get field rssi from radio message + * + * @return local signal strength + */ +static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field remrssi from radio message + * + * @return remote signal strength + */ +static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field rxerrors from radio message + * + * @return receive errors + */ +static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field serrors from radio message + * + * @return serial errors + */ +static inline uint16_t mavlink_msg_radio_get_serrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field fixed from radio message + * + * @return count of error corrected packets + */ +static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field txbuf from radio message + * + * @return number of bytes available in transmit buffer + */ +static inline uint16_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Decode a radio message into a struct + * + * @param msg The message to decode + * @param radio C-struct to decode the message contents into + */ +static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio) +{ +#if MAVLINK_NEED_BYTE_SWAP + radio->rssi = mavlink_msg_radio_get_rssi(msg); + radio->remrssi = mavlink_msg_radio_get_remrssi(msg); + radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg); + radio->serrors = mavlink_msg_radio_get_serrors(msg); + radio->fixed = mavlink_msg_radio_get_fixed(msg); + radio->txbuf = mavlink_msg_radio_get_txbuf(msg); +#else + memcpy(radio, _MAV_PAYLOAD(msg), 10); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 27485947c3..2e8fa205ab 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -827,6 +827,59 @@ static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_radio_t packet_in = { + 5, + 72, + 17339, + 17443, + 17547, + 17651, + }; + mavlink_radio_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.rssi = packet_in.rssi; + packet1.remrssi = packet_in.remrssi; + packet1.rxerrors = packet_in.rxerrors; + packet1.serrors = packet_in.serrors; + packet1.fixed = packet_in.fixed; + packet1.txbuf = packet_in.txbuf; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf ); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf ); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imsgid = MAVLINK_MSG_ID_RADIO; + return mavlink_finalize_message(msg, system_id, component_id, 10, 128); +} + +/** + * @brief Pack a radio message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param rxerrors receive errors + * @param serrors serial errors + * @param fixed count of error corrected packets + * @param txbuf number of bytes available in transmit buffer + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t rssi,uint8_t remrssi,uint16_t rxerrors,uint16_t serrors,uint16_t fixed,uint16_t txbuf) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, serrors); + _mav_put_uint16_t(buf, 4, fixed); + _mav_put_uint16_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 8, rssi); + _mav_put_uint8_t(buf, 9, remrssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_radio_t packet; + packet.rxerrors = rxerrors; + packet.serrors = serrors; + packet.fixed = fixed; + packet.txbuf = txbuf; + packet.rssi = rssi; + packet.remrssi = remrssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 128); +} + +/** + * @brief Encode a radio struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack(system_id, component_id, msg, radio->rssi, radio->remrssi, radio->rxerrors, radio->serrors, radio->fixed, radio->txbuf); +} + +/** + * @brief Send a radio message + * @param chan MAVLink channel to send the message + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param rxerrors receive errors + * @param serrors serial errors + * @param fixed count of error corrected packets + * @param txbuf number of bytes available in transmit buffer + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint16_t rxerrors, uint16_t serrors, uint16_t fixed, uint16_t txbuf) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, serrors); + _mav_put_uint16_t(buf, 4, fixed); + _mav_put_uint16_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 8, rssi); + _mav_put_uint8_t(buf, 9, remrssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, 10, 128); +#else + mavlink_radio_t packet; + packet.rxerrors = rxerrors; + packet.serrors = serrors; + packet.fixed = fixed; + packet.txbuf = txbuf; + packet.rssi = rssi; + packet.remrssi = remrssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)&packet, 10, 128); +#endif +} + +#endif + +// MESSAGE RADIO UNPACKING + + +/** + * @brief Get field rssi from radio message + * + * @return local signal strength + */ +static inline uint8_t mavlink_msg_radio_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field remrssi from radio message + * + * @return remote signal strength + */ +static inline uint8_t mavlink_msg_radio_get_remrssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field rxerrors from radio message + * + * @return receive errors + */ +static inline uint16_t mavlink_msg_radio_get_rxerrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field serrors from radio message + * + * @return serial errors + */ +static inline uint16_t mavlink_msg_radio_get_serrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field fixed from radio message + * + * @return count of error corrected packets + */ +static inline uint16_t mavlink_msg_radio_get_fixed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field txbuf from radio message + * + * @return number of bytes available in transmit buffer + */ +static inline uint16_t mavlink_msg_radio_get_txbuf(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Decode a radio message into a struct + * + * @param msg The message to decode + * @param radio C-struct to decode the message contents into + */ +static inline void mavlink_msg_radio_decode(const mavlink_message_t* msg, mavlink_radio_t* radio) +{ +#if MAVLINK_NEED_BYTE_SWAP + radio->rxerrors = mavlink_msg_radio_get_rxerrors(msg); + radio->serrors = mavlink_msg_radio_get_serrors(msg); + radio->fixed = mavlink_msg_radio_get_fixed(msg); + radio->txbuf = mavlink_msg_radio_get_txbuf(msg); + radio->rssi = mavlink_msg_radio_get_rssi(msg); + radio->remrssi = mavlink_msg_radio_get_remrssi(msg); +#else + memcpy(radio, _MAV_PAYLOAD(msg), 10); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h index 9bc89d25bb..14c0a78b34 100644 --- a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h @@ -827,6 +827,59 @@ static void mavlink_test_hwstatus(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_radio_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 29, + 96, + }; + mavlink_radio_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.rxerrors = packet_in.rxerrors; + packet1.serrors = packet_in.serrors; + packet1.fixed = packet_in.fixed; + packet1.txbuf = packet_in.txbuf; + packet1.rssi = packet_in.rssi; + packet1.remrssi = packet_in.remrssi; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_pack(system_id, component_id, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf ); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_radio_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.rssi , packet1.remrssi , packet1.rxerrors , packet1.serrors , packet1.fixed , packet1.txbuf ); + mavlink_msg_radio_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iboard voltage (mV) I2C error count + + + Status generated by radio + local signal strength + remote signal strength + receive errors + serial errors + count of error corrected packets + number of bytes available in transmit buffer + diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml index f1bbfd6ce6..ced94d83e8 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml @@ -255,5 +255,15 @@ board voltage (mV) I2C error count + + + Status generated by radio + local signal strength + remote signal strength + receive errors + serial errors + count of error corrected packets + number of bytes available in transmit buffer +