Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-04-01 16:23:42 -07:00
parent 5c8630efec 4ddeb82b26
commit 0a9fd22243
112 changed files with 4329 additions and 817 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
ArduCopter/ArduCopter.pde
View File

@ -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 <AP_Declination.h> // 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

78
ArduCopter/Attitude.pde
View File

@ -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

4
ArduCopter/GCS.h
View File

@ -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;

13
ArduCopter/GCS_Mavlink.pde
View File

@ -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

48
ArduCopter/Log.pde
View File

@ -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

39
ArduCopter/config.h
View File

@ -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

11
ArduCopter/control_modes.pde
View File

@ -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);

24
ArduCopter/defines.h
View File

@ -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
// ----------------

8
ArduCopter/motors.pde
View File

@ -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

27
ArduCopter/system.pde
View File

@ -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

4
ArduCopter/test.pde
View File

@ -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();

11
ArduPlane/ArduPlane.pde
View File

@ -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 <AP_Declination.h> // 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;
}
}

4
ArduPlane/GCS.h
View File

@ -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;

16
ArduPlane/GCS_Mavlink.pde
View File

@ -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;

2
ArduPlane/Parameters.h
View File

@ -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;

1
ArduPlane/Parameters.pde
View File

@ -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"),

4
ArduPlane/config.h
View File

@ -418,10 +418,6 @@
# define GROUND_START_DELAY 0
#endif
#ifndef AUTOMATIC_DECLINATION
#define AUTOMATIC_DECLINATION DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START
//

42
ArduPlane/system.pde
View File

@ -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

2
ArduPlane/test.pde
View File

@ -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;
}

155
Tools/ArdupilotMegaPlanner/Antenna/ArduTracker.cs Normal file
View File

@ -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; }
/// <summary>
/// 0-360
/// </summary>
public double TrimPan { get; set; }
/// <summary>
/// -90 - 90
/// </summary>
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;
}
}
}

2
Tools/ArdupilotMegaPlanner/Antenna/ITrackerOutput.cs
View File

@ -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; }

6
Tools/ArdupilotMegaPlanner/Antenna/Maestro.cs
View File

@ -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);

323
Tools/ArdupilotMegaPlanner/Antenna/Tracker.Designer.cs generated
View File

@ -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;
}
}

97
Tools/ArdupilotMegaPlanner/Antenna/Tracker.cs
View File

@ -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();
}
}
}

117
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -213,6 +213,7 @@
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="Antenna\ArduTracker.cs" />
<Compile Include="Antenna\ITrackerOutput.cs" />
<Compile Include="Antenna\Maestro.cs" />
<Compile Include="Antenna\Tracker.cs">
@ -222,6 +223,18 @@
<DependentUpon>Tracker.cs</DependentUpon>
</Compile>
<Compile Include="CodeGen.cs" />
<Compile Include="Controls\BackstageView\BackstageView.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Controls\BackstageView\BackstageView.designer.cs">
<DependentUpon>BackstageView.cs</DependentUpon>
</Compile>
<Compile Include="Controls\BackstageView\BackstageViewButton.cs">
<SubType>Component</SubType>
</Compile>
<Compile Include="Controls\BackstageView\BackStageViewMenuPanel.cs">
<SubType>Component</SubType>
</Compile>
<Compile Include="Controls\CustomMessageBox.cs" />
<Compile Include="Controls\ProgressReporterDialogue.cs">
<SubType>Form</SubType>
@ -229,7 +242,74 @@
<Compile Include="Controls\ProgressReporterDialogue.designer.cs">
<DependentUpon>ProgressReporterDialogue.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigAccelerometerCalibration.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigAccelerometerCalibration.Designer.cs">
<DependentUpon>ConfigAccelerometerCalibration.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigArducopter.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigArducopter.Designer.cs">
<DependentUpon>ConfigArducopter.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigArduplane.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigArduplane.Designer.cs">
<DependentUpon>ConfigArduplane.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigBatteryMonitoring.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigBatteryMonitoring.Designer.cs">
<DependentUpon>ConfigBatteryMonitoring.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigFlightModes.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigFlightModes.Designer.cs">
<DependentUpon>ConfigFlightModes.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigHardwareOptions.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigHardwareOptions.Designer.cs">
<DependentUpon>ConfigHardwareOptions.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigPlanner.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigPlanner.Designer.cs">
<DependentUpon>ConfigPlanner.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigRadioInput.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigRadioInput.Designer.cs">
<DependentUpon>ConfigRadioInput.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigTradHeli.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigTradHeli.Designer.cs">
<DependentUpon>ConfigTradHeli.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\Configuration.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\Configuration.Designer.cs">
<DependentUpon>Configuration.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigRawParams.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="GCSViews\ConfigurationView\ConfigRawParams.Designer.cs">
<DependentUpon>ConfigRawParams.cs</DependentUpon>
</Compile>
<Compile Include="MagCalib.cs" />
<Compile Include="PIDTunning.cs" />
<Compile Include="Radio\3DRradio.cs">
<SubType>Form</SubType>
</Compile>
@ -446,14 +526,49 @@
</Compile>
<Compile Include="Radio\Uploader.cs" />
<Compile Include="LangUtility.cs" />
<Compile Include="test.cs" />
<Compile Include="ThemeManager.cs" />
<EmbeddedResource Include="Antenna\Tracker.resx">
<DependentUpon>Tracker.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Controls\BackstageView\BackstageView.resx">
<DependentUpon>BackstageView.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Controls\ProgressReporterDialogue.resx">
<DependentUpon>ProgressReporterDialogue.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigAccelerometerCalibration.resx">
<DependentUpon>ConfigAccelerometerCalibration.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigArducopter.resx">
<DependentUpon>ConfigArducopter.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigArduplane.resx">
<DependentUpon>ConfigArduplane.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigBatteryMonitoring.resx">
<DependentUpon>ConfigBatteryMonitoring.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigFlightModes.resx">
<DependentUpon>ConfigFlightModes.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigHardwareOptions.resx">
<DependentUpon>ConfigHardwareOptions.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigPlanner.resx">
<DependentUpon>ConfigPlanner.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigRadioInput.resx">
<DependentUpon>ConfigRadioInput.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigRawParams.resx">
<DependentUpon>ConfigRawParams.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\ConfigTradHeli.resx">
<DependentUpon>ConfigTradHeli.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\ConfigurationView\Configuration.resx">
<DependentUpon>Configuration.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="Radio\3DRradio.resx">
<DependentUpon>3DRradio.cs</DependentUpon>
</EmbeddedResource>

7
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -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
}

2
Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs
View File

@ -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; }

56
Tools/ArdupilotMegaPlanner/CommsSerialPort.cs
View File

@ -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;
}
}
}

2
Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs
View File

@ -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

2
Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs
View File

@ -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

33
Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackStageViewMenuPanel.cs Normal file
View File

@ -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);
}
}
}

73
Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.Designer.cs generated Normal file
View File

@ -0,0 +1,73 @@
namespace ArdupilotMega.Controls.BackstageView
{
partial class BackstageView
{
/// <summary>
/// Required designer variable.
/// </summary>
private System.ComponentModel.IContainer components = null;
/// <summary>
/// Clean up any resources being used.
/// </summary>
/// <param name="disposing">true if managed resources should be disposed; otherwise, false.</param>
protected override void Dispose(bool disposing)
{
if (disposing && (components != null))
{
components.Dispose();
}
base.Dispose(disposing);
}
#region Component Designer generated code
/// <summary>
/// Required method for Designer support - do not modify
/// the contents of this method with the code editor.
/// </summary>
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;
}
}

225
Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.cs Normal file
View File

@ -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<BackstageViewPage> _pages= new List<BackstageViewPage>();
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>())
{
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<BackstageViewButton>().Single(b => b.Tag == _activePage);
oldButton.IsSelected = false;
associatedPage.Page.Visible = true;
var newButton = this.pnlMenu.Controls.OfType<BackstageViewButton>().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; }
}
}
}

120
Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageView.resx Normal file
View File

@ -0,0 +1,120 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
</root>

150
Tools/ArdupilotMegaPlanner/Controls/BackstageView/BackstageViewButton.cs Normal file
View File

@ -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;
}
/// <summary>
/// Whether this button should show the selected style
/// </summary>
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;
}
}
}
}

1
Tools/ArdupilotMegaPlanner/Controls/CustomMessageBox.cs
View File

@ -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);

22
Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
View File

@ -4,7 +4,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560-2.hex</url2560-2>
<name>ArduPlane V2.30 </name>
<name>ArduPlane V2.32 </name>
<desc></desc>
<format_version>12</format_version>
</Firmware>
@ -12,7 +12,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560-2.hex</url2560-2>
<name>ArduPlane V2.30 HIL</name>
<name>ArduPlane V2.32 HIL</name>
<desc>
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
@ -47,7 +47,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Quad</name>
<name>ArduCopter V2.5.3 Quad</name>
<desc>
#define FRAME_CONFIG QUAD_FRAME
@ -58,7 +58,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Tri</name>
<name>ArduCopter V2.5.3 Tri</name>
<desc>
#define FRAME_CONFIG TRI_FRAME
@ -69,7 +69,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Hexa X</name>
<name>ArduCopter V2.5.3 Hexa X</name>
<desc>
#define FRAME_CONFIG HEXA_FRAME
@ -80,7 +80,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Y6</name>
<name>ArduCopter V2.5.3 Y6</name>
<desc>
#define FRAME_CONFIG Y6_FRAME
@ -91,7 +91,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Octa V</name>
<name>ArduCopter V2.5.3 Octa V</name>
<desc>
#define FRAME_CONFIG OCTA_FRAME
#define FRAME_ORIENTATION V_FRAME
@ -103,7 +103,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Octa X</name>
<name>ArduCopter V2.5.3 Octa X</name>
<desc>
#define FRAME_CONFIG OCTA_FRAME
@ -114,7 +114,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Heli (2560 only)</name>
<name>ArduCopter V2.5.3 Heli (2560 only)</name>
<desc>
#define AUTO_RESET_LOITER 0
#define FRAME_CONFIG HELI_FRAME
@ -162,7 +162,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Quad Hil</name>
<name>ArduCopter V2.5.3 Quad Hil</name>
<desc>
#define FRAME_CONFIG QUAD_FRAME
#define FRAME_ORIENTATION PLUS_FRAME
@ -178,7 +178,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560-2.hex</url2560-2>
<name>ArduCopter V2.5 Heli Hil</name>
<name>ArduCopter V2.5.3 Heli Hil</name>
<desc>
#define HIL_MODE HIL_MODE_ATTITUDE

30
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs generated
View File

@ -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");

32
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs
View File

@ -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)
{
}
}
}

4
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -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";
}

6
Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs
View File

@ -201,9 +201,11 @@ namespace ArdupilotMega.GCSViews
}
catch { return; }
}
try
{
comPort.Write("\n\n\n");
}
catch { return; }
while (threadrun)
{
try

4
Tools/ArdupilotMegaPlanner/Log.cs
View File

@ -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")));

27
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -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);

26
Tools/ArdupilotMegaPlanner/MagCalib.cs
View File

@ -21,7 +21,7 @@ namespace ArdupilotMega
/// <summary>
/// Self contained process tlog and save/display offsets
/// </summary>
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
/// </summary>
/// <param name="fn">Filename</param>
/// <returns>Offsets</returns>
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<float, float, float>(
@ -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;

92
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -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 { }

268
Tools/ArdupilotMegaPlanner/MavlinkLog.cs
View File

@ -36,9 +36,15 @@ namespace ArdupilotMega
List<string> selection = new List<string>();
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<string> GetLogFileValidFields(string logfile)
@ -701,23 +764,36 @@ namespace ArdupilotMega
Form selectform = SelectDataToGraphForm();
Hashtable seenIt = new Hashtable();
selection = new List<string>();
List<string> options = new List<string>();
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<string> 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;

14
Tools/ArdupilotMegaPlanner/MavlinkLog.resx
View File

@ -123,7 +123,7 @@
</data>
<assembly alias="System.Drawing" name="System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" />
<data name="BUT_redokml.Location" type="System.Drawing.Point, System.Drawing">
<value>45, 12</value>
<value>143, 12</value>
</data>
<data name="BUT_redokml.Size" type="System.Drawing.Size, System.Drawing">
<value>116, 23</value>
@ -133,7 +133,7 @@
<value>8</value>
</data>
<data name="BUT_redokml.Text" xml:space="preserve">
<value>Create KML</value>
<value>Create KML + GPX</value>
</data>
<data name="&gt;&gt;BUT_redokml.Name" xml:space="preserve">
<value>BUT_redokml</value>
@ -154,7 +154,7 @@
<value>10, 42</value>
</data>
<data name="progressBar1.Size" type="System.Drawing.Size, System.Drawing">
<value>432, 26</value>
<value>622, 26</value>
</data>
<data name="progressBar1.TabIndex" type="System.Int32, mscorlib">
<value>9</value>
@ -178,7 +178,7 @@
<value>NoControl</value>
</data>
<data name="BUT_humanreadable.Location" type="System.Drawing.Point, System.Drawing">
<value>167, 12</value>
<value>265, 12</value>
</data>
<data name="BUT_humanreadable.Size" type="System.Drawing.Size, System.Drawing">
<value>116, 23</value>
@ -208,7 +208,7 @@
<value>NoControl</value>
</data>
<data name="BUT_graphmavlog.Location" type="System.Drawing.Point, System.Drawing">
<value>289, 12</value>
<value>387, 12</value>
</data>
<data name="BUT_graphmavlog.Size" type="System.Drawing.Size, System.Drawing">
<value>116, 23</value>
@ -238,7 +238,7 @@
<value>10, 74</value>
</data>
<data name="zg1.Size" type="System.Drawing.Size, System.Drawing">
<value>434, 293</value>
<value>624, 293</value>
</data>
<data name="zg1.TabIndex" type="System.Int32, mscorlib">
<value>12</value>
@ -262,7 +262,7 @@
<value>6, 13</value>
</data>
<data name="$this.ClientSize" type="System.Drawing.Size, System.Drawing">
<value>456, 379</value>
<value>646, 379</value>
</data>
<data name="$this.Icon" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>

46
Tools/ArdupilotMegaPlanner/PIDTunning.cs Normal file
View File

@ -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<double[],double> 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);
}
}
}
}
}

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -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("")]

80
Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs generated
View File

@ -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;
}
}

45
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -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");
}
}
}

718
Tools/ArdupilotMegaPlanner/Setup/Setup.resx
View File

@ -310,7 +310,7 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabRadioIn.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabRadioIn.ZOrder" xml:space="preserve">
<value>0</value>
@ -682,23 +682,11 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabModes.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabModes.ZOrder" xml:space="preserve">
<value>1</value>
</data>
<data name="BUT_MagCalibration.Location" type="System.Drawing.Point, System.Drawing">
<value>405, 25</value>
</data>
<data name="BUT_MagCalibration.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 23</value>
</data>
<data name="BUT_MagCalibration.TabIndex" type="System.Int32, mscorlib">
<value>33</value>
</data>
<data name="BUT_MagCalibration.Text" xml:space="preserve">
<value>Calibration</value>
</data>
<data name="&gt;&gt;BUT_MagCalibration.Name" xml:space="preserve">
<value>BUT_MagCalibration</value>
</data>
@ -711,21 +699,6 @@
<data name="&gt;&gt;BUT_MagCalibration.ZOrder" xml:space="preserve">
<value>0</value>
</data>
<data name="label27.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="label27.Location" type="System.Drawing.Point, System.Drawing">
<value>510, 57</value>
</data>
<data name="label27.Size" type="System.Drawing.Size, System.Drawing">
<value>150, 20</value>
</data>
<data name="label27.TabIndex" type="System.Int32, mscorlib">
<value>32</value>
</data>
<data name="label27.Text" xml:space="preserve">
<value>in Degrees eg 2° 3' W is -2.3</value>
</data>
<data name="&gt;&gt;label27.Name" xml:space="preserve">
<value>label27</value>
</data>
@ -738,24 +711,6 @@
<data name="&gt;&gt;label27.ZOrder" xml:space="preserve">
<value>1</value>
</data>
<data name="CMB_sonartype.Items" xml:space="preserve">
<value>XL-EZ0</value>
</data>
<data name="CMB_sonartype.Items1" xml:space="preserve">
<value>LV-EZ0</value>
</data>
<data name="CMB_sonartype.Items2" xml:space="preserve">
<value>XL-EZL0</value>
</data>
<data name="CMB_sonartype.Location" type="System.Drawing.Point, System.Drawing">
<value>308, 134</value>
</data>
<data name="CMB_sonartype.Size" type="System.Drawing.Size, System.Drawing">
<value>121, 21</value>
</data>
<data name="CMB_sonartype.TabIndex" type="System.Int32, mscorlib">
<value>31</value>
</data>
<data name="&gt;&gt;CMB_sonartype.Name" xml:space="preserve">
<value>CMB_sonartype</value>
</data>
@ -768,21 +723,6 @@
<data name="&gt;&gt;CMB_sonartype.ZOrder" xml:space="preserve">
<value>2</value>
</data>
<data name="CHK_enableoptflow.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enableoptflow.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 297</value>
</data>
<data name="CHK_enableoptflow.Size" type="System.Drawing.Size, System.Drawing">
<value>134, 19</value>
</data>
<data name="CHK_enableoptflow.TabIndex" type="System.Int32, mscorlib">
<value>30</value>
</data>
<data name="CHK_enableoptflow.Text" xml:space="preserve">
<value>Enable Optical Flow</value>
</data>
<data name="&gt;&gt;CHK_enableoptflow.Name" xml:space="preserve">
<value>CHK_enableoptflow</value>
</data>
@ -795,21 +735,6 @@
<data name="&gt;&gt;CHK_enableoptflow.ZOrder" xml:space="preserve">
<value>3</value>
</data>
<data name="pictureBox2.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox2.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox2.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 271</value>
</data>
<data name="pictureBox2.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox2.TabIndex" type="System.Int32, mscorlib">
<value>29</value>
</data>
<data name="&gt;&gt;pictureBox2.Name" xml:space="preserve">
<value>pictureBox2</value>
</data>
@ -822,24 +747,6 @@
<data name="&gt;&gt;pictureBox2.ZOrder" xml:space="preserve">
<value>4</value>
</data>
<data name="linkLabelmagdec.AutoSize" type="System.Boolean, mscorlib">
<value>True</value>
</data>
<data name="linkLabelmagdec.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="linkLabelmagdec.Location" type="System.Drawing.Point, System.Drawing">
<value>390, 80</value>
</data>
<data name="linkLabelmagdec.Size" type="System.Drawing.Size, System.Drawing">
<value>104, 13</value>
</data>
<data name="linkLabelmagdec.TabIndex" type="System.Int32, mscorlib">
<value>28</value>
</data>
<data name="linkLabelmagdec.Text" xml:space="preserve">
<value>Declination WebSite</value>
</data>
<data name="&gt;&gt;linkLabelmagdec.Name" xml:space="preserve">
<value>linkLabelmagdec</value>
</data>
@ -852,21 +759,6 @@
<data name="&gt;&gt;linkLabelmagdec.ZOrder" xml:space="preserve">
<value>5</value>
</data>
<data name="label100.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="label100.Location" type="System.Drawing.Point, System.Drawing">
<value>305, 57</value>
</data>
<data name="label100.Size" type="System.Drawing.Size, System.Drawing">
<value>72, 16</value>
</data>
<data name="label100.TabIndex" type="System.Int32, mscorlib">
<value>23</value>
</data>
<data name="label100.Text" xml:space="preserve">
<value>Declination</value>
</data>
<data name="&gt;&gt;label100.Name" xml:space="preserve">
<value>label100</value>
</data>
@ -879,18 +771,6 @@
<data name="&gt;&gt;label100.ZOrder" xml:space="preserve">
<value>6</value>
</data>
<data name="TXT_declination.Location" type="System.Drawing.Point, System.Drawing">
<value>383, 57</value>
</data>
<data name="TXT_declination.Size" type="System.Drawing.Size, System.Drawing">
<value>121, 20</value>
</data>
<data name="TXT_declination.TabIndex" type="System.Int32, mscorlib">
<value>20</value>
</data>
<data name="TXT_declination.ToolTip" xml:space="preserve">
<value>Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3</value>
</data>
<data name="&gt;&gt;TXT_declination.Name" xml:space="preserve">
<value>TXT_declination</value>
</data>
@ -903,21 +783,6 @@
<data name="&gt;&gt;TXT_declination.ZOrder" xml:space="preserve">
<value>7</value>
</data>
<data name="CHK_enableairspeed.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enableairspeed.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 214</value>
</data>
<data name="CHK_enableairspeed.Size" type="System.Drawing.Size, System.Drawing">
<value>103, 17</value>
</data>
<data name="CHK_enableairspeed.TabIndex" type="System.Int32, mscorlib">
<value>24</value>
</data>
<data name="CHK_enableairspeed.Text" xml:space="preserve">
<value>Enable Airspeed</value>
</data>
<data name="&gt;&gt;CHK_enableairspeed.Name" xml:space="preserve">
<value>CHK_enableairspeed</value>
</data>
@ -930,21 +795,6 @@
<data name="&gt;&gt;CHK_enableairspeed.ZOrder" xml:space="preserve">
<value>8</value>
</data>
<data name="CHK_enablesonar.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enablesonar.Location" type="System.Drawing.Point, System.Drawing">
<value>159, 136</value>
</data>
<data name="CHK_enablesonar.Size" type="System.Drawing.Size, System.Drawing">
<value>90, 17</value>
</data>
<data name="CHK_enablesonar.TabIndex" type="System.Int32, mscorlib">
<value>25</value>
</data>
<data name="CHK_enablesonar.Text" xml:space="preserve">
<value>Enable Sonar</value>
</data>
<data name="&gt;&gt;CHK_enablesonar.Name" xml:space="preserve">
<value>CHK_enablesonar</value>
</data>
@ -957,21 +807,6 @@
<data name="&gt;&gt;CHK_enablesonar.ZOrder" xml:space="preserve">
<value>9</value>
</data>
<data name="CHK_enablecompass.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enablecompass.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 56</value>
</data>
<data name="CHK_enablecompass.Size" type="System.Drawing.Size, System.Drawing">
<value>105, 17</value>
</data>
<data name="CHK_enablecompass.TabIndex" type="System.Int32, mscorlib">
<value>27</value>
</data>
<data name="CHK_enablecompass.Text" xml:space="preserve">
<value>Enable Compass</value>
</data>
<data name="&gt;&gt;CHK_enablecompass.Name" xml:space="preserve">
<value>CHK_enablecompass</value>
</data>
@ -984,21 +819,6 @@
<data name="&gt;&gt;CHK_enablecompass.ZOrder" xml:space="preserve">
<value>10</value>
</data>
<data name="pictureBox4.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox4.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox4.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 188</value>
</data>
<data name="pictureBox4.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox4.TabIndex" type="System.Int32, mscorlib">
<value>3</value>
</data>
<data name="&gt;&gt;pictureBox4.Name" xml:space="preserve">
<value>pictureBox4</value>
</data>
@ -1011,21 +831,6 @@
<data name="&gt;&gt;pictureBox4.ZOrder" xml:space="preserve">
<value>11</value>
</data>
<data name="pictureBox3.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox3.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox3.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 106</value>
</data>
<data name="pictureBox3.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox3.TabIndex" type="System.Int32, mscorlib">
<value>2</value>
</data>
<data name="&gt;&gt;pictureBox3.Name" xml:space="preserve">
<value>pictureBox3</value>
</data>
@ -1038,27 +843,6 @@
<data name="&gt;&gt;pictureBox3.ZOrder" xml:space="preserve">
<value>12</value>
</data>
<data name="pictureBox1.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox1.ErrorImage" type="System.Resources.ResXNullRef, System.Windows.Forms">
<value />
</data>
<data name="pictureBox1.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox1.InitialImage" type="System.Resources.ResXNullRef, System.Windows.Forms">
<value />
</data>
<data name="pictureBox1.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 25</value>
</data>
<data name="pictureBox1.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox1.TabIndex" type="System.Int32, mscorlib">
<value>0</value>
</data>
<data name="&gt;&gt;pictureBox1.Name" xml:space="preserve">
<value>pictureBox1</value>
</data>
@ -1093,7 +877,7 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabHardware.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabHardware.ZOrder" xml:space="preserve">
<value>2</value>
@ -1228,11 +1012,95 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabBattery.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabBattery.ZOrder" xml:space="preserve">
<value>3</value>
</data>
<data name="label48.AutoSize" type="System.Boolean, mscorlib">
<value>True</value>
</data>
<data name="label48.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="label48.Location" type="System.Drawing.Point, System.Drawing">
<value>228, 170</value>
</data>
<data name="label48.Size" type="System.Drawing.Size, System.Drawing">
<value>212, 13</value>
</data>
<data name="label48.TabIndex" type="System.Int32, mscorlib">
<value>11</value>
</data>
<data name="label48.Text" xml:space="preserve">
<value>Level your plane to set default accel offsets</value>
</data>
<data name="&gt;&gt;label48.Name" xml:space="preserve">
<value>label48</value>
</data>
<data name="&gt;&gt;label48.Type" xml:space="preserve">
<value>System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;label48.Parent" xml:space="preserve">
<value>tabArduplane</value>
</data>
<data name="&gt;&gt;label48.ZOrder" xml:space="preserve">
<value>0</value>
</data>
<data name="BUT_levelplane.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="BUT_levelplane.Location" type="System.Drawing.Point, System.Drawing">
<value>285, 199</value>
</data>
<data name="BUT_levelplane.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 23</value>
</data>
<data name="BUT_levelplane.TabIndex" type="System.Int32, mscorlib">
<value>10</value>
</data>
<data name="BUT_levelplane.Text" xml:space="preserve">
<value>Level</value>
</data>
<data name="&gt;&gt;BUT_levelplane.Name" xml:space="preserve">
<value>BUT_levelplane</value>
</data>
<data name="&gt;&gt;BUT_levelplane.Type" xml:space="preserve">
<value>ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null</value>
</data>
<data name="&gt;&gt;BUT_levelplane.Parent" xml:space="preserve">
<value>tabArduplane</value>
</data>
<data name="&gt;&gt;BUT_levelplane.ZOrder" xml:space="preserve">
<value>1</value>
</data>
<data name="tabArduplane.Location" type="System.Drawing.Point, System.Drawing">
<value>4, 22</value>
</data>
<data name="tabArduplane.Padding" type="System.Windows.Forms.Padding, System.Windows.Forms">
<value>3, 3, 3, 3</value>
</data>
<data name="tabArduplane.Size" type="System.Drawing.Size, System.Drawing">
<value>666, 393</value>
</data>
<data name="tabArduplane.TabIndex" type="System.Int32, mscorlib">
<value>7</value>
</data>
<data name="tabArduplane.Text" xml:space="preserve">
<value>ArduPlane</value>
</data>
<data name="&gt;&gt;tabArduplane.Name" xml:space="preserve">
<value>tabArduplane</value>
</data>
<data name="&gt;&gt;tabArduplane.Type" xml:space="preserve">
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabArduplane.Parent" xml:space="preserve">
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabArduplane.ZOrder" xml:space="preserve">
<value>4</value>
</data>
<data name="&gt;&gt;label28.Name" xml:space="preserve">
<value>label28</value>
</data>
@ -1324,10 +1192,10 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabArducopter.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabArducopter.ZOrder" xml:space="preserve">
<value>4</value>
<value>5</value>
</data>
<data name="&gt;&gt;groupBox5.Name" xml:space="preserve">
<value>groupBox5</value>
@ -1792,33 +1660,33 @@
<value>System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabHeli.Parent" xml:space="preserve">
<value>tabControl1</value>
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabHeli.ZOrder" xml:space="preserve">
<value>5</value>
<value>6</value>
</data>
<data name="tabControl1.Dock" type="System.Windows.Forms.DockStyle, System.Windows.Forms">
<data name="Tabs.Dock" type="System.Windows.Forms.DockStyle, System.Windows.Forms">
<value>Fill</value>
</data>
<data name="tabControl1.Location" type="System.Drawing.Point, System.Drawing">
<data name="Tabs.Location" type="System.Drawing.Point, System.Drawing">
<value>0, 0</value>
</data>
<data name="tabControl1.Size" type="System.Drawing.Size, System.Drawing">
<data name="Tabs.Size" type="System.Drawing.Size, System.Drawing">
<value>674, 419</value>
</data>
<data name="tabControl1.TabIndex" type="System.Int32, mscorlib">
<data name="Tabs.TabIndex" type="System.Int32, mscorlib">
<value>93</value>
</data>
<data name="&gt;&gt;tabControl1.Name" xml:space="preserve">
<value>tabControl1</value>
<data name="&gt;&gt;Tabs.Name" xml:space="preserve">
<value>Tabs</value>
</data>
<data name="&gt;&gt;tabControl1.Type" xml:space="preserve">
<data name="&gt;&gt;Tabs.Type" xml:space="preserve">
<value>System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;tabControl1.Parent" xml:space="preserve">
<data name="&gt;&gt;Tabs.Parent" xml:space="preserve">
<value>$this</value>
</data>
<data name="&gt;&gt;tabControl1.ZOrder" xml:space="preserve">
<data name="&gt;&gt;Tabs.ZOrder" xml:space="preserve">
<value>0</value>
</data>
<data name="&gt;&gt;CHK_mixmode.Name" xml:space="preserve">
@ -3180,6 +3048,390 @@
<data name="&gt;&gt;BUT_SaveModes.ZOrder" xml:space="preserve">
<value>28</value>
</data>
<data name="BUT_MagCalibration.Location" type="System.Drawing.Point, System.Drawing">
<value>405, 25</value>
</data>
<data name="BUT_MagCalibration.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 23</value>
</data>
<data name="BUT_MagCalibration.TabIndex" type="System.Int32, mscorlib">
<value>33</value>
</data>
<data name="BUT_MagCalibration.Text" xml:space="preserve">
<value>Calibration</value>
</data>
<data name="&gt;&gt;BUT_MagCalibration.Name" xml:space="preserve">
<value>BUT_MagCalibration</value>
</data>
<data name="&gt;&gt;BUT_MagCalibration.Type" xml:space="preserve">
<value>ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null</value>
</data>
<data name="&gt;&gt;BUT_MagCalibration.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;BUT_MagCalibration.ZOrder" xml:space="preserve">
<value>0</value>
</data>
<data name="label27.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="label27.Location" type="System.Drawing.Point, System.Drawing">
<value>510, 57</value>
</data>
<data name="label27.Size" type="System.Drawing.Size, System.Drawing">
<value>150, 20</value>
</data>
<data name="label27.TabIndex" type="System.Int32, mscorlib">
<value>32</value>
</data>
<data name="label27.Text" xml:space="preserve">
<value>in Degrees eg 2° 3' W is -2.3</value>
</data>
<data name="&gt;&gt;label27.Name" xml:space="preserve">
<value>label27</value>
</data>
<data name="&gt;&gt;label27.Type" xml:space="preserve">
<value>System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;label27.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;label27.ZOrder" xml:space="preserve">
<value>1</value>
</data>
<data name="CMB_sonartype.Items" xml:space="preserve">
<value>XL-EZ0</value>
</data>
<data name="CMB_sonartype.Items1" xml:space="preserve">
<value>LV-EZ0</value>
</data>
<data name="CMB_sonartype.Items2" xml:space="preserve">
<value>XL-EZL0</value>
</data>
<data name="CMB_sonartype.Location" type="System.Drawing.Point, System.Drawing">
<value>308, 134</value>
</data>
<data name="CMB_sonartype.Size" type="System.Drawing.Size, System.Drawing">
<value>121, 21</value>
</data>
<data name="CMB_sonartype.TabIndex" type="System.Int32, mscorlib">
<value>31</value>
</data>
<data name="&gt;&gt;CMB_sonartype.Name" xml:space="preserve">
<value>CMB_sonartype</value>
</data>
<data name="&gt;&gt;CMB_sonartype.Type" xml:space="preserve">
<value>System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;CMB_sonartype.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;CMB_sonartype.ZOrder" xml:space="preserve">
<value>2</value>
</data>
<data name="CHK_enableoptflow.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enableoptflow.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 297</value>
</data>
<data name="CHK_enableoptflow.Size" type="System.Drawing.Size, System.Drawing">
<value>134, 19</value>
</data>
<data name="CHK_enableoptflow.TabIndex" type="System.Int32, mscorlib">
<value>30</value>
</data>
<data name="CHK_enableoptflow.Text" xml:space="preserve">
<value>Enable Optical Flow</value>
</data>
<data name="&gt;&gt;CHK_enableoptflow.Name" xml:space="preserve">
<value>CHK_enableoptflow</value>
</data>
<data name="&gt;&gt;CHK_enableoptflow.Type" xml:space="preserve">
<value>System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;CHK_enableoptflow.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;CHK_enableoptflow.ZOrder" xml:space="preserve">
<value>3</value>
</data>
<data name="pictureBox2.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox2.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox2.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 271</value>
</data>
<data name="pictureBox2.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox2.TabIndex" type="System.Int32, mscorlib">
<value>29</value>
</data>
<data name="&gt;&gt;pictureBox2.Name" xml:space="preserve">
<value>pictureBox2</value>
</data>
<data name="&gt;&gt;pictureBox2.Type" xml:space="preserve">
<value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;pictureBox2.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;pictureBox2.ZOrder" xml:space="preserve">
<value>4</value>
</data>
<data name="linkLabelmagdec.AutoSize" type="System.Boolean, mscorlib">
<value>True</value>
</data>
<data name="linkLabelmagdec.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="linkLabelmagdec.Location" type="System.Drawing.Point, System.Drawing">
<value>390, 80</value>
</data>
<data name="linkLabelmagdec.Size" type="System.Drawing.Size, System.Drawing">
<value>104, 13</value>
</data>
<data name="linkLabelmagdec.TabIndex" type="System.Int32, mscorlib">
<value>28</value>
</data>
<data name="linkLabelmagdec.Text" xml:space="preserve">
<value>Declination WebSite</value>
</data>
<data name="&gt;&gt;linkLabelmagdec.Name" xml:space="preserve">
<value>linkLabelmagdec</value>
</data>
<data name="&gt;&gt;linkLabelmagdec.Type" xml:space="preserve">
<value>System.Windows.Forms.LinkLabel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;linkLabelmagdec.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;linkLabelmagdec.ZOrder" xml:space="preserve">
<value>5</value>
</data>
<data name="label100.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="label100.Location" type="System.Drawing.Point, System.Drawing">
<value>305, 57</value>
</data>
<data name="label100.Size" type="System.Drawing.Size, System.Drawing">
<value>72, 16</value>
</data>
<data name="label100.TabIndex" type="System.Int32, mscorlib">
<value>23</value>
</data>
<data name="label100.Text" xml:space="preserve">
<value>Declination</value>
</data>
<data name="&gt;&gt;label100.Name" xml:space="preserve">
<value>label100</value>
</data>
<data name="&gt;&gt;label100.Type" xml:space="preserve">
<value>System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;label100.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;label100.ZOrder" xml:space="preserve">
<value>6</value>
</data>
<data name="TXT_declination.Location" type="System.Drawing.Point, System.Drawing">
<value>383, 57</value>
</data>
<data name="TXT_declination.Size" type="System.Drawing.Size, System.Drawing">
<value>121, 20</value>
</data>
<data name="TXT_declination.TabIndex" type="System.Int32, mscorlib">
<value>20</value>
</data>
<data name="TXT_declination.ToolTip" xml:space="preserve">
<value>Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3</value>
</data>
<data name="&gt;&gt;TXT_declination.Name" xml:space="preserve">
<value>TXT_declination</value>
</data>
<data name="&gt;&gt;TXT_declination.Type" xml:space="preserve">
<value>System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;TXT_declination.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;TXT_declination.ZOrder" xml:space="preserve">
<value>7</value>
</data>
<data name="CHK_enableairspeed.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enableairspeed.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 214</value>
</data>
<data name="CHK_enableairspeed.Size" type="System.Drawing.Size, System.Drawing">
<value>103, 17</value>
</data>
<data name="CHK_enableairspeed.TabIndex" type="System.Int32, mscorlib">
<value>24</value>
</data>
<data name="CHK_enableairspeed.Text" xml:space="preserve">
<value>Enable Airspeed</value>
</data>
<data name="&gt;&gt;CHK_enableairspeed.Name" xml:space="preserve">
<value>CHK_enableairspeed</value>
</data>
<data name="&gt;&gt;CHK_enableairspeed.Type" xml:space="preserve">
<value>System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;CHK_enableairspeed.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;CHK_enableairspeed.ZOrder" xml:space="preserve">
<value>8</value>
</data>
<data name="CHK_enablesonar.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enablesonar.Location" type="System.Drawing.Point, System.Drawing">
<value>159, 136</value>
</data>
<data name="CHK_enablesonar.Size" type="System.Drawing.Size, System.Drawing">
<value>90, 17</value>
</data>
<data name="CHK_enablesonar.TabIndex" type="System.Int32, mscorlib">
<value>25</value>
</data>
<data name="CHK_enablesonar.Text" xml:space="preserve">
<value>Enable Sonar</value>
</data>
<data name="&gt;&gt;CHK_enablesonar.Name" xml:space="preserve">
<value>CHK_enablesonar</value>
</data>
<data name="&gt;&gt;CHK_enablesonar.Type" xml:space="preserve">
<value>System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;CHK_enablesonar.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;CHK_enablesonar.ZOrder" xml:space="preserve">
<value>9</value>
</data>
<data name="CHK_enablecompass.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="CHK_enablecompass.Location" type="System.Drawing.Point, System.Drawing">
<value>162, 56</value>
</data>
<data name="CHK_enablecompass.Size" type="System.Drawing.Size, System.Drawing">
<value>105, 17</value>
</data>
<data name="CHK_enablecompass.TabIndex" type="System.Int32, mscorlib">
<value>27</value>
</data>
<data name="CHK_enablecompass.Text" xml:space="preserve">
<value>Enable Compass</value>
</data>
<data name="&gt;&gt;CHK_enablecompass.Name" xml:space="preserve">
<value>CHK_enablecompass</value>
</data>
<data name="&gt;&gt;CHK_enablecompass.Type" xml:space="preserve">
<value>System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;CHK_enablecompass.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;CHK_enablecompass.ZOrder" xml:space="preserve">
<value>10</value>
</data>
<data name="pictureBox4.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox4.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox4.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 188</value>
</data>
<data name="pictureBox4.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox4.TabIndex" type="System.Int32, mscorlib">
<value>3</value>
</data>
<data name="&gt;&gt;pictureBox4.Name" xml:space="preserve">
<value>pictureBox4</value>
</data>
<data name="&gt;&gt;pictureBox4.Type" xml:space="preserve">
<value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;pictureBox4.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;pictureBox4.ZOrder" xml:space="preserve">
<value>11</value>
</data>
<data name="pictureBox3.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox3.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox3.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 106</value>
</data>
<data name="pictureBox3.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox3.TabIndex" type="System.Int32, mscorlib">
<value>2</value>
</data>
<data name="&gt;&gt;pictureBox3.Name" xml:space="preserve">
<value>pictureBox3</value>
</data>
<data name="&gt;&gt;pictureBox3.Type" xml:space="preserve">
<value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;pictureBox3.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;pictureBox3.ZOrder" xml:space="preserve">
<value>12</value>
</data>
<data name="pictureBox1.BackgroundImageLayout" type="System.Windows.Forms.ImageLayout, System.Windows.Forms">
<value>Zoom</value>
</data>
<data name="pictureBox1.ErrorImage" type="System.Resources.ResXNullRef, System.Windows.Forms">
<value />
</data>
<data name="pictureBox1.ImeMode" type="System.Windows.Forms.ImeMode, System.Windows.Forms">
<value>NoControl</value>
</data>
<data name="pictureBox1.InitialImage" type="System.Resources.ResXNullRef, System.Windows.Forms">
<value />
</data>
<data name="pictureBox1.Location" type="System.Drawing.Point, System.Drawing">
<value>78, 25</value>
</data>
<data name="pictureBox1.Size" type="System.Drawing.Size, System.Drawing">
<value>75, 75</value>
</data>
<data name="pictureBox1.TabIndex" type="System.Int32, mscorlib">
<value>0</value>
</data>
<data name="&gt;&gt;pictureBox1.Name" xml:space="preserve">
<value>pictureBox1</value>
</data>
<data name="&gt;&gt;pictureBox1.Type" xml:space="preserve">
<value>System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</data>
<data name="&gt;&gt;pictureBox1.Parent" xml:space="preserve">
<value>tabHardware</value>
</data>
<data name="&gt;&gt;pictureBox1.ZOrder" xml:space="preserve">
<value>13</value>
</data>
<data name="&gt;&gt;label31.Name" xml:space="preserve">
<value>label31</value>
</data>

18
Tools/ArdupilotMegaPlanner/ThemeManager.cs
View File

@ -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;

BIN
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb
View File

Binary file not shown.

184
Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
View File

@ -93,6 +93,190 @@
<F1>FlightMode</F1>
<F2>Thr Cruise</F2>
</MOD>
<PID-1>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-1>
<PID-2>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-2>
<PID-3>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-3>
<PID-4>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-4>
<PID-5>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-5>
<PID-6>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-6>
<PID-7>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-7>
<PID-8>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-8>
<PID-9>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-9>
<PID-10>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-10>
<PID-11>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-11>
<PID-12>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-12>
<PID-13>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-13>
<PID-14>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-14>
<PID-15>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-15>
<PID-16>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-16>
<PID-17>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-17>
<PID-18>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-18>
<PID-19>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-19>
<PID-20>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-20>
<PID-21>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-21>
<PID-22>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-22>
<PID-23>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-23>
</AC2>
<!-- -->
<APM>

4
Tools/ArdupilotMegaPlanner/bin/Release/m3u/networklink.kml
View File

@ -11,8 +11,8 @@
<Link>
<href>http://127.0.0.1:56781/network.kml</href>
<refreshMode>onInterval</refreshMode>
<refreshInterval>1</refreshInterval>
<viewRefreshTime>1</viewRefreshTime>
<refreshInterval>0.3</refreshInterval>
<viewRefreshTime>0.3</viewRefreshTime>
</Link>
</NetworkLink>
</Folder>

184
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -93,6 +93,190 @@
<F1>FlightMode</F1>
<F2>Thr Cruise</F2>
</MOD>
<PID-1>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-1>
<PID-2>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-2>
<PID-3>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-3>
<PID-4>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-4>
<PID-5>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-5>
<PID-6>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-6>
<PID-7>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-7>
<PID-8>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-8>
<PID-9>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-9>
<PID-10>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-10>
<PID-11>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-11>
<PID-12>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-12>
<PID-13>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-13>
<PID-14>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-14>
<PID-15>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-15>
<PID-16>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-16>
<PID-17>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-17>
<PID-18>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-18>
<PID-19>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-19>
<PID-20>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-20>
<PID-21>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-21>
<PID-22>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-22>
<PID-23>
<F1>Err</F1>
<F2>P</F2>
<F3>I</F3>
<F4>D</F4>
<F5>Output</F5>
<F6>Gain</F6>
</PID-23>
</AC2>
<!-- -->
<APM>

29
Tools/ArdupilotMegaPlanner/georefimage.cs
View File

@ -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);

4
Tools/ArdupilotMegaPlanner/m3u/networklink.kml
View File

@ -11,8 +11,8 @@
<Link>
<href>http://127.0.0.1:56781/network.kml</href>
<refreshMode>onInterval</refreshMode>
<refreshInterval>1</refreshInterval>
<viewRefreshTime>1</viewRefreshTime>
<refreshInterval>0.3</refreshInterval>
<viewRefreshTime>0.3</viewRefreshTime>
</Link>
</NetworkLink>
</Folder>

3
Tools/VARTest/VARTest.pde
View File

@ -28,10 +28,7 @@
#include <GCS_MAVLink.h>
#include <config.h>
#include <Parameters.h>
#if 0
#include <AP_Declination.h>
#endif
static Parameters g;

1
Tools/autotest/ArduCopter.parm
View File

@ -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

1
Tools/autotest/ArduPlane.parm
View File

@ -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

23
Tools/autotest/pysim/aircraft.py
View File

@ -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)

13
Tools/autotest/pysim/multicopter.py
View File

@ -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)

18
Tools/autotest/pysim/rotmat.py
View File

@ -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,

2
Tools/autotest/pysim/sim_multicopter.py
View File

@ -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),

4
libraries/APM_RC/APM_RC.h
View File

@ -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; }

2
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -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

18
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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;
}

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -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;

24
libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
View File

@ -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

2
libraries/AP_AHRS/AP_AHRS_Quaternion.h
View File

@ -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

2
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -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;

125
libraries/AP_Compass/Compass.cpp
View File

@ -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);
}

21
libraries/AP_Compass/Compass.h
View File

@ -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; ///<enable calibration learning
AP_Int8 _use_for_yaw; ///<enable use for yaw calculation
AP_Int8 _auto_declination; ///<enable automatic declination code
bool _null_enable; ///< enabled flag for offset nulling
bool _null_init_done; ///< first-time-around flag used by offset nulling
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
Vector3f _mag_body_last; ///< ?? used by offset nulling
///< used by offset correction
static const uint8_t _mag_history_size = 20;
uint8_t _mag_history_index;
Vector3i _mag_history[_mag_history_size];
};
#endif

219
libraries/AP_Declination/AP_Declination.cpp
View File

@ -7,11 +7,11 @@
* Scott Ferguson
* scottfromscott@gmail.com
*
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation; either version 2.1
* of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
@ -20,49 +20,97 @@
#include <avr/pgmspace.h>
#include <math.h>
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;
}

2
libraries/AP_Declination/AP_Declination.h
View File

@ -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

119
libraries/AP_Declination/examples/AP_Declination_test/AP_Declination_test.pde
View File

@ -5,19 +5,132 @@
#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_Declination.h>
#include <Filter.h>
#include <I2C.h>
#include <SPI.h>
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
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)
{
}

3
libraries/AP_Declination/examples/AP_Declination_test/Makefile
View File

@ -1 +1,4 @@
include ../../../AP_Common/Arduino.mk
sitl:
make -f ../../../../libraries/Desktop/Desktop.mk

3
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -28,9 +28,8 @@ AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
#if 0
#include <AP_Declination.h>
#endif
static float rad_diff(float rad1, float rad2)
{

3
libraries/AP_Math/examples/rotations/rotations.pde
View File

@ -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 <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#endif
// standard rotation matrices (these are the originals from the old code)

10
libraries/AP_Math/matrix3.cpp
View File

@ -163,6 +163,15 @@ Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
c.x * v.x + c.y * v.y + c.z * v.z);
}
// multiplication of transpose by a vector
template <typename T>
Vector3<T> Matrix3<T>::mul_transpose(const Vector3<T> &v) const
{
return Vector3<T>(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<T>
template <typename T>
Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const
@ -186,4 +195,5 @@ template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;

3
libraries/AP_Math/matrix3.h
View File

@ -92,6 +92,9 @@ public:
// multiplication by a vector
Vector3<T> operator *(const Vector3<T> &v) const;
// multiplication of transpose by a vector
Vector3<T> mul_transpose(const Vector3<T> &v) const;
// multiplication by another Matrix3<T>
Matrix3<T> operator *(const Matrix3<T> &m) const;

2
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -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++;

1
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
View File

@ -7,7 +7,6 @@
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <SPI.h> // Arduino SPI library
#include <AP_DCM.h> // ArduCopter DCM Library
#include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library
////////////////////////////////////////////////////////////////////////////////

8
libraries/Desktop/support/sitl.cpp
View File

@ -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;
}

8
libraries/Desktop/support/sitl_adc.h
View File

@ -8,6 +8,7 @@
#include <stdlib.h>
#include <math.h>
#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) {

4
libraries/Desktop/support/sitl_compass.cpp
View File

@ -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);
}

13
libraries/Desktop/support/util.cpp
View File

@ -13,6 +13,7 @@
#include <math.h>
#include <stdint.h>
#include <fcntl.h>
#include <AP_Math.h>
#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;
}

7
libraries/Desktop/support/util.h
View File

@ -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

7
libraries/FastSerial/FastSerial.cpp
View File

@ -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)
;

9
libraries/FastSerial/FastSerial.h
View File

@ -160,6 +160,11 @@ public:
return (1<<port) & _serialInitialized;
}
// ask for writes to be blocking or non-blocking
void set_blocking_writes(bool blocking) {
_nonblocking_writes = !blocking;
}
private:
/// Bit mask for initialized ports
@ -187,6 +192,10 @@ private:
Buffer * const _txBuffer;
bool _open;
// whether writes to the port should block waiting
// for enough space to appear
bool _nonblocking_writes;
/// Allocates a buffer of the given size
///
/// @param buffer The buffer descriptor for which the buffer will

3
libraries/Filter/AverageFilter.h
View File

@ -8,8 +8,6 @@
/// @file AverageFilter.h
/// @brief A class to provide the average of a number of samples
///
/// DO NOT CREATE AND DESTROY INSTANCES OF THIS CLASS BECAUSE THE ALLOC/MALLOC WILL LEAD TO MEMORY FRAGMENTATION
#ifndef AverageFilter_h
#define AverageFilter_h
@ -20,6 +18,7 @@
// 1st parameter <T> is the type of data being filtered.
// 2nd parameter <U> is a larger data type used during summation to prevent overflows
// 3rd parameter <FILTER_SIZE> is the number of elements in the filter
template <class T, class U, uint8_t FILTER_SIZE>
class AverageFilter : public FilterWithBuffer<T,FILTER_SIZE>
{

13
libraries/Filter/FilterWithBuffer.h
View File

@ -55,6 +55,19 @@ typedef FilterWithBuffer<uint16_t,5> FilterWithBufferUInt16_Size5;
typedef FilterWithBuffer<uint16_t,6> FilterWithBufferUInt16_Size6;
typedef FilterWithBuffer<uint16_t,7> FilterWithBufferUInt16_Size7;
typedef FilterWithBuffer<int32_t,2> FilterWithBufferInt32_Size2;
typedef FilterWithBuffer<int32_t,3> FilterWithBufferInt32_Size3;
typedef FilterWithBuffer<int32_t,4> FilterWithBufferInt32_Size4;
typedef FilterWithBuffer<int32_t,5> FilterWithBufferInt32_Size5;
typedef FilterWithBuffer<int32_t,6> FilterWithBufferInt32_Size6;
typedef FilterWithBuffer<int32_t,7> FilterWithBufferInt32_Size7;
typedef FilterWithBuffer<uint32_t,2> FilterWithBufferUInt32_Size2;
typedef FilterWithBuffer<uint32_t,3> FilterWithBufferUInt32_Size3;
typedef FilterWithBuffer<uint32_t,4> FilterWithBufferUInt32_Size4;
typedef FilterWithBuffer<uint32_t,5> FilterWithBufferUInt32_Size5;
typedef FilterWithBuffer<uint32_t,6> FilterWithBufferUInt32_Size6;
typedef FilterWithBuffer<uint32_t,7> FilterWithBufferUInt32_Size7;
// Constructor
template <class T, uint8_t FILTER_SIZE>
FilterWithBuffer<T,FILTER_SIZE>::FilterWithBuffer() :

89
libraries/Filter/LowPassFilter.h Normal file
View File

@ -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 <inttypes.h>
#include <Filter.h>
// 1st parameter <T> is the type of data being filtered.
template <class T>
class LowPassFilter : public Filter<T>
{
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<int8_t> LowPassFilterInt8;
typedef LowPassFilter<uint8_t> LowPassFilterUInt8;
typedef LowPassFilter<int16_t> LowPassFilterInt16;
typedef LowPassFilter<uint16_t> LowPassFilterUInt16;
typedef LowPassFilter<int32_t> LowPassFilterInt32;
typedef LowPassFilter<uint32_t> LowPassFilterUInt32;
typedef LowPassFilter<float> LowPassFilterFloat;
// Constructor //////////////////////////////////////////////////////////////
template <class T>
LowPassFilter<T>::LowPassFilter(float gain) :
Filter<T>(),
_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 <class T>
T LowPassFilter<T>::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

2
libraries/Filter/ModeFilter.h
View File

@ -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

33
libraries/Filter/examples/Filter/Filter.pde
View File

@ -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);
}

60
libraries/Filter/examples/LowPassFilter/LowPassFilter.pde Normal file
View File

@ -0,0 +1,60 @@
/*
Example sketch to demonstrate use of LowPassFilter library.
Code by Randy Mackay. DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <Filter.h> // Filter library
#include <LowPassFilter.h> // 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);
}

1
libraries/Filter/examples/LowPassFilter/Makefile Normal file
View File

@ -0,0 +1 @@
include ../../../AP_Common/Arduino.mk

7
libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h
View File

@ -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
}

Some files were not shown because too many files have changed in this diff Show More