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:
Michael Oborne 2011-09-18 17:43:13 +08:00
parent aa61a88c23 dc23ad4275
commit f7bff57a33
141 changed files with 20138 additions and 14851 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored Normal file
View File

@ -0,0 +1,4 @@
.metadata/
Tools/ArdupilotMegaPlanner/bin/Release/logs/
config.mk
ArduCopter/Debug/

13
ArduCopter/APM_Config.h
View File

@ -1,4 +1,6 @@
// Example config file. Take a look at confi.h. Any term define there can be overridden by defining it here.
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here.
// GPS is auto-selected
@ -46,10 +48,15 @@
CH6_TRAVERSE_SPEED
*/
# define CH7_OPTION DO_SET_HOVER
/*
DO_SET_HOVER
DO_FLIP
SIMPLE_MODE_CONTROL
*/
// See the config.h and defines.h files for how to set this up!
//
// lets use SIMPLE mode for Roll and Pitch during Alt Hold
#define ALT_HOLD_RP ROLL_PITCH_SIMPLE
// lets use Manual throttle during Loiter
//#define LOITER_THR THROTTLE_MANUAL

352
ArduCopter/ArduCopter.pde
View File

@ -1,5 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.0.43 Beta"
/*
ArduCopter Version 2.0 Beta
Authors: Jason Short
@ -127,9 +128,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
APM_BMP085_Class barometer;
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
#ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow;
#endif
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@ -201,6 +199,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
#endif
// normal dcm
AP_DCM dcm(&imu, g_gps);
#ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow;
#endif
#endif
////////////////////////////////////////////////////////////////////////////////
@ -237,7 +239,6 @@ static const char *comma = ",";
static const char* flight_mode_strings[] = {
"STABILIZE",
"ACRO",
"SIMPLE",
"ALT_HOLD",
"AUTO",
"GUIDED",
@ -275,6 +276,7 @@ static byte control_mode = STABILIZE;
static byte old_control_mode = STABILIZE;
static byte oldSwitchPosition; // for remembering the control mode switch
static int motor_out[8];
static bool do_simple = false;
// Heli
// ----
@ -316,11 +318,13 @@ static bool did_ground_start = false; // have we ground started after first ar
// ---------------------
static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
//static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static bool xtrack_enabled = false;
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
//static bool xtrack_enabled = false;
//static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static long circle_angle = 0;
static byte wp_control; // used to control - navgation or loiter
@ -336,7 +340,8 @@ static float cos_pitch_x = 1;
static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static long initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static float boost; // used to give a little extra to maintain altitude
// Acro
#if CH7_OPTION == DO_FLIP
@ -349,7 +354,7 @@ static int airspeed; // m/s * 100
// Location Errors
// ---------------
static long bearing_error; // deg * 100 : 0 to 36000
//static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude;
static long yaw_error; // how off are we pointed
@ -465,33 +470,24 @@ static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static long target_altitude; // used for
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
static struct Location optflow_offset; // optical flow base position
static boolean new_location; // flag to tell us if location has been updated
// IMU variables
// -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring
// ----------------------
static long perf_mon_timer;
//static float imu_health; // Metric based on accel gain deweighting
static int G_Dt_max; // Max main loop cycle time in milliseconds
static int gps_fix_count;
// GCS
// ---
//static char GCS_buffer[53];
//static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
static byte gps_watchdog;
// System Timers
// --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count;
static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
@ -502,7 +498,6 @@ static uint8_t delta_ms_fiftyhz;
static byte slow_loopCounter;
static int superslow_loopCounter;
static byte alt_timer; // for limiting the execution of flight mode thingys
static byte simple_timer; // for limiting the execution of flight mode thingys
@ -513,6 +508,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS
static byte counter_one_herz;
static bool GPS_enabled = false;
static byte loop_step;
static bool new_radio_frame;
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
@ -531,17 +527,11 @@ void loop()
//PORTK |= B00010000;
delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis();
//load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
mainLoop_count++;
//if (delta_ms_fast_loop > 6)
// Log_Write_Performance();
// Execute the fast loop
// ---------------------
fast_loop();
fast_loopTimeStamp = millis();
}
//PORTK &= B11101111;
@ -566,15 +556,12 @@ void loop()
}
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
gcs.send_message(MSG_PERF_REPORT);
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
resetPerfData();
}
//PORTK &= B10111111;
}
@ -608,7 +595,6 @@ static void fast_loop()
// ---------------------------------------
update_yaw_mode();
update_roll_pitch_mode();
update_throttle_mode();
// write out the servo PWM values
// ------------------------------
@ -628,6 +614,18 @@ static void medium_loop()
medium_loopCounter++;
#ifdef OPTFLOW_ENABLED
if(g.optflow_enabled){
optflow.read();
optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
// write to log
if (g.log_bitmask & MASK_LOG_OPTFLOW){
Log_Write_Optflow();
}
}
#endif
if(GPS_enabled){
update_GPS();
}
@ -644,6 +642,10 @@ static void medium_loop()
// auto_trim, uses an auto_level algorithm
auto_trim();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
break;
// This case performs some navigation computations
@ -652,13 +654,6 @@ static void medium_loop()
loop_step = 2;
medium_loopCounter++;
// hack to stop navigation in Simple mode
if (control_mode == SIMPLE){
// clear GPS data
g_gps->new_data = false;
break;
}
// Auto control modes:
if(g_gps->new_data && g_gps->fix){
loop_step = 11;
@ -674,8 +669,8 @@ static void medium_loop()
// ------------------------------------------------------
navigate();
// control mode specific updates to nav_bearing
// --------------------------------------------
// control mode specific updates
// -----------------------------
update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN)
@ -694,17 +689,14 @@ static void medium_loop()
// --------------------------
update_altitude();
// altitude smoothing
// ------------------
//calc_altitude_smoothing_error();
altitude_error = get_altitude_error();
//camera_stabilization();
// invalidate the throttle hold value
// ----------------------------------
invalid_throttle = true;
// calc boost
// ----------
boost = get_angle_boost();
break;
// This case deals with sending high rate telemetry
@ -720,11 +712,13 @@ static void medium_loop()
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
Log_Write_Attitude();
if(motor_armed){
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN && motor_armed)
Log_Write_Control_Tuning();
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
}
#endif
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
@ -783,9 +777,9 @@ static void medium_loop()
// ---------------------------
static void fifty_hz_loop()
{
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
// moved to slower loop
// --------------------
update_throttle_mode();
// Read Sonar
// ----------
@ -798,19 +792,8 @@ static void fifty_hz_loop()
hil.send_message(MSG_RADIO_OUT);
#endif
// use Yaw to find our bearing error
calc_bearing_error();
//if (throttle_slew < 0)
// throttle_slew++;
//else if (throttle_slew > 0)
// throttle_slew--;
camera_stabilization();
//throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude();
@ -885,10 +868,10 @@ static void slow_loop()
#if AUTO_RESET_LOITER == 1
if(control_mode == LOITER){
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
//if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
// reset LOITER to current position
//next_WP = current_loc;
}
//}
}
#endif
@ -918,11 +901,6 @@ static void slow_loop()
if(g.radio_tuning > 0)
tuning();
// filter out the baro offset.
//if(baro_alt_offset > 0) baro_alt_offset--;
//if(baro_alt_offset < 0) baro_alt_offset++;
#if MOTOR_LEDS == 1
update_motor_leds();
#endif
@ -960,8 +938,16 @@ static void update_GPS(void)
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if(gps_watchdog < 10){
gps_watchdog++;
}else{
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll >>= 1;
nav_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) {
gps_watchdog = 0;
// XXX We should be sending GPS data off one of the regular loops so that we send
// no-GPS-fix data too
@ -1008,85 +994,6 @@ static void update_GPS(void)
}
}
#ifdef OPTFLOW_ENABLED
// blend gps and optical flow location
void update_location(void)
{
//static int count = 0;
// get GPS position
if(GPS_enabled){
update_GPS();
}
if( g.optflow_enabled ) {
int32_t temp_lat, temp_lng, diff_lat, diff_lng;
// get optical flow position
optflow.read();
optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt);
// write to log
if (g.log_bitmask & MASK_LOG_OPTFLOW){
Log_Write_Optflow();
}
temp_lat = optflow_offset.lat + optflow.lat;
temp_lng = optflow_offset.lng + optflow.lng;
// if we have good GPS values, don't let optical flow position stray too far
if( GPS_enabled && g_gps->fix ) {
// ensure current location is within 3m of gps location
diff_lat = g_gps->latitude - temp_lat;
diff_lng = g_gps->longitude - temp_lng;
if( diff_lat > 300 ) {
optflow_offset.lat += diff_lat - 300;
//Serial.println("lat inc!");
}
if( diff_lat < -300 ) {
optflow_offset.lat += diff_lat + 300;
//Serial.println("lat dec!");
}
if( diff_lng > 300 ) {
optflow_offset.lng += diff_lng - 300;
//Serial.println("lng inc!");
}
if( diff_lng < -300 ) {
optflow_offset.lng += diff_lng + 300;
//Serial.println("lng dec!");
}
}
// update the current position
current_loc.lat = optflow_offset.lat + optflow.lat;
current_loc.lng = optflow_offset.lng + optflow.lng;
/*count++;
if( count >= 20 ) {
count = 0;
Serial.println();
Serial.print("lat:");
Serial.print(current_loc.lat);
Serial.print("\tlng:");
Serial.print(current_loc.lng);
Serial.print("\tr:");
Serial.print(nav_roll);
Serial.print("\tp:");
Serial.print(nav_pitch);
Serial.println();
}*/
// indicate we have a new position for nav functions
new_location = true;
}else{
// get current position from gps
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
new_location = g_gps->new_data;
}
}
#endif
void update_yaw_mode(void)
{
@ -1098,8 +1005,11 @@ void update_yaw_mode(void)
case YAW_HOLD:
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
//Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw);
if (control_mode <= STABILIZE){
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
}else{
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1);
}
break;
case YAW_LOOK_AT_HOME:
@ -1134,6 +1044,32 @@ void update_roll_pitch_mode(void)
int control_roll = 0, control_pitch = 0;
//read_radio();
if(do_simple && new_radio_frame){
new_radio_frame = false;
simple_timer++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_timer == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_timer > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_timer = 0;
}
// Rotate input by the initial bearing
control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
switch(roll_pitch_mode){
case ROLL_PITCH_ACRO:
@ -1142,76 +1078,57 @@ void update_roll_pitch_mode(void)
// Pitch control
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
return;
break;
case ROLL_PITCH_STABLE:
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
break;
case ROLL_PITCH_SIMPLE:
simple_timer++;
if(simple_timer > 5){
simple_timer = 0;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
// pre-calc rotation (stored in real degrees)
// roll
float cos_x = sin(radians(90 - delta));
// pitch
float sin_y = cos(radians(90 - delta));
// Rotate input by the initial bearing
// roll
nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y;
// pitch
nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x);
}
control_roll = nav_roll;
control_pitch = nav_pitch;
break;
case ROLL_PITCH_AUTO:
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break;
}
// Roll control
g.rc_1.servo_out = get_stabilize_roll(control_roll);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
}
// 50 hz update rate, not 250
void update_throttle_mode(void)
{
switch(throttle_mode){
case THROTTLE_MANUAL:
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
if (g.rc_3.control_in > 0){
g.rc_3.servo_out = g.rc_3.control_in + boost;
}else{
g.rc_3.servo_out = 0;
}
break;
case THROTTLE_HOLD:
// allow interactive changing of atitude
adjust_altitude();
// fall through
case THROTTLE_AUTO:
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
// how far off are we
altitude_error = get_altitude_error();
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s
// apply throttle control
g.rc_3.servo_out = get_throttle(nav_throttle);
// clear the new data flag
invalid_throttle = false;
}
// apply throttle control at 200 hz
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost;
break;
}
}
@ -1235,6 +1152,11 @@ static void update_navigation()
break;
case GUIDED:
wp_control = WP_MODE;
update_auto_yaw();
update_nav_wp();
break;
case RTL:
if(wp_distance > 4){
// calculates desired Yaw
@ -1246,10 +1168,9 @@ static void update_navigation()
wp_control = WP_MODE;
}else{
set_mode(LOITER);
xtrack_enabled = false;
//xtrack_enabled = false;
}
// calculates the desired Roll and Pitch
update_nav_wp();
break;
@ -1269,10 +1190,10 @@ static void update_navigation()
update_auto_yaw();
{
//circle_angle += dTnav; //1000 * (dTnav/1000);
circle_angle = wrap_360(target_bearing + 2000 + 18000);
circle_angle = wrap_360(target_bearing + 3000 + 18000);
target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle));
target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle));
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100)));
}
// calc the lat and long error to the target
@ -1298,7 +1219,7 @@ static void read_AHRS(void)
hil.update();
#endif
dcm.update_DCM_fast(G_Dt);//, _tog);
dcm.update_DCM_fast();
omega = dcm.get_gyro();
}
@ -1310,8 +1231,6 @@ static void update_trig(void){
yawvector.y = temp.b.x; // cos
yawvector.normalize();
cos_yaw_x = yawvector.y; // 0 x = north
sin_yaw_y = yawvector.x; // 1 y
sin_pitch_y = -temp.c.x;
cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x));
@ -1319,6 +1238,9 @@ static void update_trig(void){
cos_roll_x = temp.c.z / cos_pitch_x;
sin_roll_y = temp.c.y / cos_pitch_x;
cos_yaw_x = yawvector.y; // 0 x = north
sin_yaw_y = yawvector.x; // 1 y
//flat:
// 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00,
// 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00,
@ -1377,18 +1299,13 @@ static void update_altitude()
static void
adjust_altitude()
{
alt_timer++;
if(alt_timer >= 2){
alt_timer = 0;
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
}else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
}
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
}else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
}
}
@ -1443,8 +1360,8 @@ static void tuning(){
case CH6_RELAY:
g.rc_6.set_range(0,1000);
if (g.rc_6.control_in <= 600) relay_on();
if (g.rc_6.control_in >= 400) relay_off();
if (g.rc_6.control_in > 525) relay_on();
if (g.rc_6.control_in < 475) relay_off();
break;
case CH6_TRAVERSE_SPEED:
@ -1462,7 +1379,6 @@ static void tuning(){
static void update_nav_wp()
{
// XXX Guided mode!!!
if(wp_control == LOITER_MODE){
// calc a pitch to the target

27
ArduCopter/Attitude.pde
View File

@ -91,7 +91,6 @@ static int
get_nav_throttle(long z_error, int target_speed)
{
int rate_error;
int throttle;
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
// limit error to prevent I term run up
@ -101,11 +100,9 @@ get_nav_throttle(long z_error, int target_speed)
rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -110, 110);
throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
return g.throttle_cruise + throttle;
return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
}
static int
get_rate_roll(long target_rate)
{
@ -156,10 +153,16 @@ static void reset_hold_I(void)
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_nav_I(void)
static void reset_nav(void)
{
nav_throttle = 0;
invalid_throttle = true;
g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I();
long_error = 0;
lat_error = 0;
}
@ -169,11 +172,11 @@ throttle control
// user input:
// -----------
static int get_throttle(int throttle_input)
{
throttle_input = (float)throttle_input * angle_boost();
return max(throttle_input, 0);
}
//static int get_throttle(int throttle_input)
//{
// throttle_input = (float)throttle_input * angle_boost();
// return max(throttle_input, 0);
//}
static long
get_nav_yaw_offset(int yaw_input, int reset)
@ -188,7 +191,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
// re-define nav_yaw if we have stick input
if(yaw_input != 0){
// set nav_yaw + or - the current location
_yaw = (long)yaw_input + dcm.yaw_sensor;
_yaw = (long)yaw_input + dcm.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw);
@ -210,7 +213,7 @@ static int alt_hold_velocity()
}
*/
static float angle_boost()
static float get_angle_boost()
{
float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0);

161
ArduCopter/CMakeLists.txt Normal file
View File

@ -0,0 +1,161 @@
#=============================================================================#
# Author: Niklaa Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
#include(ArduinoProcessing)
set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
message(STATUS "DIR: ${CMAKE_SOURCE_DIR}")
cmake_minimum_required(VERSION 2.8)
#====================================================================#
# Setup Project #
#====================================================================#
project(ArduCopter C CXX)
find_package(Arduino 22 REQUIRED)
add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter)
#add_subdirectory(testtool)
PRINT_BOARD_SETTINGS(mega2560)
#=============================================================================#
# Author: Niklas Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
#====================================================================#
# Settings #
#====================================================================#
set(FIRMWARE_NAME arducopter)
set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board
set(${FIRMWARE_NAME}_SKETCHES
ArduCopter.pde
Attitude.pde
Camera.pde
commands.pde
commands_logic.pde
commands_process.pde
control_modes.pde
events.pde
flip.pde
GCS.pde
GCS_Ardupilot.pde
#GCS_IMU_output.pde
GCS_Jason_text.pde
GCS_Mavlink.pde
GCS_Standard.pde
GCS_Xplane.pde
heli.pde
HIL_Xplane.pde
leds.pde
Log.pde
motors_hexa.pde
motors_octa.pde
motors_octa_quad.pde
motors_quad.pde
motors_tri.pde
motors_y6.pde
navigation.pde
planner.pde
radio.pde
read_commands.pde
sensors.pde
setup.pde
system.pde
test.pde
) # Firmware sketches
#create dummy sourcefile
file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit")
set(${FIRMWARE_NAME}_SRCS
#test.cpp
${FIRMWARE_NAME}.cpp
) # Firmware sources
set(${FIRMWARE_NAME}_HDRS
APM_Config.h
APM_Config_mavlink_hil.h
APM_Config_xplane.h
config.h
defines.h
GCS.h
HIL.h
Mavlink_Common.h
Parameters.h
) # Firmware sources
set(${FIRMWARE_NAME}_LIBS
DataFlash
AP_Math
PID
RC_Channel
AP_OpticalFlow
ModeFilter
memcheck
#AP_PID
APM_PI
#APO
FastSerial
AP_Common
GCS_MAVLink
AP_GPS
APM_RC
AP_DCM
AP_ADC
AP_Compass
AP_IMU
AP_RangeFinder
APM_BMP085
c
m
)
SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX)
#${CONSOLE_PORT}
set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port
set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd
include_directories(
${CMAKE_SOURCE_DIR}/libraries/DataFlash
${CMAKE_SOURCE_DIR}/libraries/AP_Math
${CMAKE_SOURCE_DIR}/libraries/PID
${CMAKE_SOURCE_DIR}/libraries/AP_Common
${CMAKE_SOURCE_DIR}/libraries/RC_Channel
${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow
${CMAKE_SOURCE_DIR}/libraries/ModeFilter
${CMAKE_SOURCE_DIR}/libraries/memcheck
#${CMAKE_SOURCE_DIR}/libraries/AP_PID
${CMAKE_SOURCE_DIR}/libraries/APM_PI
${CMAKE_SOURCE_DIR}/libraries/FastSerial
${CMAKE_SOURCE_DIR}/libraries/AP_Compass
${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
${CMAKE_SOURCE_DIR}/libraries/AP_GPS
${CMAKE_SOURCE_DIR}/libraries/AP_IMU
${CMAKE_SOURCE_DIR}/libraries/AP_ADC
${CMAKE_SOURCE_DIR}/libraries/AP_DCM
${CMAKE_SOURCE_DIR}/libraries/APM_RC
${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
)
#====================================================================#
# Target generation #
#====================================================================#
generate_arduino_firmware(${FIRMWARE_NAME})

12
ArduCopter/Camera.pde
View File

@ -4,19 +4,9 @@
static void init_camera()
{
// ch 6 high(right) is down.
g.rc_camera_pitch.set_angle(4500);
g.rc_camera_pitch.radio_min = 1200;
g.rc_camera_pitch.radio_trim = 1500;
g.rc_camera_pitch.radio_max = 1900;
//g.rc_camera_pitch.set_reverse(1);
// ch 6 high right is down.
g.rc_camera_roll.set_angle(4500);
g.rc_camera_roll.radio_min = 1000;
g.rc_camera_roll.radio_trim = 1500;
g.rc_camera_roll.radio_max = 2000;
g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);

1
ArduCopter/GCS_Mavlink.pde
View File

@ -51,6 +51,7 @@ GCS_MAVLINK::update(void)
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
while(comm_get_available(chan))

4
ArduCopter/HIL_Xplane.pde
View File

@ -12,7 +12,7 @@ void HIL_XPLANE::output_HIL(void)
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
//output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)altitude_error); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15
output_byte((int)g.waypoint_index); // 8 bytes 16
@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
break;
case MSG_LOCATION:
break;
case MSG_LOCAL_LOCATION:
break;
case MSG_GPS_RAW:
break;
case MSG_SERVO_OUT:

142
ArduCopter/Log.pde
View File

@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv)
int dump_log_start;
int dump_log_end;
//byte last_log_num = get_num_logs();
//Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
// check that the requested log number can be read
dump_log = argv[1].i;
@ -365,16 +362,15 @@ static void Log_Write_GPS()
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(g_gps->time); // 1
DataFlash.WriteByte(g_gps->fix); // 2
DataFlash.WriteByte(g_gps->num_sats); // 3
DataFlash.WriteByte(g_gps->num_sats); // 2
DataFlash.WriteLong(current_loc.lat); // 4
DataFlash.WriteLong(current_loc.lng); // 5
DataFlash.WriteLong(current_loc.alt); // 7
DataFlash.WriteLong(current_loc.lat); // 3
DataFlash.WriteLong(current_loc.lng); // 4
DataFlash.WriteLong(current_loc.alt); // 5
DataFlash.WriteLong(g_gps->altitude); // 6
DataFlash.WriteInt(g_gps->ground_speed); // 8
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9
DataFlash.WriteInt(g_gps->ground_speed); // 7
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8
DataFlash.WriteByte(END_BYTE);
}
@ -382,21 +378,20 @@ static void Log_Write_GPS()
// Read a GPS packet
static void Log_Read_GPS()
{
Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
Serial.printf_P(PSTR("GPS, %ld, %d, "
"%4.7f, %4.7f, %4.4f, %4.4f, "
"%d, %u\n"),
DataFlash.ReadLong(), // 1 time
(int)DataFlash.ReadByte(), // 2 fix
(int)DataFlash.ReadByte(), // 3 sats
(int)DataFlash.ReadByte(), // 2 sats
(float)DataFlash.ReadLong() / t7, // 4 lat
(float)DataFlash.ReadLong() / t7, // 5 lon
(float)DataFlash.ReadLong() / 100.0, // 6 gps alt
(float)DataFlash.ReadLong() / 100.0, // 7 sensor alt
(float)DataFlash.ReadLong() / t7, // 3 lat
(float)DataFlash.ReadLong() / t7, // 4 lon
(float)DataFlash.ReadLong() / 100.0, // 5 gps alt
(float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
DataFlash.ReadInt(), // 8 ground speed
(uint16_t)DataFlash.ReadInt()); // 9 ground course
DataFlash.ReadInt(), // 7 ground speed
(uint16_t)DataFlash.ReadInt()); // 8 ground course
}
@ -508,13 +503,13 @@ static void Log_Write_Optflow()
DataFlash.WriteInt((int)optflow.dx);
DataFlash.WriteInt((int)optflow.dy);
DataFlash.WriteInt((int)optflow.surface_quality);
DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat);
DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng);
DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
DataFlash.WriteByte(END_BYTE);
}
#endif
// Read an attitude packet
static void Log_Read_Optflow()
{
Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
@ -537,9 +532,8 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteByte(wp_verify_byte); // 2
DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4
DataFlash.WriteInt((int)long_error); // 5
DataFlash.WriteInt((int)lat_error); // 6
DataFlash.WriteInt((int)long_error); // 4
DataFlash.WriteInt((int)lat_error); // 5
/*
@ -564,11 +558,10 @@ static void Log_Write_Nav_Tuning()
static void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d\n"),
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), // distance
DataFlash.ReadByte(), // wp_verify_byte
DataFlash.ReadInt(), // target_bearing
DataFlash.ReadInt(), // nav_bearing
DataFlash.ReadInt(), // long_error
DataFlash.ReadInt()); // lat_error
@ -598,8 +591,6 @@ static void Log_Read_Nav_Tuning()
DataFlash.ReadInt(),
DataFlash.ReadInt()); //nav bearing
*/
}
@ -611,23 +602,18 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// Control
//DataFlash.WriteInt((int)(g.rc_4.control_in/100));
//DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error/100);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
DataFlash.WriteInt((int)(nav_yaw/100)); //2
DataFlash.WriteInt((int)yaw_error/100); //3
// Alt hold
DataFlash.WriteInt(g.rc_3.servo_out);
DataFlash.WriteInt(sonar_alt); //
DataFlash.WriteInt(baro_alt); //
DataFlash.WriteInt(g.rc_3.servo_out); //4
DataFlash.WriteInt(sonar_alt); //5
DataFlash.WriteInt(baro_alt); //6
DataFlash.WriteInt((int)next_WP.alt); //
//DataFlash.WriteInt((int)altitude_error); //
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
DataFlash.WriteInt((int)next_WP.alt); //7
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
DataFlash.WriteByte(END_BYTE);
}
@ -637,14 +623,13 @@ static void Log_Write_Control_Tuning()
static void Log_Read_Control_Tuning()
{
Serial.printf_P(PSTR( "CTUN, "
//"%d, %d, "
"%d, %d, %d, "
"%d, %d, %d, "
"%d, %d, %d\n"),
"%d, %d\n"),
// Control
DataFlash.ReadInt(),
DataFlash.ReadInt(),
//DataFlash.ReadInt(),
//DataFlash.ReadInt(),
// yaw
DataFlash.ReadInt(),
@ -672,21 +657,18 @@ static void Log_Write_Performance()
//*
DataFlash.WriteLong( millis()- perf_mon_timer);
DataFlash.WriteInt ( mainLoop_count);
DataFlash.WriteInt ( G_Dt_max);
//DataFlash.WriteLong( millis()- perf_mon_timer);
//DataFlash.WriteInt ( mainLoop_count);
DataFlash.WriteInt ( G_Dt_max); //1
DataFlash.WriteByte( dcm.gyro_sat_count);
DataFlash.WriteByte( imu.adc_constraints);
DataFlash.WriteByte( dcm.renorm_sqrt_count);
DataFlash.WriteByte( dcm.renorm_blowup_count);
DataFlash.WriteByte( gps_fix_count);
DataFlash.WriteByte( dcm.gyro_sat_count); //2
DataFlash.WriteByte( imu.adc_constraints); //3
DataFlash.WriteByte( dcm.renorm_sqrt_count); //4
DataFlash.WriteByte( dcm.renorm_blowup_count); //5
DataFlash.WriteByte( gps_fix_count); //6
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000));
DataFlash.WriteLong ( throttle_integrator);
//*/
//PM, 20005, 3742, 10,0,0,0,0,89,1000,
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7
DataFlash.WriteLong ( throttle_integrator); //8
DataFlash.WriteByte(END_BYTE);
}
@ -694,24 +676,23 @@ static void Log_Write_Performance()
// Read a performance packet
static void Log_Read_Performance()
{
Serial.printf_P(PSTR( "PM, %ld, %d, %d, "
"%d, %d, %d, %d, %d, "
Serial.printf_P(PSTR( "PM, %d, %d, %d, "
"%d, %d, %d, "
"%d, %ld\n"),
// Control
DataFlash.ReadLong(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
//DataFlash.ReadLong(),
//DataFlash.ReadInt(),
DataFlash.ReadInt(), //1
DataFlash.ReadByte(), //2
DataFlash.ReadByte(), //3
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(), //4
DataFlash.ReadByte(), //5
DataFlash.ReadByte(), //6
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadInt(),
DataFlash.ReadLong());
DataFlash.ReadInt(), //7
DataFlash.ReadLong()); //8
}
// Write a command processing packet.
@ -762,19 +743,28 @@ static void Log_Write_Attitude()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)g.rc_1.servo_out);
DataFlash.WriteInt((int)g.rc_2.servo_out);
DataFlash.WriteInt((int)g.rc_4.servo_out);
DataFlash.WriteByte(END_BYTE);
}
// Read an attitude packet
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT, %d, %d, %u\n"),
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt());
(uint16_t)DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt());
}
// Write a mode packet. Total length : 5 bytes
@ -834,8 +824,10 @@ static void Log_Read(int start_page, int end_page)
case 1:
if(data == HEAD_BYTE2) // Head byte 2
log_step++;
else
else{
log_step = 0;
Serial.println(".");
}
break;
case 2:

593
ArduCopter/Mavlink_Common.h
View File

@ -25,14 +25,257 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
}
}
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
/*
!!NOTE!!
the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
#define NOINLINE __attribute__((noinline))
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default:
mode = control_mode + 100;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
0,
battery_voltage * 1000,
battery_remaining,
packet_drops);
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
0,
0);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
0,
0,
0,
0,
rssi);
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
motor_out[4],
motor_out[5],
motor_out[6],
motor_out[7]);
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0,
climb_rate);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
@ -40,298 +283,90 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS1:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default:
mode = control_mode + 100;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
}
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
break;
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
0,
battery_voltage * 1000,
battery_remaining,
packet_drops);
break;
}
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break;
}
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_ATTITUDE:
{
//Vector3f omega = dcm.get_gyro();
CHECK_PAYLOAD_SIZE(ATTITUDE);
mavlink_msg_attitude_send(
chan,
timeStamp,
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
break;
}
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_LOCATION:
{
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
/*current_loc.alt * 10,*/ // changed to absolute altitude
g_gps->altitude,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
break;
}
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
{
//if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
0,
0);
//}
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_LOCAL_LOCATION:
{
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(
chan,
timeStamp,
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
(current_loc.alt - home.alt) / 1.0e2,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
}
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
break;
}
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_SERVO_OUT:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
0,
0,
0,
0,
rssi);
break;
}
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
break;
}
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
motor_out[4],
motor_out[5],
motor_out[6],
motor_out[7]);
break;
}
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
/*current_loc.alt / 100.0,*/ // changed to absolute altitude
g_gps->altitude/100.0,
climb_rate);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
{
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(
chan,
timeStamp,
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send(
chan,
timeStamp,
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
break;
}
case MSG_RAW_IMU3:
{
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
break;
}
default:
break;
default:
break;
}
return true;
}

18
ArduCopter/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 107;
static const uint16_t k_format_version = 108;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -78,12 +78,12 @@ public:
k_param_top_bottom_ratio,
k_param_optflow_enabled,
k_param_input_voltage,
k_param_low_voltage,
//
// 160: Navigation parameters
//
k_param_crosstrack_entry_angle = 160,
k_param_pitch_max,
k_param_RTL_altitude,
//
@ -139,7 +139,7 @@ public:
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
k_param_simple_modes,
//
// 220: Waypoint data
@ -179,7 +179,7 @@ public:
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
AP_Int8 serial3_baud;
// Crosstrack navigation
@ -213,6 +213,7 @@ public:
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
AP_Int8 simple_modes;
// Radio settings
//
@ -221,8 +222,6 @@ public:
//AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw;
AP_Int16 pitch_max;
// Misc
//
AP_Int16 log_bitmask;
@ -239,6 +238,7 @@ public:
AP_Float top_bottom_ratio;
AP_Int8 optflow_enabled;
AP_Float input_voltage;
AP_Float low_voltage;
#if FRAME_CONFIG == HELI_FRAME
// Heli
@ -302,13 +302,14 @@ public:
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")),
low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
@ -324,8 +325,7 @@ public:
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),

33
ArduCopter/commands.pde
View File

@ -20,18 +20,6 @@ static void clear_command_queue(){
next_command.id = NO_COMMAND;
}
static void init_auto()
{
//if (g.waypoint_index == g.waypoint_total) {
// Serial.println("ia_f");
// do_RTL();
//}
// initialize commands
// -------------------
init_commands();
}
// Getters
// -------
static struct Location get_command_with_index(int i)
@ -41,8 +29,6 @@ static struct Location get_command_with_index(int i)
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
Serial.println("XCD");
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
@ -51,7 +37,6 @@ static struct Location get_command_with_index(int i)
return temp;
}else{
//Serial.println("LD");
// we can load a command, we don't process it yet
// read WP position
long mem = (WP_START_BYTE) + (i * WP_SIZE);
@ -75,10 +60,9 @@ static struct Location get_command_with_index(int i)
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
//temp.alt += home.alt;
}
//Serial.println("ADD ALT");
//}
if(temp.options & WP_OPTION_RELATIVE){
// If were relative, just offset from home
@ -190,7 +174,6 @@ static void set_next_WP(struct Location *wp)
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
@ -198,7 +181,7 @@ static void set_next_WP(struct Location *wp)
// set a new crosstrack bearing
// ----------------------------
crosstrack_bearing = target_bearing; // Used for track following
//crosstrack_bearing = target_bearing; // Used for track following
gcs.print_current_waypoints();
}
@ -218,14 +201,12 @@ static void init_home()
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
//home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
home.alt = 0; // this is a test
home.alt = 0; // Home is always 0
home_is_set = true;
// to point yaw towards home until we set it with Mavlink
target_WP = home;
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
// Save Home to EEPROM
// -------------------
// no need to save this to EPROM
@ -234,8 +215,14 @@ static void init_home()
// Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home;
// this is dangerous since we can get GPS lock at any time.
//next_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}

24
ArduCopter/commands_logic.pde
View File

@ -105,17 +105,14 @@ static void handle_process_now()
static void handle_no_commands()
{
// we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous.
// use landing commands
/*
switch (control_mode){
default:
//set_mode(RTL);
set_mode(RTL);
break;
}
return;
*/
Serial.println("Handle No CMDs");
}*/
//return;
//Serial.println("Handle No CMDs");
}
/********************************************************************************/
@ -314,13 +311,10 @@ static void do_loiter_turns()
static void do_loiter_time()
{
///*
wp_control = LOITER_MODE;
wp_control = LOITER_MODE;
set_next_WP(&current_loc);
loiter_time = millis();
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
//Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
//*/
}
/********************************************************************************/
@ -467,12 +461,8 @@ static void do_change_alt()
{
Location temp = next_WP;
condition_start = current_loc.alt;
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
condition_value = next_command.alt + home.alt;
} else {
condition_value = next_command.alt;
}
temp.alt = condition_value;
condition_value = next_command.alt;
temp.alt = next_command.alt;
set_next_WP(&temp);
}

41
ArduCopter/config.h
View File

@ -210,6 +210,8 @@
//#define OPTFLOW_ENABLED
#endif
//#define OPTFLOW_ENABLED
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
# define OPTFLOW DISABLED
#endif
@ -310,7 +312,7 @@
#endif
#ifndef SIMPLE_RP
# define SIMPLE_RP ROLL_PITCH_SIMPLE
# define SIMPLE_RP ROLL_PITCH_STABLE
#endif
#ifndef SIMPLE_THR
@ -390,7 +392,7 @@
// Attitude Control
//
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_P 4.0
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.02
@ -400,7 +402,7 @@
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 3.6
# define STABILIZE_PITCH_P 4.0
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.02
@ -413,7 +415,7 @@
// Rate Control
//
#ifndef RATE_ROLL_P
# define RATE_ROLL_P .13
# define RATE_ROLL_P 0.13
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0
@ -472,7 +474,7 @@
# define LOITER_P .4 //
#endif
#ifndef LOITER_I
# define LOITER_I 0.01 //
# define LOITER_I 0.04 //
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 12 // degrees°
@ -482,28 +484,12 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif
#ifndef NAV_I
# define NAV_I 0.1 // this
# define NAV_I 0.15 // this
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 16 // degrees
# define NAV_IMAX 20 // degrees
#endif
/*
#ifndef NAV_LOITER_P
# define NAV_LOITER_P .4 //1.4 //
#endif
#ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.05 //
#endif
#ifndef NAV_LOITER_D
# define NAV_LOITER_D 2 //
#endif
#ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 8 // degrees°
#endif
*/
#ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph
#endif
@ -518,9 +504,6 @@
#ifndef THROTTLE_I
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
#endif
//#ifndef THROTTLE_D
//# define THROTTLE_D 0.6 // upped with filter
//#endif
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 300
#endif
@ -579,7 +562,7 @@
# define LOG_ATTITUDE_FAST DISABLED
#endif
#ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED DISABLED
# define LOG_ATTITUDE_MED ENABLED
#endif
#ifndef LOG_GPS
# define LOG_GPS ENABLED
@ -671,8 +654,8 @@
# define WP_RADIUS_DEFAULT 3
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 10
#ifndef LOITER_RADIUS
# define LOITER_RADIUS 10
#endif
#ifndef ALT_HOLD_HOME

14
ArduCopter/control_modes.pde
View File

@ -13,6 +13,15 @@ static void read_control_switch()
switch_debouncer = false;
set_mode(flight_modes[switchPosition]);
#if CH7_OPTION != SIMPLE_MODE_CONTROL
// setup Simple mode
// do we enable simple mode?
do_simple = (g.simple_modes & (1 << switchPosition));
//Serial.printf("do simple: %d \n", (int)do_simple);
#endif
}else{
switch_debouncer = true;
}
@ -47,6 +56,11 @@ static void read_trim_switch()
do_flip = true;
}
#elif CH7_OPTION == SIMPLE_MODE_CONTROL
do_simple = (g.rc_7.control_in > 800);
//Serial.println(g.rc_7.control_in, DEC);
#elif CH7_OPTION == DO_SET_HOVER
// switch is engaged
if (g.rc_7.control_in > 800){

44
ArduCopter/defines.h
View File

@ -14,8 +14,7 @@
#define ROLL_PITCH_STABLE 0
#define ROLL_PITCH_ACRO 1
#define ROLL_PITCH_SIMPLE 2
#define ROLL_PITCH_AUTO 3
#define ROLL_PITCH_AUTO 2
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
@ -30,6 +29,7 @@
// CH 7 control
#define DO_SET_HOVER 0
#define DO_FLIP 1
#define SIMPLE_MODE_CONTROL 2
// Frame types
#define QUAD_FRAME 0
@ -52,7 +52,7 @@
#define FR_LED AN12 // Mega PE4 pin, OUT7
#define RE_LED AN14 // Mega PE5 pin, OUT6
#define RI_LED AN10 // Mega PH4 pin, OUT5
#define LE_LED AN8 // Mega PH5 pin, OUT4
#define LE_LED AN8 // Mega PH5 pin, OUT4
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
@ -83,21 +83,6 @@
#define MAX_SONAR_UNKNOWN 0
#define MAX_SONAR_XL 1
// Radio channels
// Note channels are from 0!
//
// XXX these should be CH_n defines from RC.h at some point.
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define CH_10 9 //PB5
#define CH_11 10 //PE3
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
@ -129,14 +114,20 @@
// ----------------
#define STABILIZE 0 // hold level position
#define ACRO 1 // rate control
#define SIMPLE 2 //
#define ALT_HOLD 3 // AUTO control
#define AUTO 4 // AUTO control
#define GUIDED 5 // AUTO control
#define LOITER 6 // Hold a single location
#define RTL 7 // AUTO control
#define CIRCLE 8 // AUTO control
#define NUM_MODES 9
#define ALT_HOLD 2 // AUTO control
#define AUTO 3 // AUTO control
#define GUIDED 4 // AUTO control
#define LOITER 5 // Hold a single location
#define RTL 6 // AUTO control
#define CIRCLE 7 // AUTO control
#define NUM_MODES 8
#define SIMPLE_1 1
#define SIMPLE_2 2
#define SIMPLE_3 4
#define SIMPLE_4 8
#define SIMPLE_5 16
#define SIMPLE_6 32
// CH_6 Tuning
// -----------
@ -252,7 +243,6 @@
#define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1

41
ArduCopter/leds.pde
View File

@ -96,26 +96,35 @@ static void clear_leds()
#if MOTOR_LEDS == 1
static void update_motor_leds(void)
{
// blink rear
static bool blink = false;
if (motor_armed == true){
if (low_batt == true){
// blink rear
static bool blink = false;
if (blink){
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
if (blink){
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
blink = !blink;
}else{
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
}else {
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}
blink = !blink;
// the variable low_batt is here to let people know the voltage is low or the pack capacity is finished
// I don't know what folks want here.
// low_batt
}
#endif

4
ArduCopter/motors.pde
View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define ARM_DELAY 10 // one secon
#define DISARM_DELAY 10 // one secon
#define ARM_DELAY 10 // one second
#define DISARM_DELAY 10 // one second
#define LEVEL_DELAY 120 // twelve seconds
#define AUTO_LEVEL_DELAY 150 // twentyfive seconds

36
ArduCopter/motors_hexa.pde
View File

@ -138,21 +138,21 @@ static void output_motor_test()
// 31
// 24
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 50;
motor_out[CH_1] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 50;
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 50;
motor_out[CH_4] += 50;
motor_out[CH_8] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[CH_7] += 50;
motor_out[CH_3] += 50;
motor_out[CH_7] += 100;
motor_out[CH_3] += 100;
}
}else{
@ -160,21 +160,21 @@ static void output_motor_test()
// 2 1
// 4
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_4] += 50;
motor_out[CH_8] += 50;
motor_out[CH_4] += 100;
motor_out[CH_8] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_7] += 50;
motor_out[CH_3] += 50;
motor_out[CH_7] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_2] += 50;
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[CH_1] += 50;
motor_out[CH_1] += 100;
}
}
@ -189,27 +189,27 @@ static void output_motor_test()
/*
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
}
*/

86
ArduCopter/motors_octa.pde
View File

@ -182,37 +182,75 @@ static void output_motors_disarmed()
static void output_motor_test()
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
delay(1000);
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
}
if( g.frame_orientation == V_FRAME )
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
}
}
#endif

31
ArduCopter/motors_octa_quad.pde
View File

@ -149,8 +149,37 @@ static void output_motors_disarmed()
static void output_motor_test()
{
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
}
#endif

25
ArduCopter/motors_quad.pde
View File

@ -4,6 +4,7 @@
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
@ -139,23 +140,23 @@ static void output_motor_test()
// 31
// 24
if(g.rc_1.control_in > 3000){
motor_out[CH_1] += 50;
motor_out[CH_4] += 50;
motor_out[CH_1] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_1.control_in < -3000){
motor_out[CH_2] += 50;
motor_out[CH_3] += 50;
motor_out[CH_2] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){
motor_out[CH_2] += 50;
motor_out[CH_4] += 50;
motor_out[CH_2] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_2.control_in < -3000){
motor_out[CH_1] += 50;
motor_out[CH_3] += 50;
motor_out[CH_1] += 100;
motor_out[CH_3] += 100;
}
}else{
@ -163,16 +164,16 @@ static void output_motor_test()
// 2 1
// 4
if(g.rc_1.control_in > 3000)
motor_out[CH_1] += 50;
motor_out[CH_1] += 100;
if(g.rc_1.control_in < -3000)
motor_out[CH_2] += 50;
motor_out[CH_2] += 100;
if(g.rc_2.control_in > 3000)
motor_out[CH_4] += 50;
motor_out[CH_4] += 100;
if(g.rc_2.control_in < -3000)
motor_out[CH_3] += 50;
motor_out[CH_3] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);

6
ArduCopter/motors_tri.pde
View File

@ -96,15 +96,15 @@ static void output_motor_test()
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 50;
motor_out[CH_1] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 50;
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_4] += 50;
motor_out[CH_4] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);

12
ArduCopter/motors_y6.pde
View File

@ -144,18 +144,18 @@ static void output_motor_test()
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 50;
motor_out[CH_7] += 50;
motor_out[CH_1] += 100;
motor_out[CH_7] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 50;
motor_out[CH_3] += 50;
motor_out[CH_2] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 50;
motor_out[CH_4] += 50;
motor_out[CH_8] += 100;
motor_out[CH_4] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);

53
ArduCopter/navigation.pde
View File

@ -30,15 +30,6 @@ static void navigate()
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will include xtrac correction
// -----------------------------------------
//xtrack_enabled = false;
if(xtrack_enabled){
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction());
}else{
nav_bearing = target_bearing;
}
}
static bool check_missed_wp()
@ -70,6 +61,9 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
}
// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0);
#define NAV_ERR_MAX 400
static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
{
@ -101,18 +95,30 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
}
// find the rates:
// calc the cos of the error to tell how fast we are moving towards the target in cm
y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0));
float temp = radians((float)g_gps->ground_course/100.0);
#ifdef OPTFLOW_ENABLED
// calc the cos of the error to tell how fast we are moving towards the target in cm
if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){
x_actual_speed = optflow.vlon * 10;
y_actual_speed = optflow.vlat * 10;
}else{
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
}
#else
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
#endif
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum
y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0));
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250);
x_rate_error = constrain(x_rate_error, -600, 600);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
}
@ -127,13 +133,6 @@ static void calc_nav_pitch_roll()
nav_pitch = -nav_pitch;
}
// ------------------------------
static void calc_bearing_error()
{
bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error);
}
static long get_altitude_error()
{
return next_WP.alt - current_loc.alt;
@ -189,6 +188,7 @@ static long wrap_180(long error)
return error;
}
/*
static long get_crosstrack_correction(void)
{
// Crosstrack Error
@ -206,19 +206,20 @@ static long get_crosstrack_correction(void)
}
return 0;
}
*/
/*
static long cross_track_test()
{
long temp = wrap_180(target_bearing - crosstrack_bearing);
return abs(temp);
}
*/
/*
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
*/
static long get_altitude_above_home(void)
{
// This is the altitude above the home location

2
ArduCopter/radio.pde
View File

@ -101,7 +101,7 @@ void output_min()
static void read_radio()
{
if (APM_RC.GetState() == 1){
new_radio_frame = true;
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));

5
ArduCopter/sensors.pde
View File

@ -113,17 +113,20 @@ static void read_battery(void)
if(g.battery_monitoring == 1)
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
if(g.battery_monitoring == 2)
battery_voltage = battery_voltage4;
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
battery_voltage = battery_voltage1;
if(g.battery_monitoring == 4) {
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
}
#if BATTERY_EVENT == 1
if(battery_voltage < LOW_VOLTAGE)
if(battery_voltage < g.low_voltage)
low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)

74
ArduCopter/setup.pde
View File

@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_flight_modes();
report_imu();
report_compass();
#ifdef OPTFLOW_ENABLED
report_optflow();
#endif
#if FRAME_CONFIG == HELI_FRAME
report_heli();
report_gyro();
@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
byte _oldSwitchPosition = 0;
byte mode = 0;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
print_hit_enter();
while(1){
@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode = constrain(mode, 0, NUM_MODES-1);
// update the user
print_switch(_switchPosition, mode);
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
// Remember switch position
_oldSwitchPosition = _switchPosition;
}
// look for stick input
if (radio_input_switch() == true){
if (abs(g.rc_1.control_in) > 3000){
mode++;
if(mode >= NUM_MODES)
mode = 0;
@ -334,13 +336,32 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
flight_modes[_switchPosition] = mode;
// print new mode
print_switch(_switchPosition, mode);
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (g.rc_4.control_in > 3000){
g.simple_modes |= (1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (g.rc_4.control_in < -3000){
g.simple_modes &= ~(1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// escape hatch
if(Serial.available() > 0){
for (mode=0; mode<6; mode++)
for (mode = 0; mode < 6; mode++)
flight_modes[mode].save();
g.simple_modes.save();
print_done();
report_flight_modes();
return (0);
@ -740,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.optflow_enabled = false;
//} else if(argv[1].i > 10){
// g.optflow_fov.set_and_save(argv[1].i);
// optflow.set_field_of_view(g.optflow_fov.get());
}else{
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
report_optflow();
@ -872,7 +889,7 @@ static void report_flight_modes()
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]);
print_switch(i, flight_modes[i], (g.simple_modes & (1<<i)));
}
print_blanks(2);
}
@ -956,10 +973,15 @@ print_radio_values()
}
static void
print_switch(byte p, byte m)
print_switch(byte p, byte m, bool b)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
Serial.printf_P(PSTR("Pos %d:\t"),p);
Serial.print(flight_mode_strings[m]);
Serial.printf_P(PSTR(",\t\tSimple: "));
if(b)
Serial.printf_P(PSTR("ON\n"));
else
Serial.printf_P(PSTR("OFF\n"));
}
static void
@ -968,32 +990,6 @@ print_done()
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
// read at 50Hz
static bool
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
}else{
return 0;
}
}
static void zero_eeprom(void)
{

57
ArduCopter/system.pde
View File

@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands);
MENU(main_menu, THISFIRMWARE, main_menu_commands);
#endif // CLI_ENABLED
@ -73,8 +73,8 @@ static void init_ardupilot()
Serial1.begin(38400, 128, 16);
#endif
Serial.printf_P(PSTR("\n\nInit ACM"
"\n\nRAM: %lu\n"),
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"),
freeRAM());
@ -204,12 +204,12 @@ static void init_ardupilot()
if(g.compass_enabled)
init_compass();
#ifdef OPTFLOW_ENABLED
#ifdef OPTFLOW_ENABLED
// init the optical flow sensor
if(g.optflow_enabled) {
init_optflow();
}
#endif
#endif
// Logging:
// --------
@ -249,11 +249,6 @@ static void init_ardupilot()
start_new_log();
}
//#if(GROUND_START_DELAY > 0)
//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
// delay(GROUND_START_DELAY * 1000);
//#endif
GPS_enabled = false;
// Read in the GPS
@ -292,10 +287,8 @@ static void init_ardupilot()
// ---------------------------
reset_control_switch();
//delay(100);
startup_ground();
//Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max);
Log_Write_Startup();
SendDebug("\nReady to FLY ");
@ -312,7 +305,9 @@ static void startup_ground(void)
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro(mavlink_delay);
report_imu();
#if CLI_ENABLED == ENABLED
report_imu();
#endif
#endif
// reset the leds
@ -328,8 +323,7 @@ static void startup_ground(void)
#define ROLL_PITCH_STABLE 0
#define ROLL_PITCH_ACRO 1
#define ROLL_PITCH_SIMPLE 2
#define ROLL_PITCH_AUTO 3
#define ROLL_PITCH_AUTO 2
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
@ -361,9 +355,7 @@ static void set_mode(byte mode)
// report the GPS and Motor arming status
led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction
xtrack_enabled = false;
reset_nav_I();
reset_nav();
switch(control_mode)
{
@ -381,13 +373,6 @@ static void set_mode(byte mode)
reset_hold_I();
break;
case SIMPLE:
yaw_mode = SIMPLE_YAW;
roll_pitch_mode = SIMPLE_RP;
throttle_mode = SIMPLE_THR;
reset_hold_I();
break;
case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
@ -395,7 +380,7 @@ static void set_mode(byte mode)
reset_hold_I();
init_throttle_cruise();
next_WP.alt = current_loc.alt;
next_WP = current_loc;
break;
case AUTO:
@ -407,11 +392,7 @@ static void set_mode(byte mode)
init_throttle_cruise();
// loads the commands from where we left off
init_auto();
// do crosstrack correction
// XXX move to flight commands
xtrack_enabled = true;
init_commands();
break;
case CIRCLE:
@ -437,9 +418,10 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
xtrack_enabled = true;
//xtrack_enabled = true;
init_throttle_cruise();
next_WP = current_loc;
set_next_WP(&guided_WP);
break;
case RTL:
@ -447,7 +429,7 @@ static void set_mode(byte mode)
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
xtrack_enabled = true;
//xtrack_enabled = true;
init_throttle_cruise();
do_RTL();
break;
@ -489,7 +471,7 @@ static void set_failsafe(boolean mode)
static void resetPerfData(void) {
mainLoop_count = 0;
//mainLoop_count = 0;
G_Dt_max = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
@ -508,7 +490,10 @@ init_compass()
static void
init_optflow()
{
optflow.init();
if( optflow.init() == false ) {
g.optflow_enabled = false;
//SendDebug("\nFailed to Init OptFlow ");
}
optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft
optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view
}
@ -539,7 +524,7 @@ static void
init_throttle_cruise()
{
// are we moving from manual throttle to auto_throttle?
if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in);
}

3
ArduCopter/test.pde
View File

@ -362,7 +362,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
while(1){
for(int i = 0; i < 9; i++){
Serial.printf_P(PSTR("%d,"),adc.Ch(i));
Serial.printf_P(PSTR("%u,"),adc.Ch(i));
}
Serial.println();
delay(20);
@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
static int8_t
test_optflow(uint8_t argc, const Menu::arg *argv)
{
///*
if(g.optflow_enabled) {
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
print_hit_enter();

3
ArduPlane/APM_Config.h
View File

@ -1,5 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here.

17
ArduPlane/APM_Config.h.reference
View File

@ -304,19 +304,6 @@
//#define FLIGHT_MODE_6 MANUAL
//
//////////////////////////////////////////////////////////////////////////////
// RC_5_FUNCT OPTIONAL
// RC_6_FUNCT OPTIONAL
// RC_7_FUNCT OPTIONAL
// RC_8_FUNCT OPTIONAL
//
// The channel 5 through 8 function assignments allow configuration of those
// channels for use with differential ailerons, flaps, flaperons, or camera
// or intrument mounts
//
//#define RC_5_FUNCT RC_5_FUNCT_NONE
//etc.
//////////////////////////////////////////////////////////////////////////////
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
@ -411,9 +398,9 @@
// also means that you should avoid switching out of MANUAL while you have
// any control stick deflection.
//
// The default is to enable AUTO_TRIM.
// The default is to disable AUTO_TRIM.
//
//#define AUTO_TRIM ENABLED
//#define AUTO_TRIM DISABLED
//
//////////////////////////////////////////////////////////////////////////////

5
ArduPlane/ArduPlane.pde
View File

@ -415,6 +415,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
@ -501,7 +502,7 @@ static void fast_loop()
hil.update();
#endif
dcm.update_DCM(G_Dt);
dcm.update_DCM();
// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();
@ -710,6 +711,8 @@ static void slow_loop()
// ----------------------------------
update_servo_switches();
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
break;
case 2:

36
ArduPlane/Attitude.pde
View File

@ -61,7 +61,7 @@ static void stabilize()
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) {
// TODO: use RC_Channel control_mix function?
@ -92,7 +92,7 @@ static void stabilize()
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
// important for steering on the ground during landing
// -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
@ -274,21 +274,16 @@ static void set_servos(void)
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
G_RC_AUX(k_aileron)->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
} else {
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.servo_out = g.channel_roll.servo_out;
g.rc_5.calc_pwm();
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.servo_out = g.channel_roll.servo_out;
g.rc_6.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron]) {
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
}
}else{
@ -320,8 +315,7 @@ static void set_servos(void)
}
if(control_mode <= FLY_BY_WIRE_B) {
if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
G_RC_AUX(k_flap_auto)->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
} else if (control_mode >= FLY_BY_WIRE_C) {
if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise;
@ -329,14 +323,11 @@ static void set_servos(void)
flapSpeedSource = g.throttle_cruise;
}
if ( flapSpeedSource > g.flap_1_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
G_RC_AUX(k_flap_auto)->servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent;
G_RC_AUX(k_flap_auto)->servo_out = g.flap_1_percent;
} else {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent;
G_RC_AUX(k_flap_auto)->servo_out = g.flap_2_percent;
}
}
@ -347,8 +338,11 @@ static void set_servos(void)
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
g.rc_7.output_ch(CH_7);
g.rc_8.output_ch(CH_8);
#endif
}

164
ArduPlane/CMakeLists.txt Normal file
View File

@ -0,0 +1,164 @@
#=============================================================================#
# Author: Niklaa Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
#include(ArduinoProcessing)
set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
message(STATUS "DIR: ${CMAKE_SOURCE_DIR}")
cmake_minimum_required(VERSION 2.8)
#====================================================================#
# Setup Project #
#====================================================================#
project(ArduPlane C CXX)
find_package(Arduino 22 REQUIRED)
add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter)
#add_subdirectory(testtool)
PRINT_BOARD_SETTINGS(mega2560)
#=============================================================================#
# Author: Niklas Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
#====================================================================#
# Settings #
#====================================================================#
set(FIRMWARE_NAME ArduPlane)
set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board
set(${FIRMWARE_NAME}_SKETCHES
ArduPlane.pde
Attitude.pde
climb_rate.pde
commands.pde
commands_logic.pde
commands_process.pde
control_modes.pde
events.pde
#flip.pde
#GCS.pde
#GCS_Ardupilot.pde
#GCS_IMU_output.pde
#GCS_Jason_text.pde
GCS_Mavlink.pde
#GCS_Standard.pde
#GCS_Xplane.pde
#heli.pde
HIL_Xplane.pde
#leds.pde
Log.pde
#motors_hexa.pde
#motors_octa.pde
#motors_octa_quad.pde
#motors_quad.pde
#motors_tri.pde
#motors_y6.pde
navigation.pde
planner.pde
radio.pde
#read_commands.pde
sensors.pde
setup.pde
system.pde
test.pde
) # Firmware sketches
#create dummy sourcefile
file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit")
set(${FIRMWARE_NAME}_SRCS
#test.cpp
${FIRMWARE_NAME}.cpp
) # Firmware sources
set(${FIRMWARE_NAME}_HDRS
APM_Config.h
APM_Config_mavlink_hil.h
APM_Config_xplane.h
config.h
defines.h
GCS.h
HIL.h
Mavlink_Common.h
Parameters.h
) # Firmware sources
set(${FIRMWARE_NAME}_LIBS
DataFlash
AP_Math
PID
RC_Channel
AP_OpticalFlow
ModeFilter
memcheck
#AP_PID
APM_PI
#APO
FastSerial
AP_Common
GCS_MAVLink
AP_GPS
APM_RC
AP_DCM
AP_ADC
AP_Compass
AP_IMU
AP_RangeFinder
APM_BMP085
c
m
)
SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX)
#${CONSOLE_PORT}
set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port
set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd
include_directories(
${CMAKE_SOURCE_DIR}/libraries/DataFlash
${CMAKE_SOURCE_DIR}/libraries/AP_Math
${CMAKE_SOURCE_DIR}/libraries/PID
${CMAKE_SOURCE_DIR}/libraries/AP_Common
${CMAKE_SOURCE_DIR}/libraries/RC_Channel
${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow
${CMAKE_SOURCE_DIR}/libraries/ModeFilter
${CMAKE_SOURCE_DIR}/libraries/memcheck
#${CMAKE_SOURCE_DIR}/libraries/AP_PID
${CMAKE_SOURCE_DIR}/libraries/APM_PI
${CMAKE_SOURCE_DIR}/libraries/FastSerial
${CMAKE_SOURCE_DIR}/libraries/AP_Compass
${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
${CMAKE_SOURCE_DIR}/libraries/AP_GPS
${CMAKE_SOURCE_DIR}/libraries/AP_IMU
${CMAKE_SOURCE_DIR}/libraries/AP_ADC
${CMAKE_SOURCE_DIR}/libraries/AP_DCM
${CMAKE_SOURCE_DIR}/libraries/APM_RC
${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
#new
#${CMAKE_SOURCE_DIR}/libraries/Wire
#${CMAKE_SOURCE_DIR}/libraries/SPI
)
#====================================================================#
# Target generation #
#====================================================================#
generate_arduino_firmware(${FIRMWARE_NAME})

24
ArduPlane/GCS_Mavlink.pde
View File

@ -51,6 +51,7 @@ GCS_MAVLINK::update(void)
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
while(comm_get_available(chan))
@ -1084,15 +1085,6 @@ GCS_MAVLINK::_queued_send()
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
static void send_rate(uint16_t low, uint16_t high) {
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(low, high);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(low,high);
#endif
}
/*
a delay() callback that processes MAVLink packets. We set this as the
callback in long running library initialisation routines to allow
@ -1102,7 +1094,7 @@ static void send_rate(uint16_t low, uint16_t high) {
static void mavlink_delay(unsigned long t)
{
unsigned long tstart;
static unsigned long last_1hz, last_3hz, last_10hz, last_50hz;
static unsigned long last_1hz, last_50hz;
if (in_mavlink_delay) {
// this should never happen, but let's not tempt fate by
@ -1119,18 +1111,11 @@ static void mavlink_delay(unsigned long t)
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs.send_message(MSG_HEARTBEAT);
gcs.send_message(MSG_EXTENDED_STATUS1);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
hil.send_message(MSG_EXTENDED_STATUS1);
#endif
send_rate(1, 3);
}
if (tnow - last_3hz > 333) {
last_3hz = tnow;
send_rate(3, 5);
}
if (tnow - last_10hz > 100) {
last_10hz = tnow;
send_rate(5, 45);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
@ -1138,7 +1123,6 @@ static void mavlink_delay(unsigned long t)
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.update();
#endif
send_rate(45, 1000);
}
delay(1);
} while (millis() - tstart < t);

2
ArduPlane/HIL_Xplane.pde
View File

@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
break;
case MSG_LOCATION:
break;
case MSG_LOCAL_LOCATION:
break;
case MSG_GPS_RAW:
break;
case MSG_SERVO_OUT:

636
ArduPlane/Mavlink_Common.h
View File

@ -25,327 +25,369 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
}
}
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
/*
!!NOTE!!
the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
#define NOINLINE __attribute__((noinline))
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage * 1000,
battery_remaining,
packet_drops);
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
crosstrack_error);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(int)g.channel_throttle.servo_out,
current_loc.alt / 100.0,
climb_rate);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-g.ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
}
break;
case MSG_EXTENDED_STATUS1:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
}
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage * 1000,
battery_remaining,
packet_drops);
break;
}
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_ATTITUDE:
{
CHECK_PAYLOAD_SIZE(ATTITUDE);
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(
chan,
timeStamp,
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
break;
}
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_LOCATION:
{
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
break;
}
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_NAV_CONTROLLER_OUTPUT:
{
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
crosstrack_error);
}
break;
}
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_LOCAL_LOCATION:
{
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(
chan,
timeStamp,
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
(current_loc.alt - home.alt) / 1.0e2,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
}
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
break;
}
case MSG_SERVO_OUT:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
break;
}
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
break;
}
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
break;
}
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(int)g.channel_throttle.servo_out,
current_loc.alt / 100.0,
climb_rate);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
{
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(
chan,
timeStamp,
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send(
chan,
timeStamp,
(float)barometer.Press/100.0,
(float)(barometer.Press-g.ground_pressure)/100.0,
(int)(barometer.Temp*10));
break;
}
case MSG_RAW_IMU3:
{
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
break;
}
default:
break;
default:
break;
}
return true;
}

32
ArduPlane/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 11;
static const uint16_t k_format_version = 12;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -95,6 +95,9 @@ public:
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_input_voltage,
k_param_pack_capacity,
k_param_airspeed_offset,
k_param_sonar_enabled,
@ -135,11 +138,6 @@ public:
k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
k_param_rc_5_funct,
k_param_rc_6_funct,
k_param_rc_7_funct,
k_param_rc_8_funct,
//
// 200: Feed-forward gains
//
@ -313,6 +311,9 @@ public:
AP_Int8 compass_enabled;
AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Float input_voltage;
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 sonar_enabled;
@ -327,14 +328,10 @@ public:
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel rc_5;
RC_Channel rc_6;
RC_Channel rc_7;
RC_Channel rc_8;
AP_Int8 rc_5_funct;
AP_Int8 rc_6_funct;
AP_Int8 rc_7_funct;
AP_Int8 rc_8_funct;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
// PID controllers
//
@ -424,14 +421,13 @@ public:
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")),
curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")),
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")),
rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")),
rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")),
rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!

15
ArduPlane/config.h
View File

@ -226,19 +226,6 @@
# define CH8_MAX 2000
#endif
//////////////////////////////////////////////////////////////////////////////
#ifndef RC_5_FUNCT
# define RC_5_FUNCT RC_5_FUNCT_NONE
#endif
#ifndef RC_6_FUNCT
# define RC_6_FUNCT RC_6_FUNCT_NONE
#endif
#ifndef RC_7_FUNCT
# define RC_7_FUNCT RC_7_FUNCT_NONE
#endif
#ifndef RC_8_FUNCT
# define RC_8_FUNCT RC_8_FUNCT_NONE
#endif
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
@ -313,7 +300,7 @@
// AUTO_TRIM
//
#ifndef AUTO_TRIM
# define AUTO_TRIM ENABLED
# define AUTO_TRIM DISABLED
#endif

32
ArduPlane/defines.h
View File

@ -41,39 +41,12 @@
#define GPS_PROTOCOL_MTK16 6
#define GPS_PROTOCOL_AUTO 7
// Radio channels
// Note channels are from 0!
//
// XXX these should be CH_n defines from RC.h at some point.
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define RC_5_FUNCT_NONE 0
#define RC_5_FUNCT_AILERON 1
#define RC_5_FUNCT_FLAP_AUTO 2
#define RC_5_FUNCT_FLAPERON 3
#define RC_6_FUNCT_NONE 0
#define RC_6_FUNCT_AILERON 1
#define RC_6_FUNCT_FLAP_AUTO 2
#define RC_6_FUNCT_FLAPERON 3
#define RC_7_FUNCT_NONE 0
#define RC_8_FUNCT_NONE 0
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2
@ -190,7 +163,6 @@
#define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1
@ -247,9 +219,9 @@
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers

2
ArduPlane/events.pde
View File

@ -43,12 +43,12 @@ static void failsafe_long_on_event()
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
case CIRCLE:
set_mode(RTL);
break;
case AUTO:
case LOITER:
case CIRCLE:
if(g.long_fs_action == 1) {
set_mode(RTL);
}

34
ArduPlane/radio.pde
View File

@ -23,24 +23,7 @@ static void init_rc_in()
g.channel_throttle.dead_zone = 6;
//set auxiliary ranges
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.set_angle(SERVO_MAX);
} else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) {
g.rc_5.set_range(0,100);
} else {
g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.set_angle(SERVO_MAX);
} else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) {
g.rc_6.set_range(0,100);
} else {
g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here
g.rc_8.set_range(0,1000);
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
}
static void init_rc_out()
@ -53,7 +36,7 @@ static void init_rc_out()
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.Init(); // APM Radio initialization
@ -173,8 +156,7 @@ static void trim_control_surfaces()
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{
elevon1_trim = ch1_temp;
@ -191,8 +173,7 @@ static void trim_control_surfaces()
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}
static void trim_radio()
@ -208,8 +189,7 @@ static void trim_radio()
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
elevon1_trim = ch1_temp;
@ -225,7 +205,5 @@ static void trim_radio()
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}

2
ArduPlane/system.pde
View File

@ -41,7 +41,7 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "APM trunk", main_menu_commands);
MENU(main_menu, THISFIRMWARE, main_menu_commands);
#endif // CLI_ENABLED

6
ArduPlane/test.pde
View File

@ -418,7 +418,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
delay(1000);
while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i));
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i));
Serial.println();
delay(100);
if(Serial.available() > 0){
@ -476,7 +476,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
dcm.update_DCM(G_Dt);
dcm.update_DCM();
if(g.compass_enabled) {
medium_loopCounter++;
@ -563,7 +563,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
dcm.update_DCM(G_Dt);
dcm.update_DCM();
medium_loopCounter++;
if(medium_loopCounter == 5){

2
Tools/ArduTracker/APM_Config.h.reference
View File

@ -2,7 +2,7 @@
// Example and reference ArduPilot Mega configuration file
// =======================================================
//
// This file contains documentation and examples for configuration options
// This file contains documentation and examples of configuration options
// supported by the ArduPilot Mega software.
//
// Most of these options are just that - optional. You should create

6
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -188,6 +188,7 @@
<SubType>Component</SubType>
</Compile>
<Compile Include="CommsUdpSerial.cs" />
<Compile Include="georefimage.cs" />
<Compile Include="HIL\Aircraft.cs" />
<Compile Include="HIL\Point3d.cs" />
<Compile Include="HIL\QuadCopter.cs" />
@ -232,7 +233,6 @@
<Compile Include="MyUserControl.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="NetSerialServer.cs" />
<Compile Include="ArduinoSTKv2.cs">
<SubType>Component</SubType>
</Compile>
@ -302,7 +302,6 @@
<DependentUpon>ElevationProfile.cs</DependentUpon>
</Compile>
<Compile Include="MAVLink.cs" />
<Compile Include="NetSerial.cs" />
<Compile Include="ArduinoSTK.cs">
<SubType>Component</SubType>
</Compile>
@ -338,6 +337,7 @@
<Compile Include="Splash.Designer.cs">
<DependentUpon>Splash.cs</DependentUpon>
</Compile>
<Compile Include="srtm.cs" />
<Compile Include="temp.cs">
<SubType>Form</SubType>
</Compile>
@ -374,6 +374,7 @@
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\Firmware.zh-Hans.resx">
<DependentUpon>Firmware.cs</DependentUpon>
<SubType>Designer</SubType>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\FlightData.zh-Hans.resx">
<DependentUpon>FlightData.cs</DependentUpon>
@ -424,6 +425,7 @@
<EmbeddedResource Include="GCSViews\Firmware.resx">
<DependentUpon>Firmware.cs</DependentUpon>
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
<SubType>Designer</SubType>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\FlightData.resx">
<DependentUpon>FlightData.cs</DependentUpon>

13
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -240,13 +240,12 @@ namespace ArdupilotMega
{
STABILIZE = 0, // hold level position
ACRO = 1, // rate control
SIMPLE = 2, //
ALT_HOLD = 3, // AUTO control
AUTO = 4, // AUTO control
GUIDED = 5, // AUTO control
LOITER = 6, // Hold a single location
RTL = 7, // AUTO control
CIRCLE = 8
ALT_HOLD = 2, // AUTO control
AUTO = 3, // AUTO control
GUIDED = 4, // AUTO control
LOITER = 5, // Hold a single location
RTL = 6, // AUTO control
CIRCLE = 7
}
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode)

1
Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs
View File

@ -6,7 +6,6 @@ using System.IO.Ports;
using System.Threading;
using System.Net; // dns, ip address
using System.Net.Sockets; // tcplistner
using SerialProxy;
namespace System.IO.Ports
{

53
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -65,7 +65,7 @@ namespace ArdupilotMega
public float gz { get; set; }
// calced turn rate
public float turnrate { get { if (groundspeed == 0) return 0; return (roll * 9.8f) / groundspeed; } }
public float turnrate { get { if (groundspeed <= 1) return 0; return (roll * 9.8f) / groundspeed; } }
//radio
public float ch1in { get; set; }
@ -101,17 +101,17 @@ namespace ArdupilotMega
//nav state
public float nav_roll { get; set; }
public float nav_pitch { get; set; }
public short nav_bearing { get; set; }
public short target_bearing { get; set; }
public ushort wp_dist { get { return (ushort)(_wpdist * multiplierdist); } set { _wpdist = value; } }
public float nav_bearing { get; set; }
public float target_bearing { get; set; }
public float wp_dist { get { return (_wpdist * multiplierdist); } set { _wpdist = value; } }
public float alt_error { get { return _alt_error * multiplierdist; } set { _alt_error = value; } }
public float ber_error { get { return (target_bearing - yaw); } set { } }
public float aspd_error { get { return _aspd_error * multiplierspeed; } set { _aspd_error = value; } }
public float xtrack_error { get; set; }
public int wpno { get; set; }
public float wpno { get; set; }
public string mode { get; set; }
public float climbrate { get; set; }
ushort _wpdist;
float _wpdist;
float _aspd_error;
float _alt_error;
@ -150,7 +150,8 @@ namespace ArdupilotMega
public float brklevel { get; set; }
// stats
public ushort packetdrop { get; set; }
public ushort packetdropremote { get; set; }
public ushort linkqualitygcs { get; set; }
// requested stream rates
public byte rateattitude { get; set; }
@ -284,24 +285,21 @@ namespace ArdupilotMega
mode = "Acro";
break;
case (byte)102:
mode = "Simple";
break;
case (byte)103:
mode = "Alt Hold";
break;
case (byte)104:
case (byte)103:
mode = "Auto";
break;
case (byte)105:
case (byte)104:
mode = "Guided";
break;
case (byte)106:
case (byte)105:
mode = "Loiter";
break;
case (byte)107:
case (byte)106:
mode = "RTL";
break;
case (byte)108:
case (byte)107:
mode = "Circle";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
@ -361,7 +359,7 @@ namespace ArdupilotMega
battery_voltage = sysstatus.vbat;
battery_remaining = sysstatus.battery_remaining;
packetdrop = sysstatus.packet_drop;
packetdropremote = sysstatus.packet_drop;
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{
@ -468,7 +466,7 @@ namespace ArdupilotMega
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);
int oldwp = wpno;
int oldwp = (int)wpno;
wpno = wpcur.seq;
@ -545,6 +543,27 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null)
{
var imu = new MAVLink.__mavlink_scaled_imu_t();
object temp = imu;
MAVLink.ByteArrayToStructure(MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6);
imu = (MAVLink.__mavlink_scaled_imu_t)(temp);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
{
MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();

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

@ -30,8 +30,8 @@
{
this.components = new System.ComponentModel.Container();
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration));
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
this.Params = new System.Windows.Forms.DataGridView();
this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn();
this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn();
@ -167,9 +167,6 @@
this.label21 = new System.Windows.Forms.Label();
this.THR_HOLD_P = new System.Windows.Forms.DomainUpDown();
this.label22 = new System.Windows.Forms.Label();
this.groupBox18 = new System.Windows.Forms.GroupBox();
this.PITCH_MAX = new System.Windows.Forms.DomainUpDown();
this.label27 = new System.Windows.Forms.Label();
this.groupBox19 = new System.Windows.Forms.GroupBox();
this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown();
this.label28 = new System.Windows.Forms.Label();
@ -285,7 +282,6 @@
this.groupBox4.SuspendLayout();
this.groupBox6.SuspendLayout();
this.groupBox7.SuspendLayout();
this.groupBox18.SuspendLayout();
this.groupBox19.SuspendLayout();
this.groupBox20.SuspendLayout();
this.groupBox21.SuspendLayout();
@ -302,14 +298,14 @@
this.Params.AllowUserToAddRows = false;
this.Params.AllowUserToDeleteRows = false;
resources.ApplyResources(this.Params, "Params");
dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon;
dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3;
dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon;
dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1;
this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
this.Command,
@ -318,14 +314,14 @@
this.mavScale,
this.RawValue});
this.Params.Name = "Params";
dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption;
dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4;
dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption;
dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2;
this.Params.RowHeadersVisible = false;
this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged);
//
@ -990,7 +986,6 @@
this.TabAC2.Controls.Add(this.groupBox4);
this.TabAC2.Controls.Add(this.groupBox6);
this.TabAC2.Controls.Add(this.groupBox7);
this.TabAC2.Controls.Add(this.groupBox18);
this.TabAC2.Controls.Add(this.groupBox19);
this.TabAC2.Controls.Add(this.groupBox20);
this.TabAC2.Controls.Add(this.groupBox21);
@ -1157,24 +1152,6 @@
resources.ApplyResources(this.label22, "label22");
this.label22.Name = "label22";
//
// groupBox18
//
this.groupBox18.Controls.Add(this.PITCH_MAX);
this.groupBox18.Controls.Add(this.label27);
resources.ApplyResources(this.groupBox18, "groupBox18");
this.groupBox18.Name = "groupBox18";
this.groupBox18.TabStop = false;
//
// PITCH_MAX
//
resources.ApplyResources(this.PITCH_MAX, "PITCH_MAX");
this.PITCH_MAX.Name = "PITCH_MAX";
//
// label27
//
resources.ApplyResources(this.label27, "label27");
this.label27.Name = "label27";
//
// groupBox19
//
this.groupBox19.Controls.Add(this.HLD_LAT_IMAX);
@ -1887,7 +1864,6 @@
this.groupBox4.ResumeLayout(false);
this.groupBox6.ResumeLayout(false);
this.groupBox7.ResumeLayout(false);
this.groupBox18.ResumeLayout(false);
this.groupBox19.ResumeLayout(false);
this.groupBox20.ResumeLayout(false);
this.groupBox21.ResumeLayout(false);
@ -2031,9 +2007,6 @@
private System.Windows.Forms.Label label21;
private System.Windows.Forms.DomainUpDown THR_HOLD_P;
private System.Windows.Forms.Label label22;
private System.Windows.Forms.GroupBox groupBox18;
private System.Windows.Forms.DomainUpDown PITCH_MAX;
private System.Windows.Forms.Label label27;
private System.Windows.Forms.GroupBox groupBox19;
private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX;
private System.Windows.Forms.Label label28;

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

@ -257,6 +257,8 @@ namespace ArdupilotMega.GCSViews
internal void processToScreen()
{
Params.Rows.Clear();
// process hashdefines and update display
@ -541,7 +543,10 @@ namespace ArdupilotMega.GCSViews
private void BUT_writePIDS_Click(object sender, EventArgs e)
{
foreach (string value in changes.Keys)
Hashtable temp = (Hashtable)changes.Clone();
foreach (string value in temp.Keys)
{
try
{
@ -566,6 +571,7 @@ namespace ArdupilotMega.GCSViews
if (row.Cells[0].Value.ToString() == value)
{
row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
changes.Remove(value);
break;
}
}
@ -575,8 +581,6 @@ namespace ArdupilotMega.GCSViews
}
catch { MessageBox.Show("Set " + value + " Failed"); }
}
changes.Clear();
}
const float rad2deg = (float)(180 / Math.PI);
@ -600,9 +604,11 @@ namespace ArdupilotMega.GCSViews
MainV2.fixtheme(temp);
TabSetup.Controls.Clear();
temp.ShowDialog();
TabSetup.Controls.Add(temp.Controls[0]);
startup = true;
processToScreen();
startup = false;
}
}
}

10694
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx
View File

File diff suppressed because it is too large Load Diff

7
Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
View File

@ -519,13 +519,18 @@ namespace ArdupilotMega.GCSViews
}
catch (Exception ex) { lbl_status.Text = "Failed download"; MessageBox.Show("Failed to download new firmware : " + ex.Message); return; }
UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board);
}
void UploadFlash(string filename, string board)
{
byte[] FLASH = new byte[1];
StreamReader sr = null;
try
{
lbl_status.Text = "Reading Hex File";
this.Refresh();
sr = new StreamReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex");
sr = new StreamReader(filename);
FLASH = readIntelHEXv2(sr);
sr.Close();
Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length);

12
Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx
View File

@ -343,16 +343,16 @@
<value>NoControl</value>
</data>
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
<value>101, 165</value>
<value>113, 167</value>
</data>
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
<value>79, 13</value>
<value>56, 13</value>
</data>
<data name="label1.TabIndex" type="System.Int32, mscorlib">
<value>8</value>
</data>
<data name="label1.Text" xml:space="preserve">
<value>ArduPilot Mega</value>
<value>ArduPlane</value>
</data>
<data name="&gt;&gt;label1.Name" xml:space="preserve">
<value>label1</value>
@ -403,16 +403,16 @@
<value>NoControl</value>
</data>
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
<value>57, 362</value>
<value>74, 361</value>
</data>
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
<value>168, 13</value>
<value>142, 13</value>
</data>
<data name="label3.TabIndex" type="System.Int32, mscorlib">
<value>10</value>
</data>
<data name="label3.Text" xml:space="preserve">
<value>ArduPilot Mega (Xplane simulator)</value>
<value>ArduPlane (Xplane simulator)</value>
</data>
<data name="&gt;&gt;label3.Name" xml:space="preserve">
<value>label3</value>

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

@ -436,10 +436,13 @@ namespace ArdupilotMega.GCSViews
{
this.BeginInvoke((MethodInvoker)delegate()
{
tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100);
lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%");
try
{
tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100);
lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%");
}
catch { }
});
}
@ -861,6 +864,8 @@ namespace ArdupilotMega.GCSViews
if (MainV2.comPort.logplaybackfile != null)
MainV2.comPort.logplaybackfile.BaseStream.Position = (long)(MainV2.comPort.logplaybackfile.BaseStream.Length * (tracklog.Value / 100.0));
updateLogPlayPosition();
}
bool loaded = false;
@ -1223,7 +1228,7 @@ namespace ArdupilotMega.GCSViews
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
if (!(typeCode == TypeCode.Single || typeCode == TypeCode.Double || typeCode == TypeCode.Int32))
if (!(typeCode == TypeCode.Single))
continue;
CheckBox chk_box = new CheckBox();

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

@ -347,7 +347,7 @@ namespace ArdupilotMega.GCSViews
{
TXT_mouselat.Text = lat.ToString();
TXT_mouselong.Text = lng.ToString();
TXT_mousealt.Text = alt.ToString();
TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0");
try
{
@ -1228,7 +1228,7 @@ namespace ArdupilotMega.GCSViews
if (CHK_holdalt.Checked)
{
port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist * 100);
port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist);
}
else
{
@ -1360,7 +1360,7 @@ namespace ArdupilotMega.GCSViews
}
}
TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist / 100).ToString("0");
TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist).ToString("0");
TXT_WPRad.Text = ((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist).ToString("0");
try
{

140
Tools/ArdupilotMegaPlanner/HUD.cs
View File

@ -23,7 +23,7 @@ using OpenTK.Graphics;
namespace hud
{
public partial class HUD : GLControl
public class HUD : GLControl// : Graphics
{
object paintlock = new object();
object streamlock = new object();
@ -38,7 +38,9 @@ namespace hud
int[] charbitmaptexid = new int[6000];
int[] charwidth = new int[6000];
int huddrawtime = 0;
public int huddrawtime = 0;
public bool opengl = true;
public HUD()
{
@ -46,7 +48,7 @@ namespace hud
return;
InitializeComponent();
graphicsObject = this;
//graphicsObject = this;
//graphicsObject = Graphics.FromImage(objBitmap);
}
@ -150,7 +152,6 @@ System.ComponentModel.Category("Values")]
Bitmap objBitmap = new Bitmap(1024, 1024);
int count = 0;
DateTime countdate = DateTime.Now;
HUD graphicsObject; // Graphics
DateTime starttime = DateTime.MinValue;
@ -236,40 +237,45 @@ System.ComponentModel.Category("Values")]
{
//Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds);
//e.Graphics.DrawImageUnscaled(objBitmap, 0, 0);
return;
//return;
}
starttime = DateTime.Now;
//Console.WriteLine(DateTime.Now.Millisecond);
if (Console.CursorLeft > 0)
{
//Console.WriteLine(" "+ Console.CursorLeft +" ");
}
//Console.Write("HUD a "+(DateTime.Now - starttime).TotalMilliseconds);
MakeCurrent();
//GL.LoadIdentity();
//GL.ClearColor(Color.Red);
//Console.Write(" b " + (DateTime.Now - starttime).TotalMilliseconds);
GL.Clear(ClearBufferMask.ColorBufferBit);
//GL.LoadIdentity();
//Console.Write(" c " + (DateTime.Now - starttime).TotalMilliseconds);
//GL.Viewport(0, 0, Width, Height);
doPaint(e);
doPaint();
//Console.Write(" d " + (DateTime.Now - starttime).TotalMilliseconds);
this.SwapBuffers();
//Console.Write(" e " + (DateTime.Now - starttime).TotalMilliseconds);
count++;
huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds;
if (DateTime.Now.Second != countdate.Second)
{
countdate = DateTime.Now;
Console.WriteLine("HUD " + count + " hz drawtime " + huddrawtime);
Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count));
count = 0;
huddrawtime = 0;
}
huddrawtime = (int)(DateTime.Now - starttime).TotalMilliseconds;
#if DEBUG
//Console.WriteLine("HUD e " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond);
#endif
}
void Clear(Color color)
@ -282,7 +288,7 @@ System.ComponentModel.Category("Values")]
//graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90);
void DrawArc(Pen penn,RectangleF rect, float start,float degrees)
public void DrawArc(Pen penn,RectangleF rect, float start,float degrees)
{
//GL.Disable(EnableCap.Texture2D);
@ -309,7 +315,7 @@ System.ComponentModel.Category("Values")]
//GL.Enable(EnableCap.Texture2D);
}
void DrawEllipse(Pen penn, Rectangle rect)
public void DrawEllipse(Pen penn, Rectangle rect)
{
//GL.Disable(EnableCap.Texture2D);
@ -340,7 +346,7 @@ System.ComponentModel.Category("Values")]
int texture;
Bitmap bitmap = new Bitmap(512,512);
void DrawImage(Image img, int x, int y, int width, int height)
public void DrawImage(Image img, int x, int y, int width, int height)
{
if (img == null)
return;
@ -392,7 +398,7 @@ System.ComponentModel.Category("Values")]
GL.Disable(EnableCap.Texture2D);
}
void DrawPath(Pen penn,GraphicsPath gp)
public void DrawPath(Pen penn, GraphicsPath gp)
{
try
{
@ -401,7 +407,7 @@ System.ComponentModel.Category("Values")]
catch { }
}
void FillPath(Brush brushh,GraphicsPath gp)
public void FillPath(Brush brushh, GraphicsPath gp)
{
try
{
@ -410,32 +416,32 @@ System.ComponentModel.Category("Values")]
catch { }
}
void SetClip(Rectangle rect)
public void SetClip(Rectangle rect)
{
}
void ResetClip()
public void ResetClip()
{
}
void ResetTransform()
public void ResetTransform()
{
GL.LoadIdentity();
}
void RotateTransform(float angle)
public void RotateTransform(float angle)
{
GL.Rotate(angle,0,0,1);
}
void TranslateTransform(float x, float y)
public void TranslateTransform(float x, float y)
{
GL.Translate(x, y, 0f);
}
void FillPolygon(Brush brushh, Point[] list)
public void FillPolygon(Brush brushh, Point[] list)
{
//GL.Disable(EnableCap.Texture2D);
@ -456,7 +462,7 @@ System.ComponentModel.Category("Values")]
//GL.Enable(EnableCap.Texture2D);
}
void FillPolygon(Brush brushh, PointF[] list)
public void FillPolygon(Brush brushh, PointF[] list)
{
//GL.Disable(EnableCap.Texture2D);
@ -478,7 +484,7 @@ System.ComponentModel.Category("Values")]
//graphicsObject.DrawPolygon(redPen, pointlist);
void DrawPolygon(Pen penn, Point[] list)
public void DrawPolygon(Pen penn, Point[] list)
{
//GL.Disable(EnableCap.Texture2D);
@ -499,7 +505,7 @@ System.ComponentModel.Category("Values")]
//GL.Enable(EnableCap.Texture2D);
}
void DrawPolygon(Pen penn, PointF[] list)
public void DrawPolygon(Pen penn, PointF[] list)
{
//GL.Disable(EnableCap.Texture2D);
@ -523,7 +529,7 @@ System.ComponentModel.Category("Values")]
//graphicsObject.FillRectangle(linearBrush, bg);
void FillRectangle(Brush brushh,RectangleF rectf)
public void FillRectangle(Brush brushh, RectangleF rectf)
{
float x1 = rectf.X;
float y1 = rectf.Y;
@ -571,12 +577,12 @@ System.ComponentModel.Category("Values")]
//graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height);
void DrawRectangle(Pen penn, RectangleF rect)
public void DrawRectangle(Pen penn, RectangleF rect)
{
DrawRectangle(penn, rect.X, rect.Y, rect.Width, rect.Height);
}
void DrawRectangle(Pen penn,double x1,double y1, double width,double height)
public void DrawRectangle(Pen penn, double x1, double y1, double width, double height)
{
//GL.Disable(EnableCap.Texture2D);
@ -597,7 +603,7 @@ System.ComponentModel.Category("Values")]
//GL.Enable(EnableCap.Texture2D);
}
void DrawLine(Pen penn,double x1,double y1, double x2,double y2)
public void DrawLine(Pen penn, double x1, double y1, double x2, double y2)
{
//GL.Disable(EnableCap.Texture2D);
@ -616,8 +622,13 @@ System.ComponentModel.Category("Values")]
//GL.Enable(EnableCap.Texture2D);
}
void doPaint()
void doPaint(object e)
{
HUD graphicsObject = this;
//Graphics graphicsObject = ((PaintEventArgs)e).Graphics;
//graphicsObject.SmoothingMode = SmoothingMode.AntiAlias;
try
{
graphicsObject.Clear(Color.Gray);
@ -1055,7 +1066,7 @@ System.ComponentModel.Category("Values")]
}
}
greenPen.Width = 4;
// vsi
@ -1131,6 +1142,20 @@ System.ComponentModel.Category("Values")]
drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5);
drawstring(graphicsObject, (int)disttowp + ">" + wpno, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + fontsize + 2 + 10);
graphicsObject.DrawLine(greenPen, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize) - 2 - 20);
graphicsObject.DrawLine(greenPen, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 15, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize) - 2 - 20);
graphicsObject.DrawLine(greenPen, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 10, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize ) - 2 - 20);
drawstring(graphicsObject, ArdupilotMega.MainV2.cs.linkqualitygcs.ToString("0") + "%", font, fontsize, whiteBrush, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20);
if (ArdupilotMega.MainV2.cs.linkqualitygcs == 0)
{
graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2);
graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20);
}
drawstring(graphicsObject, ArdupilotMega.MainV2.cs.datetime.ToString("HH:mm:ss"), font, fontsize, whiteBrush, scrollbg.Left - 20, scrollbg.Top - fontsize - 2 - 20);
// battery
graphicsObject.ResetTransform();
@ -1191,8 +1216,6 @@ System.ComponentModel.Category("Values")]
Console.WriteLine("hud error "+ex.ToString());
//MessageBox.Show(ex.ToString());
}
count++;
}
protected override void OnPaintBackground(PaintEventArgs e)
@ -1356,6 +1379,31 @@ System.ComponentModel.Category("Values")]
}
}
void drawstring(Graphics e, string text, Font font, float fontsize, Brush brush, float x, float y)
{
if (text == null || text == "")
return;
pth.Reset();
if (text != null)
pth.AddString(text, font.FontFamily, 0, fontsize + 5, new Point((int)x, (int)y), StringFormat.GenericTypographic);
//Draw the edge
// this uses lots of cpu time
//e.SmoothingMode = SmoothingMode.HighSpeed;
e.DrawPath(P, pth);
//Draw the face
e.FillPath(brush, pth);
//pth.Dispose();
}
protected override void OnResize(EventArgs e)
{
if (DesignMode)
@ -1365,6 +1413,18 @@ System.ComponentModel.Category("Values")]
charbitmaps = new Bitmap[charbitmaps.Length];
try
{
foreach (int texid in charbitmaptexid)
{
if (texid != 0)
GL.DeleteTexture(texid);
}
}
catch { }
GC.Collect();
try

52
Tools/ArdupilotMegaPlanner/Log.cs
View File

@ -366,45 +366,6 @@ namespace ArdupilotMega
}
}
private byte[] readline(NetSerial comport)
{
byte[] temp = new byte[1024];
int count = 0;
int timeout = 0;
while (timeout <= 100)
{
if (!comport.IsOpen) { break; }
if (comport.BytesToRead > 0)
{
byte letter = (byte)comport.ReadByte();
temp[count] = letter;
if (letter == '\n') // normal line
{
break;
}
count++;
if (count == temp.Length)
break;
timeout = 0;
} else {
timeout++;
System.Threading.Thread.Sleep(5);
}
}
if (timeout >= 100) {
Console.WriteLine("readline timeout");
}
Array.Resize<byte>(ref temp, count + 1);
return temp;
}
List<string> modelist = new List<string>();
List<Core.Geometry.Point3D>[] position = new List<Core.Geometry.Point3D>[200];
int positionindex = 0;
@ -414,6 +375,13 @@ namespace ArdupilotMega
{
Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink };
AltitudeMode altmode = AltitudeMode.absolute;
if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
altmode = AltitudeMode.relativeToGround; // because of sonar, this is both right and wrong. right for sonar, wrong in terms of gps as the land slopes off.
}
KMLRoot kml = new KMLRoot();
Folder fldr = new Folder("Log");
@ -436,7 +404,7 @@ namespace ArdupilotMega
continue;
LineString ls = new LineString();
ls.AltitudeMode = AltitudeMode.absolute;
ls.AltitudeMode = altmode;
ls.Extrude = true;
//ls.Tessellate = true;
@ -523,7 +491,7 @@ namespace ArdupilotMega
pmplane.visibility = false;
Model model = mod.model;
model.AltitudeMode = AltitudeMode.absolute;
model.AltitudeMode = altmode;
model.Scale.x = 2;
model.Scale.y = 2;
model.Scale.z = 2;
@ -545,7 +513,7 @@ namespace ArdupilotMega
catch { }
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude);
pmplane.Point.AltitudeMode = AltitudeMode.absolute;
pmplane.Point.AltitudeMode = altmode;
Link link = new Link();
link.href = "block_plane_0.dae";

234
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -16,6 +16,9 @@ namespace ArdupilotMega
{
public ICommsSerial BaseStream = new SerialPort();
/// <summary>
/// used for outbound packet sending
/// </summary>
byte packetcount = 0;
public byte sysid = 0;
public byte compid = 0;
@ -29,6 +32,8 @@ namespace ArdupilotMega
public DateTime lastvalidpacket = DateTime.Now;
bool oldlogformat = false;
byte mavlinkversion = 0;
public PointLatLngAlt[] wps = new PointLatLngAlt[200];
public bool debugmavlink = false;
@ -45,7 +50,12 @@ namespace ArdupilotMega
public int bps = 0;
public DateTime bpstime = DateTime.Now;
int recvpacketcount = 0;
int packetslost = 0;
float synclost;
float packetslost = 0;
float packetsnotlost = 0;
DateTime packetlosttimer = DateTime.Now;
//Stopwatch stopwatch = new Stopwatch();
@ -169,15 +179,25 @@ namespace ArdupilotMega
if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
{
__mavlink_heartbeat_t hb = new __mavlink_heartbeat_t();
object temp = hb;
MAVLink.ByteArrayToStructure(buffer, ref temp, 6);
hb = (MAVLink.__mavlink_heartbeat_t)(temp);
mavlinkversion = hb.mavlink_version;
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
Console.WriteLine("ID " + sysid + " " + compid);
Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion);
break;
}
}
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + ") ";
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") ";
frm.Refresh();
if (getparams == true)
getParamList();
@ -190,6 +210,7 @@ namespace ArdupilotMega
Console.WriteLine("Done open " + sysid + " " + compid);
packetslost = 0;
}
public static byte[] StructureToByteArrayEndian(params object[] list)
@ -331,12 +352,28 @@ namespace ArdupilotMega
/// <param name="indata">struct of data</param>
public void generatePacket(byte messageType, object indata)
{
byte[] data = StructureToByteArrayEndian(indata);
byte[] data;
if (mavlinkversion == 3)
{
data = StructureToByteArray(indata);
}
else
{
data = StructureToByteArrayEndian(indata);
}
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
byte[] packet = new byte[data.Length + 6 + 2];
packet[0] = (byte)'U';
if (mavlinkversion == 3)
{
packet[0] = 254;
}
else if (mavlinkversion == 2)
{
packet[0] = (byte)'U';
}
packet[1] = (byte)data.Length;
packet[2] = packetcount;
packet[3] = 255; // this is always 255 - MYGCS
@ -351,6 +388,12 @@ namespace ArdupilotMega
}
ushort checksum = crc_calculate(packet, packet[1] + 6);
if (mavlinkversion == 3)
{
checksum = crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum);
}
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
byte ck_b = (byte)(checksum >> 8); ///< Low byte
@ -425,6 +468,8 @@ namespace ArdupilotMega
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
modifyParamForDisplay(false, paramname, ref value);
Array.Resize(ref temp, 15);
req.param_id = temp;
@ -458,7 +503,31 @@ namespace ArdupilotMega
{
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
{
param[paramname] = req.param_value;
__mavlink_param_value_t par = new __mavlink_param_value_t();
object tempobj = par;
ByteArrayToStructure(buffer, ref tempobj, 6);
par = (__mavlink_param_value_t)tempobj;
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
if (st != paramname)
{
Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st);
continue;
}
param[st] = (par.param_value);
MainV2.givecomport = false;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
return true;
@ -559,6 +628,8 @@ namespace ArdupilotMega
st = st.Substring(0, pos);
}
modifyParamForDisplay(true, st, ref par.param_value);
param[st] = (par.param_value);
a++;
@ -575,17 +646,21 @@ namespace ArdupilotMega
return param;
}
public _param getParam()
void modifyParamForDisplay(bool fromapm, string paramname, ref float value)
{
throw new Exception("getParam Not implemented");
/*
_param temp = new _param();
temp.name = "none";
temp.value = 0;
return temp;
*/
if (paramname.ToUpper().EndsWith("_IMAX") || paramname.ToUpper().EndsWith("ALT_HOLD_RTL") || paramname.ToUpper().EndsWith("TRIM_ARSPD_CM")
|| paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN")
|| paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX"))
{
if (fromapm)
{
value /= 100.0f;
}
else
{
value *= 100.0f;
}
}
}
/// <summary>
@ -951,6 +1026,8 @@ namespace ArdupilotMega
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
@ -972,11 +1049,14 @@ namespace ArdupilotMega
MainV2.givecomport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
object temp = (object)wp;
@ -1070,7 +1150,7 @@ namespace ArdupilotMega
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
@ -1412,12 +1492,13 @@ namespace ArdupilotMega
}
else
{
MainV2.cs.datetime = DateTime.Now;
temp[count] = (byte)BaseStream.ReadByte();
}
}
catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; }
if (temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync
if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync
{
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n')
{
@ -1432,7 +1513,7 @@ namespace ArdupilotMega
// reset count on valid packet
readcount = 0;
if (temp[0] == 'U')
if (temp[0] == 'U' || temp[0] == 254)
{
length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length
if (count >= 5 || logreadmode)
@ -1455,16 +1536,18 @@ namespace ArdupilotMega
else
{
int to = 0;
while (BaseStream.BytesToRead < length)
while (BaseStream.BytesToRead < (length - 4))
{
if (to > 1000)
{
Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
break;
}
System.Threading.Thread.Sleep(2);
System.Threading.Thread.Sleep(1);
System.Windows.Forms.Application.DoEvents();
to++;
//Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead);
}
int read = BaseStream.Read(temp, 6, length - 4);
}
@ -1484,9 +1567,18 @@ namespace ArdupilotMega
Array.Resize<byte>(ref temp, count);
if (packetlosttimer.AddSeconds(10) < DateTime.Now)
{
packetlosttimer = DateTime.Now;
packetslost = (int)(packetslost * 0.8f);
packetsnotlost = (int)(packetsnotlost * 0.8f);
}
MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100);
if (bpstime.Second != DateTime.Now.Second && !logreadmode)
{
Console.WriteLine("bps {0} loss {1} left {2}", bps1, packetslost, BaseStream.BytesToRead);
Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead);
bps2 = bps1; // prev sec
bps1 = 0; // current sec
bpstime = DateTime.Now;
@ -1498,11 +1590,16 @@ namespace ArdupilotMega
if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet
{
return new byte[0];
return temp;// new byte[0];
}
ushort crc = crc_calculate(temp, temp.Length - 2);
if (temp.Length > 5 && temp[0] == 254)
{
crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc);
}
if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff))
{
int packetno = 0;
@ -1516,13 +1613,27 @@ namespace ArdupilotMega
try
{
if (temp[0] == 'U' && temp.Length >= temp[1])
if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1])
{
if (temp[2] != ((recvpacketcount + 1) % 0x100))
{
Console.WriteLine("lost {0}", temp[2]);
packetslost++; // actualy sync loss's
synclost++; // actualy sync loss's
if (temp[2] < ((recvpacketcount + 1) % 0x100))
{
packetslost += 0x100 - recvpacketcount + temp[2];
}
else
{
packetslost += temp[2] - recvpacketcount;
}
Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost);
}
packetsnotlost++;
recvpacketcount = temp[2];
//MAVLINK_MSG_ID_GPS_STATUS
@ -1672,6 +1783,8 @@ namespace ArdupilotMega
lastlogread = date1;
MainV2.cs.datetime = lastlogread;
int length = 5;
int a = 0;
while (a < length)
@ -1751,6 +1864,34 @@ namespace ArdupilotMega
}
public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)
{
if (bytearray[0] == 'U')
{
ByteArrayToStructureEndian(bytearray, ref obj, startoffset);
}
else
{
int len = Marshal.SizeOf(obj);
IntPtr i = Marshal.AllocHGlobal(len);
// create structure from ptr
obj = Marshal.PtrToStructure(i, obj.GetType());
try
{
// copy byte array to ptr
Marshal.Copy(bytearray, startoffset, i, len);
}
catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); }
obj = Marshal.PtrToStructure(i, obj.GetType());
Marshal.FreeHGlobal(i);
}
}
public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset)
{
int len = Marshal.SizeOf(obj);
@ -1779,51 +1920,12 @@ namespace ArdupilotMega
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
if (typeCode != TypeCode.Object)
{/*
switch (Marshal.SizeOf(fieldValue))
{
case 1:
Marshal.WriteByte(i, reversestartoffset - 6, bytearray[reversestartoffset]);
break;
case 2:
byte[] temp = new byte[2];
temp[0] = bytearray[reversestartoffset + 1];
temp[1] = bytearray[reversestartoffset + 0];
Marshal.WriteInt16(i, reversestartoffset - 6, BitConverter.ToInt16(temp, 0));
break;
case 4:
byte[] temp2 = new byte[4];
temp2[0] = bytearray[reversestartoffset + 3];
temp2[1] = bytearray[reversestartoffset + 2];
temp2[2] = bytearray[reversestartoffset + 1];
temp2[3] = bytearray[reversestartoffset + 0];
Marshal.WriteInt32(i, reversestartoffset - 6, BitConverter.ToInt32(temp2, 0));
break;
case 8:
byte[] temp3 = new byte[8];
temp3[0] = bytearray[reversestartoffset + 7];
temp3[1] = bytearray[reversestartoffset + 6];
temp3[2] = bytearray[reversestartoffset + 5];
temp3[3] = bytearray[reversestartoffset + 4];
temp3[4] = bytearray[reversestartoffset + 3];
temp3[5] = bytearray[reversestartoffset + 2];
temp3[6] = bytearray[reversestartoffset + 1];
temp3[7] = bytearray[reversestartoffset + 0];
Marshal.WriteInt64(i, reversestartoffset - 6, BitConverter.ToInt64(temp3, 0));
break;
default:
Console.WriteLine("bytearraytostruct Bad value");
break;
} */
{
Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue));
reversestartoffset += Marshal.SizeOf(fieldValue);
}
else
{
/*
for (int c = 0; c < ((byte[])fieldValue).Length;c++)
Marshal.WriteByte(i, c, bytearray[reversestartoffset + c]);
*/
reversestartoffset += ((byte[])fieldValue).Length;
}

1918
Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs
View File

File diff suppressed because it is too large Load Diff

152
Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs
View File

@ -7,159 +7,7 @@ namespace ArdupilotMega
{
partial class MAVLink
{
public enum MAV_CLASS
{
MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything
MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch
MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu
MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com
MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org
MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints
MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands
MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set
MAV_CLASS_NONE = 8, ///< No valid autopilot
MAV_CLASS_NB ///< Number of autopilot classes
};
public enum MAV_ACTION
{
MAV_ACTION_HOLD = 0,
MAV_ACTION_MOTORS_START = 1,
MAV_ACTION_LAUNCH = 2,
MAV_ACTION_RETURN = 3,
MAV_ACTION_EMCY_LAND = 4,
MAV_ACTION_EMCY_KILL = 5,
MAV_ACTION_CONFIRM_KILL = 6,
MAV_ACTION_CONTINUE = 7,
MAV_ACTION_MOTORS_STOP = 8,
MAV_ACTION_HALT = 9,
MAV_ACTION_SHUTDOWN = 10,
MAV_ACTION_REBOOT = 11,
MAV_ACTION_SET_MANUAL = 12,
MAV_ACTION_SET_AUTO = 13,
MAV_ACTION_STORAGE_READ = 14,
MAV_ACTION_STORAGE_WRITE = 15,
MAV_ACTION_CALIBRATE_RC = 16,
MAV_ACTION_CALIBRATE_GYRO = 17,
MAV_ACTION_CALIBRATE_MAG = 18,
MAV_ACTION_CALIBRATE_ACC = 19,
MAV_ACTION_CALIBRATE_PRESSURE = 20,
MAV_ACTION_REC_START = 21,
MAV_ACTION_REC_PAUSE = 22,
MAV_ACTION_REC_STOP = 23,
MAV_ACTION_TAKEOFF = 24,
MAV_ACTION_NAVIGATE = 25,
MAV_ACTION_LAND = 26,
MAV_ACTION_LOITER = 27,
MAV_ACTION_SET_ORIGIN = 28,
MAV_ACTION_RELAY_ON = 29,
MAV_ACTION_RELAY_OFF = 30,
MAV_ACTION_GET_IMAGE = 31,
MAV_ACTION_VIDEO_START = 32,
MAV_ACTION_VIDEO_STOP = 33,
MAV_ACTION_RESET_MAP = 34,
MAV_ACTION_RESET_PLAN = 35,
MAV_ACTION_DELAY_BEFORE_COMMAND = 36,
MAV_ACTION_ASCEND_AT_RATE = 37,
MAV_ACTION_CHANGE_MODE = 38,
MAV_ACTION_LOITER_MAX_TURNS = 39,
MAV_ACTION_LOITER_MAX_TIME = 40,
MAV_ACTION_START_HILSIM = 41,
MAV_ACTION_STOP_HILSIM = 42,
MAV_ACTION_NB ///< Number of MAV actions
};
public enum MAV_MODE
{
MAV_MODE_UNINIT = 0, ///< System is in undefined state
MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe
MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control
MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint
MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation
MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use
MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use
MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use
MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive
MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
};
public enum MAV_STATE
{
MAV_STATE_UNINIT = 0,
MAV_STATE_BOOT,
MAV_STATE_CALIBRATING,
MAV_STATE_STANDBY,
MAV_STATE_ACTIVE,
MAV_STATE_CRITICAL,
MAV_STATE_EMERGENCY,
MAV_STATE_HILSIM,
MAV_STATE_POWEROFF
};
public enum MAV_NAV
{
MAV_NAV_GROUNDED = 0,
MAV_NAV_LIFTOFF,
MAV_NAV_HOLD,
MAV_NAV_WAYPOINT,
MAV_NAV_VECTOR,
MAV_NAV_RETURNING,
MAV_NAV_LANDING,
MAV_NAV_LOST,
MAV_NAV_LOITER,
MAV_NAV_FREE_DRIFT
};
public enum MAV_TYPE
{
MAV_GENERIC = 0,
MAV_FIXED_WING = 1,
MAV_QUADROTOR = 2,
MAV_COAXIAL = 3,
MAV_HELICOPTER = 4,
MAV_GROUND = 5,
OCU = 6,
MAV_AIRSHIP = 7,
MAV_FREE_BALLOON = 8,
MAV_ROCKET = 9,
UGV_GROUND_ROVER = 10,
UGV_SURFACE_SHIP = 11
};
public enum MAV_AUTOPILOT_TYPE
{
MAV_AUTOPILOT_GENERIC = 0,
MAV_AUTOPILOT_PIXHAWK = 1,
MAV_AUTOPILOT_SLUGS = 2,
MAV_AUTOPILOT_ARDUPILOTMEGA = 3,
MAV_AUTOPILOT_NONE = 4
};
public enum MAV_COMPONENT
{
MAV_COMP_ID_GPS,
MAV_COMP_ID_WAYPOINTPLANNER,
MAV_COMP_ID_BLOBTRACKER,
MAV_COMP_ID_PATHPLANNER,
MAV_COMP_ID_AIRSLAM,
MAV_COMP_ID_MAPPER,
MAV_COMP_ID_CAMERA,
MAV_COMP_ID_IMU = 200,
MAV_COMP_ID_IMU_2 = 201,
MAV_COMP_ID_IMU_3 = 202,
MAV_COMP_ID_UDP_BRIDGE = 240,
MAV_COMP_ID_UART_BRIDGE = 241,
MAV_COMP_ID_SYSTEM_CONTROL = 250
};
public enum MAV_FRAME
{
MAV_FRAME_GLOBAL = 0,
MAV_FRAME_LOCAL = 1,
MAV_FRAME_MISSION = 2,
MAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
MAV_FRAME_LOCAL_ENU = 4
};
}
}

50
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -76,6 +76,14 @@ namespace ArdupilotMega
//System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US");
//System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US");
srtm.datadirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "srtm";
georefimage temp = new georefimage();
//temp.dowork(141);
//return;
var t = Type.GetType("Mono.Runtime");
MAC = (t != null);
@ -153,6 +161,15 @@ namespace ArdupilotMega
}
catch (Exception e) { MessageBox.Show("A Major error has occured : " + e.ToString()); this.Close(); }
GCSViews.FlightData.myhud.Refresh();
GCSViews.FlightData.myhud.Refresh();
GCSViews.FlightData.myhud.Refresh();
if (GCSViews.FlightData.myhud.huddrawtime > 1000)
{
MessageBox.Show("The HUD draw time is above 1 seconds. Please update your graphics card driver.");
}
changeunits();
try
@ -867,6 +884,8 @@ namespace ArdupilotMega
DateTime speechcustomtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialthread)
{
try
@ -919,6 +938,21 @@ namespace ArdupilotMega
speechcustomtime = DateTime.Now;
}
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
MainV2.cs.linkqualitygcs = 0;
}
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds >= 1)
{
GCSViews.FlightData.myhud.Invalidate();
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainV2.cs.linkqualitygcs = (ushort)(MainV2.cs.linkqualitygcs * 0.8f);
linkqualitytime = DateTime.Now;
}
}
if (speechenable && talk != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
@ -939,7 +973,7 @@ namespace ArdupilotMega
if (heatbeatsend.Second != DateTime.Now.Second)
{
Console.WriteLine("remote lost {0}", cs.packetdrop);
Console.WriteLine("remote lost {0}", cs.packetdropremote);
MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t();
@ -952,10 +986,12 @@ namespace ArdupilotMega
}
// data loss warning
if (speechenable && talk != null && (DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
if (MainV2.talk.State == SynthesizerState.Ready)
MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
if (speechenable && talk != null) {
if (MainV2.talk.State == SynthesizerState.Ready)
MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
}
}
//Console.WriteLine(comPort.BaseStream.BytesToRead);
@ -1459,6 +1495,12 @@ namespace ArdupilotMega
MainV2.comPort.Open(false);
return true;
}
if (keyData == (Keys.Control | Keys.Y)) // for ryan beall
{
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE");
return true;
}
if (keyData == (Keys.Control | Keys.J)) // for jani
{
string data = "!!";

5
Tools/ArdupilotMegaPlanner/MavlinkLog.cs
View File

@ -431,13 +431,14 @@ namespace ArdupilotMega
// bar moves to 100 % in this step
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
Application.DoEvents();
progressBar1.Refresh();
//Application.DoEvents();
byte[] packet = mine.readPacket();
string text = "";
mine.DebugPacket(packet, ref text);
sw.Write(text);
sw.Write(mine.lastlogread +" "+text);
}
sw.Close();

2
Tools/ArdupilotMegaPlanner/MyButton.cs
View File

@ -50,7 +50,7 @@ namespace ArdupilotMega
if (!this.Enabled)
{
SolidBrush brush = new SolidBrush(Color.FromArgb(200, 0x2b, 0x3a, 0x03));
SolidBrush brush = new SolidBrush(Color.FromArgb(150, 0x2b, 0x3a, 0x03));
gr.FillRectangle(brush, 0, 0, this.Width, this.Height);
}

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.0.67")]
[assembly: AssemblyFileVersion("1.0.70")]
[assembly: NeutralResourcesLanguageAttribute("")]

14
Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt
View File

@ -1,4 +1,4 @@
== MAVLink Parameters ==
== MAVLink Parameters == (this is a copy fo the wiki page FYI)
This is a list of all the user-modifiable MAVLink parameters and what they do. You can modify them via the MAVLink parameters window in any compatible GCS, such as the Mission Planner, HK GCS or !QGroundControl.
@ -213,9 +213,9 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some
||SYSID_SW_MREV|| || ||0|| || ||Description coming soon||
||SYSID_SW_TYPE|| || ||0|| || ||Description coming soon||
||THR_SLEWRATE||0||100||0||1||1||THROTTLE_SLEW_RATE - 0 = Disabled, otherwise it limits throttle movement rate. Units are % per second. This is a test feature and may go away.||
||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL||
||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||

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

@ -32,22 +32,11 @@
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup));
this.tabControl1 = new System.Windows.Forms.TabControl();
this.tabReset = new System.Windows.Forms.TabPage();
this.BUT_reset = new ArdupilotMega.MyButton();
this.tabRadioIn = new System.Windows.Forms.TabPage();
this.CHK_revch3 = new System.Windows.Forms.CheckBox();
this.CHK_revch4 = new System.Windows.Forms.CheckBox();
this.CHK_revch2 = new System.Windows.Forms.CheckBox();
this.CHK_revch1 = new System.Windows.Forms.CheckBox();
this.BUT_Calibrateradio = new ArdupilotMega.MyButton();
this.BAR8 = new ArdupilotMega.HorizontalProgressBar2();
this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components);
this.BAR7 = new ArdupilotMega.HorizontalProgressBar2();
this.BAR6 = new ArdupilotMega.HorizontalProgressBar2();
this.BAR5 = new ArdupilotMega.HorizontalProgressBar2();
this.BARpitch = new ArdupilotMega.VerticalProgressBar2();
this.BARthrottle = new ArdupilotMega.VerticalProgressBar2();
this.BARyaw = new ArdupilotMega.HorizontalProgressBar2();
this.BARroll = new ArdupilotMega.HorizontalProgressBar2();
this.tabModes = new System.Windows.Forms.TabPage();
this.label14 = new System.Windows.Forms.Label();
this.LBL_flightmodepwm = new System.Windows.Forms.Label();
@ -71,7 +60,6 @@
this.CMB_fmode2 = new System.Windows.Forms.ComboBox();
this.label1 = new System.Windows.Forms.Label();
this.CMB_fmode1 = new System.Windows.Forms.ComboBox();
this.BUT_SaveModes = new ArdupilotMega.MyButton();
this.tabHardware = new System.Windows.Forms.TabPage();
this.linkLabelmagdec = new System.Windows.Forms.LinkLabel();
this.label106 = new System.Windows.Forms.Label();
@ -90,7 +78,6 @@
this.pictureBox1 = new System.Windows.Forms.PictureBox();
this.tabArducopter = new System.Windows.Forms.TabPage();
this.label28 = new System.Windows.Forms.Label();
this.BUT_levelac2 = new ArdupilotMega.MyButton();
this.label16 = new System.Windows.Forms.Label();
this.label15 = new System.Windows.Forms.Label();
this.pictureBoxQuadX = new System.Windows.Forms.PictureBox();
@ -119,6 +106,20 @@
this.HS2_REV = new System.Windows.Forms.CheckBox();
this.HS1_REV = new System.Windows.Forms.CheckBox();
this.label17 = new System.Windows.Forms.Label();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_reset = new ArdupilotMega.MyButton();
this.BUT_Calibrateradio = new ArdupilotMega.MyButton();
this.BAR8 = new ArdupilotMega.HorizontalProgressBar2();
this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components);
this.BAR7 = new ArdupilotMega.HorizontalProgressBar2();
this.BAR6 = new ArdupilotMega.HorizontalProgressBar2();
this.BAR5 = new ArdupilotMega.HorizontalProgressBar2();
this.BARpitch = new ArdupilotMega.VerticalProgressBar2();
this.BARthrottle = new ArdupilotMega.VerticalProgressBar2();
this.BARyaw = new ArdupilotMega.HorizontalProgressBar2();
this.BARroll = new ArdupilotMega.HorizontalProgressBar2();
this.BUT_SaveModes = new ArdupilotMega.MyButton();
this.BUT_levelac2 = new ArdupilotMega.MyButton();
this.BUT_saveheliconfig = new ArdupilotMega.MyButton();
this.BUT_0collective = new ArdupilotMega.MyButton();
this.HS4 = new ArdupilotMega.VerticalProgressBar2();
@ -128,11 +129,15 @@
this.HS2_TRIM = new ArdupilotMega.MyTrackBar();
this.HS1_TRIM = new ArdupilotMega.MyTrackBar();
this.Gservoloc = new AGaugeApp.AGauge();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.CB_simple1 = new System.Windows.Forms.CheckBox();
this.CB_simple2 = new System.Windows.Forms.CheckBox();
this.CB_simple3 = new System.Windows.Forms.CheckBox();
this.CB_simple4 = new System.Windows.Forms.CheckBox();
this.CB_simple5 = new System.Windows.Forms.CheckBox();
this.CB_simple6 = new System.Windows.Forms.CheckBox();
this.tabControl1.SuspendLayout();
this.tabReset.SuspendLayout();
this.tabRadioIn.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
this.tabModes.SuspendLayout();
this.tabHardware.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit();
@ -143,6 +148,7 @@
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit();
this.tabHeli.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit();
((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit();
@ -169,14 +175,6 @@
this.tabReset.Name = "tabReset";
this.tabReset.UseVisualStyleBackColor = true;
//
// BUT_reset
//
resources.ApplyResources(this.BUT_reset, "BUT_reset");
this.BUT_reset.Name = "BUT_reset";
this.BUT_reset.Tag = "";
this.BUT_reset.UseVisualStyleBackColor = true;
this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click);
//
// tabRadioIn
//
this.tabRadioIn.Controls.Add(this.CHK_revch3);
@ -224,139 +222,14 @@
this.CHK_revch1.UseVisualStyleBackColor = true;
this.CHK_revch1.CheckedChanged += new System.EventHandler(this.CHK_revch1_CheckedChanged);
//
// BUT_Calibrateradio
//
resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio");
this.BUT_Calibrateradio.Name = "BUT_Calibrateradio";
this.BUT_Calibrateradio.UseVisualStyleBackColor = true;
this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click);
//
// BAR8
//
this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true));
this.BAR8.Label = "Radio 8";
resources.ApplyResources(this.BAR8, "BAR8");
this.BAR8.Maximum = 2200;
this.BAR8.maxline = 0;
this.BAR8.Minimum = 800;
this.BAR8.minline = 0;
this.BAR8.Name = "BAR8";
this.BAR8.Value = 1500;
this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// currentStateBindingSource
//
this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState);
//
// BAR7
//
this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true));
this.BAR7.Label = "Radio 7";
resources.ApplyResources(this.BAR7, "BAR7");
this.BAR7.Maximum = 2200;
this.BAR7.maxline = 0;
this.BAR7.Minimum = 800;
this.BAR7.minline = 0;
this.BAR7.Name = "BAR7";
this.BAR7.Value = 1500;
this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BAR6
//
this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true));
this.BAR6.Label = "Radio 6";
resources.ApplyResources(this.BAR6, "BAR6");
this.BAR6.Maximum = 2200;
this.BAR6.maxline = 0;
this.BAR6.Minimum = 800;
this.BAR6.minline = 0;
this.BAR6.Name = "BAR6";
this.BAR6.Value = 1500;
this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BAR5
//
this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true));
this.BAR5.Label = "Radio 5";
resources.ApplyResources(this.BAR5, "BAR5");
this.BAR5.Maximum = 2200;
this.BAR5.maxline = 0;
this.BAR5.Minimum = 800;
this.BAR5.minline = 0;
this.BAR5.Name = "BAR5";
this.BAR5.Value = 1500;
this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARpitch
//
this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true));
this.BARpitch.Label = "Pitch";
resources.ApplyResources(this.BARpitch, "BARpitch");
this.BARpitch.Maximum = 2200;
this.BARpitch.maxline = 0;
this.BARpitch.Minimum = 800;
this.BARpitch.minline = 0;
this.BARpitch.Name = "BARpitch";
this.BARpitch.Value = 1500;
this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARthrottle
//
this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69)))));
this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true));
this.BARthrottle.Label = "Throttle";
resources.ApplyResources(this.BARthrottle, "BARthrottle");
this.BARthrottle.Maximum = 2200;
this.BARthrottle.maxline = 0;
this.BARthrottle.Minimum = 800;
this.BARthrottle.minline = 0;
this.BARthrottle.Name = "BARthrottle";
this.BARthrottle.Value = 1000;
this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31)))));
//
// BARyaw
//
this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true));
this.BARyaw.Label = "Yaw";
resources.ApplyResources(this.BARyaw, "BARyaw");
this.BARyaw.Maximum = 2200;
this.BARyaw.maxline = 0;
this.BARyaw.Minimum = 800;
this.BARyaw.minline = 0;
this.BARyaw.Name = "BARyaw";
this.BARyaw.Value = 1500;
this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARroll
//
this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true));
this.BARroll.Label = "Roll";
resources.ApplyResources(this.BARroll, "BARroll");
this.BARroll.Maximum = 2200;
this.BARroll.maxline = 0;
this.BARroll.Minimum = 800;
this.BARroll.minline = 0;
this.BARroll.Name = "BARroll";
this.BARroll.Value = 1500;
this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// tabModes
//
this.tabModes.Controls.Add(this.CB_simple6);
this.tabModes.Controls.Add(this.CB_simple5);
this.tabModes.Controls.Add(this.CB_simple4);
this.tabModes.Controls.Add(this.CB_simple3);
this.tabModes.Controls.Add(this.CB_simple2);
this.tabModes.Controls.Add(this.CB_simple1);
this.tabModes.Controls.Add(this.label14);
this.tabModes.Controls.Add(this.LBL_flightmodepwm);
this.tabModes.Controls.Add(this.label13);
@ -519,13 +392,6 @@
resources.ApplyResources(this.CMB_fmode1, "CMB_fmode1");
this.CMB_fmode1.Name = "CMB_fmode1";
//
// BUT_SaveModes
//
resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes");
this.BUT_SaveModes.Name = "BUT_SaveModes";
this.BUT_SaveModes.UseVisualStyleBackColor = true;
this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click);
//
// tabHardware
//
this.tabHardware.BackColor = System.Drawing.Color.DarkRed;
@ -661,11 +527,11 @@
// tabArducopter
//
this.tabArducopter.Controls.Add(this.label28);
this.tabArducopter.Controls.Add(this.BUT_levelac2);
this.tabArducopter.Controls.Add(this.label16);
this.tabArducopter.Controls.Add(this.label15);
this.tabArducopter.Controls.Add(this.pictureBoxQuadX);
this.tabArducopter.Controls.Add(this.pictureBoxQuad);
this.tabArducopter.Controls.Add(this.BUT_levelac2);
resources.ApplyResources(this.tabArducopter, "tabArducopter");
this.tabArducopter.Name = "tabArducopter";
this.tabArducopter.UseVisualStyleBackColor = true;
@ -675,13 +541,6 @@
resources.ApplyResources(this.label28, "label28");
this.label28.Name = "label28";
//
// BUT_levelac2
//
resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2");
this.BUT_levelac2.Name = "BUT_levelac2";
this.BUT_levelac2.UseVisualStyleBackColor = true;
this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click);
//
// label16
//
resources.ApplyResources(this.label16, "label16");
@ -879,6 +738,159 @@
resources.ApplyResources(this.label17, "label17");
this.label17.Name = "label17";
//
// BUT_reset
//
resources.ApplyResources(this.BUT_reset, "BUT_reset");
this.BUT_reset.Name = "BUT_reset";
this.BUT_reset.Tag = "";
this.BUT_reset.UseVisualStyleBackColor = true;
this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click);
//
// BUT_Calibrateradio
//
resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio");
this.BUT_Calibrateradio.Name = "BUT_Calibrateradio";
this.BUT_Calibrateradio.UseVisualStyleBackColor = true;
this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click);
//
// BAR8
//
this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true));
this.BAR8.Label = "Radio 8";
resources.ApplyResources(this.BAR8, "BAR8");
this.BAR8.Maximum = 2200;
this.BAR8.maxline = 0;
this.BAR8.Minimum = 800;
this.BAR8.minline = 0;
this.BAR8.Name = "BAR8";
this.BAR8.Value = 1500;
this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// currentStateBindingSource
//
this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState);
//
// BAR7
//
this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true));
this.BAR7.Label = "Radio 7";
resources.ApplyResources(this.BAR7, "BAR7");
this.BAR7.Maximum = 2200;
this.BAR7.maxline = 0;
this.BAR7.Minimum = 800;
this.BAR7.minline = 0;
this.BAR7.Name = "BAR7";
this.BAR7.Value = 1500;
this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BAR6
//
this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true));
this.BAR6.Label = "Radio 6";
resources.ApplyResources(this.BAR6, "BAR6");
this.BAR6.Maximum = 2200;
this.BAR6.maxline = 0;
this.BAR6.Minimum = 800;
this.BAR6.minline = 0;
this.BAR6.Name = "BAR6";
this.BAR6.Value = 1500;
this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BAR5
//
this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true));
this.BAR5.Label = "Radio 5";
resources.ApplyResources(this.BAR5, "BAR5");
this.BAR5.Maximum = 2200;
this.BAR5.maxline = 0;
this.BAR5.Minimum = 800;
this.BAR5.minline = 0;
this.BAR5.Name = "BAR5";
this.BAR5.Value = 1500;
this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARpitch
//
this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true));
this.BARpitch.Label = "Pitch";
resources.ApplyResources(this.BARpitch, "BARpitch");
this.BARpitch.Maximum = 2200;
this.BARpitch.maxline = 0;
this.BARpitch.Minimum = 800;
this.BARpitch.minline = 0;
this.BARpitch.Name = "BARpitch";
this.BARpitch.Value = 1500;
this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARthrottle
//
this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69)))));
this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true));
this.BARthrottle.Label = "Throttle";
resources.ApplyResources(this.BARthrottle, "BARthrottle");
this.BARthrottle.Maximum = 2200;
this.BARthrottle.maxline = 0;
this.BARthrottle.Minimum = 800;
this.BARthrottle.minline = 0;
this.BARthrottle.Name = "BARthrottle";
this.BARthrottle.Value = 1000;
this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31)))));
//
// BARyaw
//
this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true));
this.BARyaw.Label = "Yaw";
resources.ApplyResources(this.BARyaw, "BARyaw");
this.BARyaw.Maximum = 2200;
this.BARyaw.maxline = 0;
this.BARyaw.Minimum = 800;
this.BARyaw.minline = 0;
this.BARyaw.Name = "BARyaw";
this.BARyaw.Value = 1500;
this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BARroll
//
this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255)))));
this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder;
this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true));
this.BARroll.Label = "Roll";
resources.ApplyResources(this.BARroll, "BARroll");
this.BARroll.Maximum = 2200;
this.BARroll.maxline = 0;
this.BARroll.Minimum = 800;
this.BARroll.minline = 0;
this.BARroll.Name = "BARroll";
this.BARroll.Value = 1500;
this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255)))));
//
// BUT_SaveModes
//
resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes");
this.BUT_SaveModes.Name = "BUT_SaveModes";
this.BUT_SaveModes.UseVisualStyleBackColor = true;
this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click);
//
// BUT_levelac2
//
resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2");
this.BUT_levelac2.Name = "BUT_levelac2";
this.BUT_levelac2.UseVisualStyleBackColor = true;
this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click);
//
// BUT_saveheliconfig
//
resources.ApplyResources(this.BUT_saveheliconfig, "BUT_saveheliconfig");
@ -925,8 +937,8 @@
//
// HS4_TRIM
//
this.HS4_TRIM.LargeChange = 1000;
resources.ApplyResources(this.HS4_TRIM, "HS4_TRIM");
this.HS4_TRIM.LargeChange = 1000;
this.HS4_TRIM.Maximum = 2000D;
this.HS4_TRIM.Minimum = 1000D;
this.HS4_TRIM.Name = "HS4_TRIM";
@ -937,8 +949,8 @@
//
// HS3_TRIM
//
this.HS3_TRIM.LargeChange = 1000;
resources.ApplyResources(this.HS3_TRIM, "HS3_TRIM");
this.HS3_TRIM.LargeChange = 1000;
this.HS3_TRIM.Maximum = 2000D;
this.HS3_TRIM.Minimum = 1000D;
this.HS3_TRIM.Name = "HS3_TRIM";
@ -949,8 +961,8 @@
//
// HS2_TRIM
//
this.HS2_TRIM.LargeChange = 1000;
resources.ApplyResources(this.HS2_TRIM, "HS2_TRIM");
this.HS2_TRIM.LargeChange = 1000;
this.HS2_TRIM.Maximum = 2000D;
this.HS2_TRIM.Minimum = 1000D;
this.HS2_TRIM.Name = "HS2_TRIM";
@ -961,8 +973,8 @@
//
// HS1_TRIM
//
this.HS1_TRIM.LargeChange = 1000;
resources.ApplyResources(this.HS1_TRIM, "HS1_TRIM");
this.HS1_TRIM.LargeChange = 1000;
this.HS1_TRIM.Maximum = 2000D;
this.HS1_TRIM.Minimum = 1000D;
this.HS1_TRIM.Name = "HS1_TRIM";
@ -1114,6 +1126,42 @@
this.Gservoloc.Value2 = 180F;
this.Gservoloc.Value3 = 0F;
//
// CB_simple1
//
resources.ApplyResources(this.CB_simple1, "CB_simple1");
this.CB_simple1.Name = "CB_simple1";
this.CB_simple1.UseVisualStyleBackColor = true;
//
// CB_simple2
//
resources.ApplyResources(this.CB_simple2, "CB_simple2");
this.CB_simple2.Name = "CB_simple2";
this.CB_simple2.UseVisualStyleBackColor = true;
//
// CB_simple3
//
resources.ApplyResources(this.CB_simple3, "CB_simple3");
this.CB_simple3.Name = "CB_simple3";
this.CB_simple3.UseVisualStyleBackColor = true;
//
// CB_simple4
//
resources.ApplyResources(this.CB_simple4, "CB_simple4");
this.CB_simple4.Name = "CB_simple4";
this.CB_simple4.UseVisualStyleBackColor = true;
//
// CB_simple5
//
resources.ApplyResources(this.CB_simple5, "CB_simple5");
this.CB_simple5.Name = "CB_simple5";
this.CB_simple5.UseVisualStyleBackColor = true;
//
// CB_simple6
//
resources.ApplyResources(this.CB_simple6, "CB_simple6");
this.CB_simple6.Name = "CB_simple6";
this.CB_simple6.UseVisualStyleBackColor = true;
//
// Setup
//
resources.ApplyResources(this, "$this");
@ -1126,7 +1174,6 @@
this.tabReset.ResumeLayout(false);
this.tabRadioIn.ResumeLayout(false);
this.tabRadioIn.PerformLayout();
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
this.tabModes.ResumeLayout(false);
this.tabModes.PerformLayout();
this.tabHardware.ResumeLayout(false);
@ -1141,6 +1188,7 @@
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit();
this.tabHeli.ResumeLayout(false);
this.tabHeli.PerformLayout();
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit();
((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).EndInit();
@ -1250,6 +1298,12 @@
private System.Windows.Forms.CheckBox CHK_revch4;
private System.Windows.Forms.CheckBox CHK_revch2;
private System.Windows.Forms.CheckBox CHK_revch1;
private System.Windows.Forms.CheckBox CB_simple6;
private System.Windows.Forms.CheckBox CB_simple5;
private System.Windows.Forms.CheckBox CB_simple4;
private System.Windows.Forms.CheckBox CB_simple3;
private System.Windows.Forms.CheckBox CB_simple2;
private System.Windows.Forms.CheckBox CB_simple1;
}
}

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

@ -231,7 +231,6 @@ namespace ArdupilotMega.Setup
MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc);
MainV2.comPort.param = MainV2.comPort.getParamList();
if (Configuration != null)
{
Configuration.startup = true;
@ -253,6 +252,13 @@ namespace ArdupilotMega.Setup
{
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) // APM
{
CB_simple1.Visible = false;
CB_simple2.Visible = false;
CB_simple3.Visible = false;
CB_simple4.Visible = false;
CB_simple5.Visible = false;
CB_simple6.Visible = false;
CMB_fmode1.Items.Clear();
CMB_fmode2.Items.Clear();
CMB_fmode3.Items.Clear();
@ -411,6 +417,10 @@ namespace ArdupilotMega.Setup
MainV2.comPort.setParam("FLTMODE4", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode4.Text));
MainV2.comPort.setParam("FLTMODE5", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode5.Text));
MainV2.comPort.setParam("FLTMODE6", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode6.Text));
float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 <<2 : 0)
+ (CB_simple4.Checked ? 1 << 3 : 0) + (CB_simple5.Checked ? 1 << 4 : 0) + (CB_simple6.Checked ? 1 << 5 : 0);
MainV2.comPort.setParam("SIMPLE", value);
}
}
catch { MessageBox.Show("Failed to set Flight modes"); }

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

File diff suppressed because it is too large Load Diff

10694
Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx
View File

File diff suppressed because it is too large Load Diff

12
Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx
View File

@ -343,16 +343,16 @@
<value>NoControl</value>
</data>
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
<value>101, 165</value>
<value>113, 167</value>
</data>
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
<value>79, 13</value>
<value>56, 13</value>
</data>
<data name="label1.TabIndex" type="System.Int32, mscorlib">
<value>8</value>
</data>
<data name="label1.Text" xml:space="preserve">
<value>ArduPilot Mega</value>
<value>ArduPlane</value>
</data>
<data name="&gt;&gt;label1.Name" xml:space="preserve">
<value>label1</value>
@ -403,16 +403,16 @@
<value>NoControl</value>
</data>
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
<value>57, 362</value>
<value>74, 361</value>
</data>
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
<value>168, 13</value>
<value>142, 13</value>
</data>
<data name="label3.TabIndex" type="System.Int32, mscorlib">
<value>10</value>
</data>
<data name="label3.Text" xml:space="preserve">
<value>ArduPilot Mega (Xplane simulator)</value>
<value>ArduPlane (Xplane simulator)</value>
</data>
<data name="&gt;&gt;label3.Name" xml:space="preserve">
<value>label3</value>

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

@ -22,9 +22,8 @@
<F1>WP Dist</F1>
<F2>WP Verify</F2>
<F3>Target Bear</F3>
<F4>Nav Bear</F4>
<F5>Long Err</F5>
<F6>Lat Err</F6>
<F4>Long Err</F4>
<F5>Lat Err</F5>
</NTUN>
<CTUN>
<F1>Yaw Sensor</F1>
@ -56,6 +55,13 @@
<F5>Accel Y</F5>
<F6>Accel Z</F6>
</RAW>
<CURR>
<F1>Throttle in</F1>
<F2>Throttle intergrator</F2>
<F3>Voltage</F3>
<F4>Current</F4>
<F5>Current total</F5>
</CURR>
</AC2>
<!-- -->
<APM>

12
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -22,9 +22,8 @@
<F1>WP Dist</F1>
<F2>WP Verify</F2>
<F3>Target Bear</F3>
<F4>Nav Bear</F4>
<F5>Long Err</F5>
<F6>Lat Err</F6>
<F4>Long Err</F4>
<F5>Lat Err</F5>
</NTUN>
<CTUN>
<F1>Yaw Sensor</F1>
@ -56,6 +55,13 @@
<F5>Accel Y</F5>
<F6>Accel Z</F6>
</RAW>
<CURR>
<F1>Throttle in</F1>
<F2>Throttle intergrator</F2>
<F3>Voltage</F3>
<F4>Current</F4>
<F5>Current total</F5>
</CURR>
</AC2>
<!-- -->
<APM>

127
Tools/ArdupilotMegaPlanner/georefimage.cs Normal file
View File

@ -0,0 +1,127 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
namespace ArdupilotMega
{
class georefimage
{
public string logFile = "";
public string dirWithImages = "";
DateTime getPhotoTime(string fn)
{
DateTime dtaken = DateTime.MinValue;
try
{
Image myImage = Image.FromFile(fn);
PropertyItem propItem = myImage.GetPropertyItem(36867); // 36867 // 306
//Convert date taken metadata to a DateTime object
string sdate = Encoding.UTF8.GetString(propItem.Value).Trim();
string secondhalf = sdate.Substring(sdate.IndexOf(" "), (sdate.Length - sdate.IndexOf(" ")));
string firsthalf = sdate.Substring(0, 10);
firsthalf = firsthalf.Replace(":", "-");
sdate = firsthalf + secondhalf;
dtaken = DateTime.Parse(sdate);
myImage.Dispose();
}
catch { }
return dtaken;
}
List<string[]> readLog(string fn)
{
List<string[]> list = new List<string[]>();
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
string[] vals = line.Split(new char[] {',',':'});
if (lasttime == vals[1])
continue;
lasttime = vals[1];
list.Add(vals);
}
}
sr.Close();
sr.Dispose();
return list;
}
public void dowork(float offsetseconds)
{
DateTime startTime = DateTime.MinValue;
logFile = @"C:\Users\hog\Pictures\sams mums 22-6-2011\23-06-11 10-03 4.log";
List<string[]> list = readLog(logFile);
dirWithImages = @"C:\Users\hog\Pictures\sams mums 22-6-2011";
string[] files = Directory.GetFiles(dirWithImages);
StreamWriter sw2 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.txt");
StreamWriter sw = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.tel");
sw.WriteLine("version=1");
sw.WriteLine("#longitude and latitude - in degrees");
sw.WriteLine("#name utc longitude latitude height");
foreach (string file in files)
{
if (file.ToLower().EndsWith(".jpg"))
{
DateTime dt = getPhotoTime(file);
if (startTime == DateTime.MinValue)
startTime = new DateTime(dt.Year,dt.Month,dt.Day,0,0,0,0,DateTimeKind.Utc).ToLocalTime();
foreach (string[] arr in list)
{
DateTime crap = startTime.AddMilliseconds(int.Parse(arr[1])).AddSeconds(offsetseconds);
//Console.Write(dt + " " + crap + "\r");
if (dt.Equals(crap))
{
sw2.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6]);
sw.WriteLine(Path.GetFileNameWithoutExtension(file) + "\t" + crap.ToString("yyyy:MM:dd HH:mm:ss") +"\t"+ arr[5] + "\t" + arr[4] + "\t" + arr[6]);
sw.Flush();
sw2.Flush();
Console.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6] + " ");
break;
}
//Console.WriteLine(crap);
}
}
}
sw2.Close();
sw.Close();
}
}
}

1149
Tools/ArdupilotMegaPlanner/hudold.cs Normal file
View File

File diff suppressed because it is too large Load Diff

132
Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl Normal file
View File

@ -0,0 +1,132 @@
$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/message_definitions/";
#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/pixhawk-mavlink-c91adfb/include/common/";
opendir(DIR,$dir) || die print $!;
@files = readdir(DIR);
closedir(DIR);
open(OUT,">MAVLinkTypes.cs");
print OUT <<EOF;
using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;
namespace ArdupilotMega
{
partial class MAVLink
{
EOF
foreach $file (@files) {
if (!($file eq "common.xml" || $file eq "ardupilotmega.xml"))
{
next;
}
print "$file\n";
open(F,$dir.$file);
$start = 0;
while ($line = <F>) {
if ($line =~ /enum name="(MAV_.*)"/) {
$start = 1;
print OUT "\t\tpublic enum $1\n\t\t{ \n";
}
if ($line =~ /<message id="([0-9]+)" name="([^"]+)">/) {
$name = lc($2);
print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n";
print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n";
print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n";
$no = $1;
$start = 1;
#__mavlink_gps_raw_t
$structs[$no] = "__mavlink_".$name."_t";
} # __mavlink_heartbeat_t
$line =~ s/MAV_CMD_NAV_//;
$line =~ s/MAV_CMD_//;
if ($line =~ /<entry value="([0-9]+)" name="([^"]+)">/)
{
print OUT "\t\t\t$2 = $1,\n";
}
#<field type="uint8_t" name="type">
if ($line =~ /<field type="([^"]+)" name="([^"]+)">(.*)<\/field>/)
{
$type = $1;
$name = $2;
$desc = $3;
print "$type = $name\n";
$type =~ s/byte_mavlink_version/public byte/;
$type =~ s/array/public byte/;
$type =~ s/uint8_t/public byte/;
$type =~ s/int8_t/public byte/;
$type =~ s/float/public float/;
$type =~ s/uint16_t/public ushort/;
$type =~ s/uint32_t/public uint/;
$type =~ s/uint64_t/public ulong/;
$type =~ s/int16_t/public short/;
$type =~ s/int32_t/public int/;
$type =~ s/int64_t/public long/;
if ($type =~ /\[(.*)\]/) { # array
print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n";
$type =~ s/\[.*\]//;
$type =~ s/public\s+([^\s]+)/public $1\[\]/o;
}
print OUT "\t\t\t$type $name; ///< $desc\n";
}
if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) {
print OUT "\t\t};\n\n";
$start = 0;
}
}
close(F);
}
print OUT "Type[] mavstructs = new Type[] {";
for ($a = 0; $a <= 256;$a++)
{
if (defined($structs[$a])) {
print OUT "typeof(".$structs[$a] .") ,";
} else {
print OUT "null ,";
}
}
print OUT "};\n\n";
print OUT <<EOF;
}
}
EOF
close OUT;
1;

125
Tools/ArdupilotMegaPlanner/mavlinklist.pl
View File

@ -1,12 +1,27 @@
$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/message_definitions/";
#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/pixhawk-mavlink-c91adfb/include/common/";
$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/";
$dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/";
# mavlink 1.0 with old structs
#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/common/";
#$dir2 = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/";
opendir(DIR,$dir) || die print $!;
@files2 = readdir(DIR);
closedir(DIR);
opendir(DIR,$dir2) || die print $!;
@files = readdir(DIR);
closedir(DIR);
push(@files,@files2);
push(@files,"../mavlink_types.h");
open(OUT,">MAVLinkTypes.cs");
$crcs = 0;
print OUT <<EOF;
using System;
using System.Collections.Generic;
@ -20,88 +35,72 @@ namespace ArdupilotMega
EOF
foreach $file (@files) {
print "$file\n";
if (!($file eq "common.xml" || $file eq "ardupilotmega.xml"))
{
if ($done{$file} == 1) {
next;
}
print "$file\n";
$done{$file} = 1;
open(F,$dir.$file);
open(F,$dir.$file) || open(F,$dir2.$file);
$start = 0;
while ($line = <F>) {
if ($line =~ /enum name="(MAV_.*)"/) {
$start = 1;
print OUT "\t\tpublic enum $1\n\t\t{ \n";
if ($line =~ /(MAVLINK_MESSAGE_LENGTHS|MAVLINK_MESSAGE_CRCS) (.*)/ && $crcs < 2) {
print OUT "\t\tpublic byte[] $1 = new byte[] $2;\n";
$crcs++;
}
if ($line =~ /<message id="([0-9]+)" name="([^"]+)">/) {
$name = lc($2);
if ($line =~ /enum (MAV_.*)/) {
$start = 1;
print OUT "\t\tpublic ";
}
print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n";
if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) {
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
$no = $2;
}
if ($line =~ /typedef struct(.*)/) {
if ($1 =~ /__mavlink_system|param_union/) {
last;
}
$start = 1;
print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n";
print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n";
$no = $1;
$start = 1;
#__mavlink_gps_raw_t
$structs[$no] = "__mavlink_".$name."_t";
} # __mavlink_heartbeat_t
$line =~ s/MAV_CMD_NAV_//;
$line =~ s/MAV_CMD_//;
if ($line =~ /<entry value="([0-9]+)" name="([^"]+)">/)
{
print OUT "\t\t\t$2 = $1,\n";
$structs[$no] = $1;
}
if ($start) {
$line =~ s/MAV_CMD_NAV_//;
#<field type="uint8_t" name="type">
if ($line =~ /<field type="([^"]+)" name="([^"]+)">(.*)<\/field>/)
{
$line =~ s/MAV_CMD_//;
$type = $1;
$name = $2;
$desc = $3;
$line =~ s/typedef/public/;
$line =~ s/uint8_t/public byte/;
$line =~ s/int8_t/public byte/;
$line =~ s/^\s+float/public float/;
$line =~ s/uint16_t/public ushort/;
$line =~ s/uint32_t/public uint/;
$line =~ s/uint64_t/public ulong/;
$line =~ s/int16_t/public short/;
$line =~ s/int32_t/public int/;
$line =~ s/int64_t/public long/;
$line =~ s/typedef/public/;
print "$type = $name\n";
$line =~ s/}.*/};\n/;
$type =~ s/byte_mavlink_version/public byte/;
$type =~ s/array/public byte/;
$type =~ s/uint8_t/public byte/;
$type =~ s/int8_t/public byte/;
$type =~ s/float/public float/;
$type =~ s/uint16_t/public ushort/;
$type =~ s/uint32_t/public uint/;
$type =~ s/uint64_t/public ulong/;
$type =~ s/int16_t/public short/;
$type =~ s/int32_t/public int/;
$type =~ s/int64_t/public long/;
if ($type =~ /\[(.*)\]/) { # array
print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n";
$type =~ s/\[.*\]//;
$type =~ s/public\s+([^\s]+)/public $1\[\]/o;
if ($line =~ /\[(.*)\].*;/) { # array
print OUT "\t\t[MarshalAs(
UnmanagedType.ByValArray,
SizeConst=". $1 .")] \n";
$line =~ s/\[.*\]//;
$line =~ s/public\s+([^\s]+)/public $1\[\]/o;
}
print OUT "\t\t\t$type $name; ///< $desc\n";
print OUT "\t\t".$line;
}
if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) {
print OUT "\t\t};\n\n";
if ($line =~ /}/) {
$start = 0;
}
@ -129,4 +128,6 @@ EOF
close OUT;
<STDIN>;
1;

171
Tools/ArdupilotMegaPlanner/srtm.cs Normal file
View File

@ -0,0 +1,171 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
namespace ArdupilotMega
{
class srtm
{
public static string datadirectory;
public static int getAltitude(double lat, double lng)
{
short alt = -32768;
lat += 0.00083333333333333;
//lng += 0.0008;
int x = (int)Math.Floor(lng);
int y = (int)Math.Floor(lat);
string ns;
if (y > 0)
ns = "N";
else
ns = "S";
string ew;
if (x > 0)
ew = "E";
else
ew = "W";
string filename = ns + Math.Abs(y).ToString("00") + ew + Math.Abs(x).ToString("000") + ".hgt";
string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc";
if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename))
{ // srtm hgt files
FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read);
float posx = 0;
float row = 0;
if (fs.Length <= (1201 * 1201 * 2))
{
posx = (int)(((float)(lng - x)) * (1201 * 2));
row = (int)(((float)(lat - y)) * 1201) * (1201 * 2);
row = (1201 * 1201 * 2) - row;
}
else
{
posx = (int)(((float)(lng - x)) * (3601 * 2));
row = (int)(((float)(lat - y)) * 3601) * (3601 * 2);
row = (3601 * 3601 * 2) - row;
}
if (posx % 2 == 1)
{
posx--;
}
//Console.WriteLine(filename + " row " + row + " posx" + posx);
byte[] data = new byte[2];
fs.Seek((int)(row + posx), SeekOrigin.Begin);
fs.Read(data, 0, data.Length);
fs.Close();
fs.Dispose();
Array.Reverse(data);
alt = BitConverter.ToInt16(data, 0);
return alt;
}
else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2))
{
// this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes
FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read);
StreamReader sr = new StreamReader(fs);
int nox = 0;
int noy = 0;
float left = 0;
float top = 0;
int nodata = -9999;
float cellsize = 0;
int rowcounter = 0;
float wantrow = 0;
float wantcol = 0;
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.StartsWith("ncols"))
{
nox = int.Parse(line.Substring(line.IndexOf(' ')));
//hgtdata = new int[nox * noy];
}
else if (line.StartsWith("nrows"))
{
noy = int.Parse(line.Substring(line.IndexOf(' ')));
//hgtdata = new int[nox * noy];
}
else if (line.StartsWith("xllcorner"))
{
left = float.Parse(line.Substring(line.IndexOf(' ')));
}
else if (line.StartsWith("yllcorner"))
{
top = float.Parse(line.Substring(line.IndexOf(' ')));
}
else if (line.StartsWith("cellsize"))
{
cellsize = float.Parse(line.Substring(line.IndexOf(' ')));
}
else if (line.StartsWith("NODATA_value"))
{
nodata = int.Parse(line.Substring(line.IndexOf(' ')));
}
else
{
string[] data = line.Split(new char[] { ' ' });
if (data.Length == (nox + 1))
{
wantcol = (float)((lng - Math.Round(left,0)));
wantrow = (float)((lat - Math.Round(top,0)));
wantrow =(int)( wantrow / cellsize);
wantcol =(int)( wantcol / cellsize);
wantrow = noy - wantrow;
if (rowcounter == wantrow)
{
Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize));
return int.Parse(data[(int)wantcol]);
}
rowcounter++;
}
}
}
return alt;
}
return alt;
}
}
}

122
Tools/CPUInfo/CPUInfo.pde Normal file
View File

@ -0,0 +1,122 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
test CPU speed
Andrew Tridgell September 2011
*/
#include <math.h>
#include <FastSerial.h>
FastSerialPort0(Serial);
#define SERIAL0_BAUD 115200
void setup() {
Serial.begin(SERIAL0_BAUD, 128, 128);
}
static void show_sizes(void)
{
Serial.println("Type sizes:");
Serial.printf("char : %d\n", sizeof(char));
Serial.printf("short : %d\n", sizeof(short));
Serial.printf("int : %d\n", sizeof(int));
Serial.printf("long : %d\n", sizeof(long));
Serial.printf("long long : %d\n", sizeof(long long));
Serial.printf("bool : %d\n", sizeof(bool));
Serial.printf("void* : %d\n", sizeof(void *));
}
#define TENTIMES(x) do { x; x; x; x; x; x; x; x; x; x; } while (0)
#define FIFTYTIMES(x) do { TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); } while (0)
#define TIMEIT(name, op, count) do { \
uint32_t us_end, us_start; \
us_start = micros(); \
for (uint8_t i=0; i<count; i++) { \
FIFTYTIMES(op); \
} \
us_end = micros(); \
Serial.printf("%-10s %7.2f usec/call\n", name, double(us_end-us_start)/(count*50.0)); \
} while (0)
volatile float v_f = 1.0;
volatile float v_out;
volatile double v_d = 1.0;
volatile double v_out_d;
volatile uint32_t v_32 = 1;
volatile uint32_t v_out_32 = 1;
volatile uint16_t v_16 = 1;
volatile uint16_t v_out_16 = 1;
volatile uint8_t v_8 = 1;
volatile uint8_t v_out_8 = 1;
volatile uint8_t mbuf1[128], mbuf2[128];
static void show_timings(void)
{
v_f = 1+(micros() % 5);
v_out = 1+(micros() % 3);
v_32 = 1+(micros() % 5);
v_out_32 = 1+(micros() % 3);
v_16 = 1+(micros() % 5);
v_out_16 = 1+(micros() % 3);
v_8 = 1+(micros() % 5);
v_out_8 = 1+(micros() % 3);
Serial.println("Operation timings:");
Serial.println("Note: timings for some operations are very data dependent");
TIMEIT("nop", asm volatile("nop"::), 255);
TIMEIT("cli/sei", cli(); sei(), 255);
TIMEIT("micros()", micros(), 200);
TIMEIT("millis()", millis(), 200);
TIMEIT("fadd", v_out += v_f, 100);
TIMEIT("fsub", v_out -= v_f, 100);
TIMEIT("fmul", v_out *= v_f, 100);
TIMEIT("fdiv", v_out /= v_f, 100);
TIMEIT("dadd", v_out_d += v_d, 100);
TIMEIT("dsub", v_out_d -= v_d, 100);
TIMEIT("dmul", v_out_d *= v_d, 100);
TIMEIT("ddiv", v_out_d /= v_d, 100);
TIMEIT("sin()", v_out = sin(v_f), 20);
TIMEIT("cos()", v_out = cos(v_f), 20);
TIMEIT("tan()", v_out = tan(v_f), 20);
TIMEIT("iadd8", v_out_8 += v_8, 100);
TIMEIT("isub8", v_out_8 -= v_8, 100);
TIMEIT("imul8", v_out_8 *= v_8, 100);
TIMEIT("idiv8", v_out_8 /= v_8, 100);
TIMEIT("iadd16", v_out_16 += v_16, 100);
TIMEIT("isub16", v_out_16 -= v_16, 100);
TIMEIT("imul16", v_out_16 *= v_16, 100);
TIMEIT("idiv16", v_out_16 /= v_16, 100);
TIMEIT("iadd32", v_out_32 += v_32, 100);
TIMEIT("isub32", v_out_32 -= v_32, 100);
TIMEIT("imul32", v_out_32 *= v_32, 100);
TIMEIT("idiv32", v_out_32 /= v_32, 100);
TIMEIT("memcpy128", memcpy((void*)mbuf1, (const void *)mbuf2, sizeof(mbuf1)), 20);
TIMEIT("memset128", memset((void*)mbuf1, 1, sizeof(mbuf1)), 20);
TIMEIT("delay(1)", delay(1), 5);
}
void loop()
{
show_sizes();
Serial.println("");
show_timings();
Serial.println("");
delay(3000);
}

4
Tools/CPUInfo/Makefile Normal file
View File

@ -0,0 +1,4 @@
#
# Trivial makefile for building APM
#
include ../../libraries/AP_Common/Arduino.mk

41
Tools/CPUInfo/output.txt Normal file
View File

@ -0,0 +1,41 @@
Type sizes:
char : 1
short : 2
int : 2
long : 4
long long : 8
bool : 1
void* : 2
Operation timings:
Note: timings for some operations are very data dependent
nop 0.07 usec/call
cli/sei 0.14 usec/call
micros() 3.04 usec/call
millis() 1.34 usec/call
fadd 8.68 usec/call
fsub 8.74 usec/call
fmul 7.21 usec/call
fdiv 7.21 usec/call
dadd 8.68 usec/call
dsub 8.74 usec/call
dmul 5.64 usec/call
ddiv 5.57 usec/call
sin() 114.10 usec/call
cos() 103.03 usec/call
tan() 147.92 usec/call
iadd8 0.48 usec/call
isub8 0.48 usec/call
imul8 0.67 usec/call
idiv8 12.93 usec/call
iadd16 0.92 usec/call
isub16 0.92 usec/call
imul16 1.48 usec/call
idiv16 13.24 usec/call
iadd32 1.80 usec/call
isub32 1.80 usec/call
imul32 4.57 usec/call
idiv32 37.71 usec/call
memcpy128 56.76 usec/call
memset128 49.71 usec/call
delay(1) 1005.14 usec/call

44
Tools/scripts/frame_sizes.py Executable file
View File

@ -0,0 +1,44 @@
#!/usr/bin/env python
import re, sys, operator, os
code_line = re.compile("^\s*\d+:/")
frame_line = re.compile("^\s*\d+\s+/\* frame size = (\d+) \*/")
class frame(object):
def __init__(self, code, frame_size):
self.code = code
self.frame_size = int(frame_size)
frames = []
def process_lst(filename):
'''process one lst file'''
last_code = ''
h = open(filename, mode='r')
for line in h:
if code_line.match(line):
last_code = line.strip()
elif frame_line.match(line):
frames.append(frame(last_code, frame_line.match(line).group(1)))
h.close()
if len(sys.argv) > 1:
dname = sys.argv[1]
else:
dname = '.'
for root, dirs, files in os.walk(dname):
for f in files:
if f.endswith(".lst"):
process_lst(os.path.join(root, f))
sorted_frames = sorted(frames,
key=operator.attrgetter('frame_size'),
reverse=True)
print("FrameSize Code")
for frame in sorted_frames:
if frame.frame_size > 0:
print("%9u %s" % (frame.frame_size, frame.code))

BIN
cmake/PdeP.jar Normal file
View File

Binary file not shown.

103
cmake/modules/ArduinoProcessing.cmake Normal file
View File

@ -0,0 +1,103 @@
# 1. Concatenate all PDE files
# 2. Write #include "WProgram.h"
# 3. Write prototypes
# 4. Write original sources
#
#
# Prefix Writer
# 1. Scrub comments
# 2. Optionally subsitute Unicode
# 3. Find imports
# 4. Find prototypes
#
# Find prototypes
# 1. Strip comments, quotes, preprocessor directives
# 2. Collapse braches
# 3. Regex
set(SINGLE_QUOTES_REGEX "('.')")
set(DOUBLE_QUOTES_REGEX "(\"([^\"\\\\]|\\\\.)*\")")
set(SINGLE_COMMENT_REGEX "([ ]*//[^\n]*)")
set(MULTI_COMMENT_REGEX "(/[*][^/]*[*]/)")
set(PREPROC_REGEX "([ ]*#(\\\\[\n]|[^\n])*)")
#"[\w\[\]\*]+\s+[&\[\]\*\w\s]+\([&,\[\]\*\w\s]*\)(?=\s*\{)"
set(PROTOTPYE_REGEX "([a-zA-Z0-9]+[ ]*)*[a-zA-Z0-9]+[ ]*\([^{]*\)[ ]*{")
function(READ_SKETCHES VAR_NAME )
set(SKETCH_SOURCE)
foreach(SKETCH ${ARGN})
if(EXISTS ${SKETCH})
message(STATUS "${SKETCH}")
file(READ ${SKETCH} SKETCH_CONTENTS)
set(SKETCH_SOURCE "${SKETCH_SOURCE}\n${SKETCH_CONTENTS}")
else()
message(FATAL_ERROR "Sketch file does not exist: ${SKETCH}")
endif()
endforeach()
set(${VAR_NAME} "${SKETCH_SOURCE}" PARENT_SCOPE)
endfunction()
function(STRIP_SOURCES VAR_NAME SOURCES)
string(REGEX REPLACE "${SINGLE_QUOTES_REGEX}|${DOUBLE_QUOTES_REGEX}|${SINGLE_COMMENT_REGEX}|${MULTI_COMMENT_REGEX}|${PREPROC_REGEX}"
""
SOURCES
"${SOURCES}")
set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE)
endfunction()
function(COLLAPSE_BRACES VAR_NAME SOURCES)
set(PARSED_SOURCES)
string(LENGTH "${SOURCES}" SOURCES_LENGTH)
math(EXPR SOURCES_LENGTH "${SOURCES_LENGTH}-1")
set(NESTING 0)
set(START 0)
foreach(INDEX RANGE ${SOURCES_LENGTH})
string(SUBSTRING "${SOURCES}" ${INDEX} 1 CURRENT_CHAR)
#message("${CURRENT_CHAR}")
if(CURRENT_CHAR STREQUAL "{")
if(NESTING EQUAL 0)
math(EXPR SUBLENGTH "${INDEX}-${START} +1")
string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK)
set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}")
#message("INDEX: ${INDEX} START: ${START} LENGTH: ${SUBLENGTH}")
endif()
math(EXPR NESTING "${NESTING}+1")
elseif(CURRENT_CHAR STREQUAL "}")
math(EXPR NESTING "${NESTING}-1")
if(NESTING EQUAL 0)
set(START ${INDEX})
endif()
endif()
endforeach()
math(EXPR SUBLENGTH "${SOURCES_LENGTH}-${START} +1")
string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK)
set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}")
set(${VAR_NAME} "${PARSED_SOURCES}" PARENT_SCOPE)
endfunction()
function(extract_prototypes VAR_NAME SOURCES)
string(REGEX MATCHALL "${PROTOTPYE_REGEX}"
SOURCES
"${SOURCES}")
set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE)
endfunction()
read_sketches(SKETCH_SOURCE ${FILES})
strip_sources(SKETCH_SOURCE "${SKETCH_SOURCE}")
collapse_braces(SKETCH_SOURCE "${SKETCH_SOURCE}")
extract_prototypes(SKETCH_SOURCE "${SKETCH_SOURCE}")
message("===============")
foreach(ENTRY ${SKETCH_SOURCE})
message("START]]]${ENTRY}[[[END")
endforeach()
message("===============")
#message("${SKETCH_SOURCE}")

581
cmake/modules/FindArduino.cmake Normal file
View File

@ -0,0 +1,581 @@
# - Generate firmware and libraries for Arduino Devices
# generate_arduino_firmware(TARGET_NAME)
# TARGET_NAME - Name of target
# Creates a Arduino firmware target.
#
# The target options can be configured by setting options of
# the following format:
# ${TARGET_NAME}${SUFFIX}
# The following suffixes are availabe:
# _SRCS # Sources
# _HDRS # Headers
# _SKETCHES # Arduino sketch files
# _LIBS # Libraries to linked in
# _BOARD # Board name (such as uno, mega2560, ...)
# _PORT # Serial port, for upload and serial targets [OPTIONAL]
# _AFLAGS # Override global Avrdude flags for target
# _SERIAL # Serial command for serial target [OPTIONAL]
# _NO_AUTOLIBS # Disables Arduino library detection
# Here is a short example for a target named test:
# set(test_SRCS test.cpp)
# set(test_HDRS test.h)
# set(test_BOARD uno)
#
# generate_arduino_firmware(test)
#
#
# generate_arduino_library(TARGET_NAME)
# TARGET_NAME - Name of target
# Creates a Arduino firmware target.
#
# The target options can be configured by setting options of
# the following format:
# ${TARGET_NAME}${SUFFIX}
# The following suffixes are availabe:
#
# _SRCS # Sources
# _HDRS # Headers
# _LIBS # Libraries to linked in
# _BOARD # Board name (such as uno, mega2560, ...)
# _NO_AUTOLIBS # Disables Arduino library detection
#
# Here is a short example for a target named test:
# set(test_SRCS test.cpp)
# set(test_HDRS test.h)
# set(test_BOARD uno)
#
# generate_arduino_library(test)
find_path(ARDUINO_SDK_PATH
NAMES lib/version.txt hardware libraries
PATH_SUFFIXES share/arduino
DOC "Arduino Development Kit path.")
# load_board_settings()
#
# Load the Arduino SDK board settings from the boards.txt file.
#
function(LOAD_BOARD_SETTINGS)
if(NOT ARDUINO_BOARDS AND ARDUINO_BOARDS_PATH)
file(STRINGS ${ARDUINO_BOARDS_PATH} BOARD_SETTINGS)
foreach(BOARD_SETTING ${BOARD_SETTINGS})
if("${BOARD_SETTING}" MATCHES "^.*=.*")
string(REGEX MATCH "^[^=]+" SETTING_NAME ${BOARD_SETTING})
string(REGEX MATCH "[^=]+$" SETTING_VALUE ${BOARD_SETTING})
string(REPLACE "." ";" SETTING_NAME_TOKENS ${SETTING_NAME})
list(LENGTH SETTING_NAME_TOKENS SETTING_NAME_TOKENS_LEN)
# Add Arduino to main list of arduino boards
list(GET SETTING_NAME_TOKENS 0 BOARD_ID)
list(FIND ARDUINO_BOARDS ${BOARD_ID} ARDUINO_BOARD_INDEX)
if(ARDUINO_BOARD_INDEX LESS 0)
list(APPEND ARDUINO_BOARDS ${BOARD_ID})
endif()
# Add setting to board settings list
list(GET SETTING_NAME_TOKENS 1 BOARD_SETTING)
list(FIND ${BOARD_ID}.SETTINGS ${BOARD_SETTING} BOARD_SETTINGS_LEN)
if(BOARD_SETTINGS_LEN LESS 0)
list(APPEND ${BOARD_ID}.SETTINGS ${BOARD_SETTING})
set(${BOARD_ID}.SETTINGS ${${BOARD_ID}.SETTINGS}
CACHE INTERNAL "Arduino ${BOARD_ID} Board settings list")
endif()
set(ARDUINO_SETTING_NAME ${BOARD_ID}.${BOARD_SETTING})
# Add sub-setting to board sub-settings list
if(SETTING_NAME_TOKENS_LEN GREATER 2)
list(GET SETTING_NAME_TOKENS 2 BOARD_SUBSETTING)
list(FIND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING} BOARD_SUBSETTINGS_LEN)
if(BOARD_SUBSETTINGS_LEN LESS 0)
list(APPEND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING})
set(${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS}
CACHE INTERNAL "Arduino ${BOARD_ID} Board sub-settings list")
endif()
set(ARDUINO_SETTING_NAME ${ARDUINO_SETTING_NAME}.${BOARD_SUBSETTING})
endif()
# Save setting value
set(${ARDUINO_SETTING_NAME} ${SETTING_VALUE} CACHE INTERNAL "Arduino ${BOARD_ID} Board setting")
endif()
endforeach()
set(ARDUINO_BOARDS ${ARDUINO_BOARDS} CACHE STRING "List of detected Arduino Board configurations")
mark_as_advanced(ARDUINO_BOARDS)
endif()
endfunction()
# print_board_settings(ARDUINO_BOARD)
#
# ARDUINO_BOARD - Board id
#
# Print the detected Arduino board settings.
#
function(PRINT_BOARD_SETTINGS ARDUINO_BOARD)
if(${ARDUINO_BOARD}.SETTINGS)
message(STATUS "Arduino ${ARDUINO_BOARD} Board:")
foreach(BOARD_SETTING ${${ARDUINO_BOARD}.SETTINGS})
if(${ARDUINO_BOARD}.${BOARD_SETTING})
message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}}")
endif()
if(${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS)
foreach(BOARD_SUBSETTING ${${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS})
if(${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING})
message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}}")
endif()
endforeach()
endif()
message(STATUS "")
endforeach()
endif()
endfunction()
# generate_arduino_library(TARGET_NAME)
#
# see documentation at top
function(GENERATE_ARDUINO_LIBRARY TARGET_NAME)
load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources
_HDRS # Headers
_LIBS # Libraries to linked in
_BOARD) # Board name (such as uno, mega2560, ...)
set(INPUT_AUTOLIBS True)
if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS)
set(INPUT_AUTOLIBS False)
endif()
message(STATUS "Generating ${TARGET_NAME}")
set(ALL_LIBS)
set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS})
setup_arduino_compiler(${INPUT_BOARD})
setup_arduino_core(CORE_LIB ${INPUT_BOARD})
if(INPUT_AUTOLIBS)
setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}")
endif()
list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS})
add_library(${TARGET_NAME} ${ALL_SRCS})
target_link_libraries(${TARGET_NAME} ${ALL_LIBS})
endfunction()
# generate_arduino_firmware(TARGET_NAME)
#
# see documentation at top
function(GENERATE_ARDUINO_FIRMWARE TARGET_NAME)
load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources
_HDRS # Headers
_LIBS # Libraries to linked in
_BOARD # Board name (such as uno, mega2560, ...)
_PORT # Serial port, for upload and serial targets
_AFLAGS # Override global Avrdude flags for target
_SKETCHES # Arduino sketch files
_SERIAL) # Serial command for serial target
set(INPUT_AUTOLIBS True)
if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS)
set(INPUT_AUTOLIBS False)
endif()
message(STATUS "Generating ${TARGET_NAME}")
message(STATUS "Sketches ${INPUT_SKETCHES}")
set(ALL_LIBS)
set(ALL_SRCS ${INPUT_SKETCHES} ${INPUT_SRCS} ${INPUT_HDRS} )
#set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS} )
#set compile flags and file properties for pde files
#SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES LANGUAGE CXX)
#SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES COMPILE_FLAGS "-x c++" )
setup_arduino_compiler(${INPUT_BOARD})
setup_arduino_core(CORE_LIB ${INPUT_BOARD})
#setup_arduino_sketch(SKETCH_SRCS ${INPUT_SKETCHES})
if(INPUT_AUTOLIBS)
setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}")
endif()
list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS})
setup_arduino_target(${TARGET_NAME} "${ALL_SRCS}" "${ALL_LIBS}")
#SET_TARGET_PROPERTIES(${TARGET_NAME} PROPERTIES LINKER_LANGUAGE CXX)
#setup_arduino_target(${TARGET_NAME} "${INPUT_SKETCHES}" ${INPUT_HDRS} "${ALL_LIBS}")
if(INPUT_PORT)
setup_arduino_upload(${INPUT_BOARD} ${TARGET_NAME} ${INPUT_PORT})
endif()
if(INPUT_SERIAL)
setup_serial_target(${TARGET_NAME} "${INPUT_SERIAL}")
endif()
endfunction()
# load_generator_settings(TARGET_NAME PREFIX [SUFFIX_1 SUFFIX_2 .. SUFFIX_N])
#
# TARGET_NAME - The base name of the user settings
# PREFIX - The prefix name used for generator settings
# SUFFIX_XX - List of suffixes to load
#
# Loads a list of user settings into the generators scope. User settings have
# the following syntax:
#
# ${BASE_NAME}${SUFFIX}
#
# The BASE_NAME is the target name and the suffix is a specific generator settings.
#
# For every user setting found a generator setting is created of the follwoing fromat:
#
# ${PREFIX}${SUFFIX}
#
# The purpose of loading the settings into the generator is to not modify user settings
# and to have a generic naming of the settings within the generator.
#
function(LOAD_GENERATOR_SETTINGS TARGET_NAME PREFIX)
foreach(GEN_SUFFIX ${ARGN})
if(${TARGET_NAME}${GEN_SUFFIX})
set(${PREFIX}${GEN_SUFFIX} ${${TARGET_NAME}${GEN_SUFFIX}} PARENT_SCOPE)
endif()
endforeach()
endfunction()
# setup_arduino_compiler(BOARD_ID)
#
# BOARD_ID - The board id name
#
# Configures the the build settings for the specified Arduino Board.
#
macro(setup_arduino_compiler BOARD_ID)
set(BOARD_CORE ${${BOARD_ID}.build.core})
if(BOARD_CORE)
set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE})
include_directories(${BOARD_CORE_PATH})
include_directories(${ARDUINO_LIBRARIES_PATH})
add_definitions(-DF_CPU=${${BOARD_ID}.build.f_cpu}
-DARDUINO=${ARDUINO_SDK_VERSION}
-mmcu=${${BOARD_ID}.build.mcu}
)
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE)
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE)
set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE)
endif()
endmacro()
# setup_arduino_core(VAR_NAME BOARD_ID)
#
# VAR_NAME - Variable name that will hold the generated library name
# BOARD_ID - Arduino board id
#
# Creates the Arduino Core library for the specified board,
# each board gets it's own version of the library.
#
function(setup_arduino_core VAR_NAME BOARD_ID)
set(CORE_LIB_NAME ${BOARD_ID}_CORE)
set(BOARD_CORE ${${BOARD_ID}.build.core})
if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME})
set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE})
find_sources(CORE_SRCS ${BOARD_CORE_PATH})
add_library(${CORE_LIB_NAME} ${CORE_SRCS})
set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE)
endif()
endfunction()
# find_arduino_libraries(VAR_NAME SRCS)
#
# VAR_NAME - Variable name which will hold the results
# SRCS - Sources that will be analized
#
# returns a list of paths to libraries found.
#
# Finds all Arduino type libraries included in sources. Available libraries
# are ${ARDUINO_SDK_PATH}/libraries and ${CMAKE_CURRENT_SOURCE_DIR}.
#
# A Arduino library is a folder that has the same name as the include header.
# For example, if we have a include "#include <LibraryName.h>" then the following
# directory structure is considered a Arduino library:
#
# LibraryName/
# |- LibraryName.h
# `- LibraryName.c
#
# If such a directory is found then all sources within that directory are considred
# to be part of that Arduino library.
#
function(find_arduino_libraries VAR_NAME SRCS)
set(ARDUINO_LIBS )
foreach(SRC ${SRCS})
file(STRINGS ${SRC} SRC_CONTENTS)
foreach(SRC_LINE ${SRC_CONTENTS})
if("${SRC_LINE}" MATCHES "^ *#include *[<\"](.*)[>\"]")
get_filename_component(INCLUDE_NAME ${CMAKE_MATCH_1} NAME_WE)
foreach(LIB_SEARCH_PATH ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR})
if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1})
list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME})
break()
endif()
endforeach()
endif()
endforeach()
endforeach()
if(ARDUINO_LIBS)
list(REMOVE_DUPLICATES ARDUINO_LIBS)
endif()
set(${VAR_NAME} ${ARDUINO_LIBS} PARENT_SCOPE)
endfunction()
# setup_arduino_library(VAR_NAME BOARD_ID LIB_PATH)
#
# VAR_NAME - Vairable wich will hold the generated library names
# BOARD_ID - Board name
# LIB_PATH - path of the library
#
# Creates an Arduino library, with all it's library dependencies.
#
function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH)
set(LIB_TARGETS)
get_filename_component(LIB_NAME ${LIB_PATH} NAME)
set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME})
if(NOT TARGET ${TARGET_LIB_NAME})
find_sources(LIB_SRCS ${LIB_PATH})
if(LIB_SRCS)
message(STATUS "Generating Arduino ${LIB_NAME} library")
include_directories(${LIB_PATH} ${LIB_PATH}/utility)
add_library(${TARGET_LIB_NAME} STATIC ${LIB_SRCS})
find_arduino_libraries(LIB_DEPS "${LIB_SRCS}")
foreach(LIB_DEP ${LIB_DEPS})
setup_arduino_library(DEP_LIB_SRCS ${BOARD_ID} ${LIB_DEP})
list(APPEND LIB_TARGETS ${DEP_LIB_SRCS})
endforeach()
target_link_libraries(${TARGET_LIB_NAME} ${BOARD_ID}_CORE ${LIB_TARGETS})
list(APPEND LIB_TARGETS ${TARGET_LIB_NAME})
endif()
else()
# Target already exists, skiping creating
include_directories(${LIB_PATH} ${LIB_PATH}/utility)
list(APPEND LIB_TARGETS ${TARGET_LIB_NAME})
endif()
if(LIB_TARGETS)
list(REMOVE_DUPLICATES LIB_TARGETS)
endif()
set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE)
endfunction()
# setup_arduino_libraries(VAR_NAME BOARD_ID SRCS)
#
# VAR_NAME - Vairable wich will hold the generated library names
# BOARD_ID - Board ID
# SRCS - source files
#
# Finds and creates all dependency libraries based on sources.
#
function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS)
set(LIB_TARGETS)
find_arduino_libraries(TARGET_LIBS ${SRCS})
foreach(TARGET_LIB ${TARGET_LIBS})
setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources
list(APPEND LIB_TARGETS ${LIB_DEPS})
endforeach()
set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE)
endfunction()
# setup_arduino_target(TARGET_NAME ALL_SRCS ALL_LIBS)
#
# TARGET_NAME - Target name
# ALL_SRCS - All sources
# ALL_LIBS - All libraries
#
# Creates an Arduino firmware target.
#
function(setup_arduino_target TARGET_NAME ALL_SRCS ALL_LIBS)
add_executable(${TARGET_NAME} ${ALL_SRCS})
target_link_libraries(${TARGET_NAME} ${ALL_LIBS})
set_target_properties(${TARGET_NAME} PROPERTIES SUFFIX ".elf")
set(TARGET_PATH ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME})
add_custom_command(TARGET ${TARGET_NAME} POST_BUILD
COMMAND ${CMAKE_OBJCOPY}
ARGS ${ARDUINO_OBJCOPY_EEP_FLAGS}
${TARGET_PATH}.elf
${TARGET_PATH}.eep
VERBATIM)
add_custom_command(TARGET ${TARGET_NAME} POST_BUILD
COMMAND ${CMAKE_OBJCOPY}
ARGS ${ARDUINO_OBJCOPY_HEX_FLAGS}
${TARGET_PATH}.elf
${TARGET_PATH}.hex
VERBATIM)
endfunction()
# setup_arduino_upload(BOARD_ID TARGET_NAME PORT)
#
# BOARD_ID - Arduino board id
# TARGET_NAME - Target name
# PORT - Serial port for upload
#
# Create an upload target (${TARGET_NAME}-upload) for the specified Arduino target.
#
function(setup_arduino_upload BOARD_ID TARGET_NAME PORT)
set(AVRDUDE_FLAGS ${ARDUINO_AVRDUDE_FLAGS})
if(DEFINED ${TARGET_NAME}_AFLAGS)
set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS})
endif()
add_custom_target(${TARGET_NAME}-upload
${ARDUINO_AVRDUDE_PROGRAM}
-U flash:w:${TARGET_NAME}.hex
${AVRDUDE_FLAGS}
-C ${ARDUINO_AVRDUDE_CONFIG_PATH}
-p ${${BOARD_ID}.build.mcu}
-c ${${BOARD_ID}.upload.protocol}
-b ${${BOARD_ID}.upload.speed}
-P ${PORT}
DEPENDS ${TARGET_NAME})
if(NOT TARGET upload)
add_custom_target(upload)
endif()
add_dependencies(upload ${TARGET_NAME}-upload)
endfunction()
# find_sources(VAR_NAME LIB_PATH)
#
# VAR_NAME - Variable name that will hold the detected sources
# LIB_PATH - The base path
#
# Finds all C/C++ sources located at the specified path.
#
function(find_sources VAR_NAME LIB_PATH)
file(GLOB_RECURSE LIB_FILES ${LIB_PATH}/*.cpp
${LIB_PATH}/*.c
${LIB_PATH}/*.cc
${LIB_PATH}/*.cxx
${LIB_PATH}/*.h
${LIB_PATH}/*.hh
${LIB_PATH}/*.hxx)
if(LIB_FILES)
set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE)
endif()
endfunction()
# setup_serial_target(TARGET_NAME CMD)
#
# TARGET_NAME - Target name
# CMD - Serial terminal command
#
# Creates a target (${TARGET_NAME}-serial) for launching the serial termnial.
#
function(setup_serial_target TARGET_NAME CMD)
string(CONFIGURE "${CMD}" FULL_CMD @ONLY)
add_custom_target(${TARGET_NAME}-serial
${FULL_CMD})
endfunction()
# detect_arduino_version(VAR_NAME)
#
# VAR_NAME - Variable name where the detected version will be saved
#
# Detects the Arduino SDK Version based on the revisions.txt file.
#
function(detect_arduino_version VAR_NAME)
if(ARDUINO_VERSION_PATH)
file(READ ${ARDUINO_VERSION_PATH} ARD_VERSION)
if("${ARD_VERSION}" MATCHES " *[0]+([0-9]+)")
set(${VAR_NAME} ${CMAKE_MATCH_1} PARENT_SCOPE)
endif()
endif()
endfunction()
function(convert_arduino_sketch VAR_NAME SRCS)
endfunction()
# Setting up Arduino enviroment settings
if(NOT ARDUINO_FOUND)
find_file(ARDUINO_CORES_PATH
NAMES cores
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_LIBRARIES_PATH
NAMES libraries
PATHS ${ARDUINO_SDK_PATH})
find_file(ARDUINO_BOARDS_PATH
NAMES boards.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_PROGRAMMERS_PATH
NAMES programmers.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_REVISIONS_PATH
NAMES revisions.txt
PATHS ${ARDUINO_SDK_PATH})
find_file(ARDUINO_VERSION_PATH
NAMES lib/version.txt
PATHS ${ARDUINO_SDK_PATH})
find_program(ARDUINO_AVRDUDE_PROGRAM
NAMES avrdude
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/tools)
find_program(ARDUINO_AVRDUDE_CONFIG_PATH
NAMES avrdude.conf
PATHS ${ARDUINO_SDK_PATH} /etc/avrdude
PATH_SUFFIXES hardware/tools
hardware/tools/avr/etc)
set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0
CACHE STRING "")
set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom
CACHE STRING "")
set(ARDUINO_AVRDUDE_FLAGS -V -F
CACHE STRING "Arvdude global flag list.")
if(ARDUINO_SDK_PATH)
detect_arduino_version(ARDUINO_SDK_VERSION)
set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version")
endif(ARDUINO_SDK_PATH)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Arduino
REQUIRED_VARS ARDUINO_SDK_PATH
ARDUINO_SDK_VERSION
VERSION_VAR ARDUINO_SDK_VERSION)
mark_as_advanced(ARDUINO_CORES_PATH
ARDUINO_SDK_VERSION
ARDUINO_LIBRARIES_PATH
ARDUINO_BOARDS_PATH
ARDUINO_PROGRAMMERS_PATH
ARDUINO_REVISIONS_PATH
ARDUINO_AVRDUDE_PROGRAM
ARDUINO_AVRDUDE_CONFIG_PATH
ARDUINO_OBJCOPY_EEP_FLAGS
ARDUINO_OBJCOPY_HEX_FLAGS)
load_board_settings()
endif()

0
ArduCopter/flight_control.pde → cmake/modules/Platform/Arduino.cmake
View File

21
cmake/modules/Platform/ArduinoPaths.cmake Normal file
View File

@ -0,0 +1,21 @@
if(UNIX)
include(Platform/UnixPaths)
if(APPLE)
list(APPEND CMAKE_SYSTEM_PREFIX_PATH ~/Applications
/Applications
/Developer/Applications
/sw # Fink
/opt/local) # MacPorts
endif()
elseif(WIN32)
include(Platform/WindowsPaths)
endif()
if(ARDUINO_SDK_PATH)
if(WIN32)
list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin)
list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/utils/bin)
elseif(APPLE)
list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin)
endif()
endif()

66
cmake/toolchains/Arduino.cmake Normal file
View File

@ -0,0 +1,66 @@
set(CMAKE_SYSTEM_NAME Arduino)
set(CMAKE_C_COMPILER avr-gcc)
set(CMAKE_CXX_COMPILER avr-g++)
#=============================================================================#
# C Flags #
#=============================================================================#
set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
set(CMAKE_C_FLAGS "-g -Os ${ARDUINO_C_FLAGS}" CACHE STRING "")
set(CMAKE_C_FLAGS_DEBUG "-g ${ARDUINO_C_FLAGS}" CACHE STRING "")
set(CMAKE_C_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_C_FLAGS}" CACHE STRING "")
set(CMAKE_C_FLAGS_RELEASE "-Os -DNDEBUG -w ${ARDUINO_C_FLAGS}" CACHE STRING "")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-Os -g -w ${ARDUINO_C_FLAGS}" CACHE STRING "")
#=============================================================================#
# C++ Flags #
#=============================================================================#
set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions")
set(CMAKE_CXX_FLAGS "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
set(CMAKE_CXX_FLAGS_DEBUG "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
set(CMAKE_CXX_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
set(CMAKE_CXX_FLAGS_RELEASE "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-Os -g ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
#=============================================================================#
# Executable Linker Flags #
#=============================================================================#
set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections -lc -lm")
set(CMAKE_EXE_LINKER_FLAGS "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
set(CMAKE_EXE_LINKER_FLAGS_MINSIZEREL "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
#=============================================================================#
# Shared Lbrary Linker Flags #
#=============================================================================#
set(CMAKE_SHARED_LINKER_FLAGS "" CACHE STRING "")
set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "" CACHE STRING "")
set(CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "")
set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "" CACHE STRING "")
set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "")
set(CMAKE_MODULE_LINKER_FLAGS "" CACHE STRING "")
set(CMAKE_MODULE_LINKER_FLAGS_DEBUG "" CACHE STRING "")
set(CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "")
set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "" CACHE STRING "")
set(CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "")
set(ARDUINO_PATHS)
foreach(VERSION RANGE 22 1)
list(APPEND ARDUINO_PATHS arduino-00${VERSION})
endforeach()
find_path(ARDUINO_SDK_PATH
NAMES lib/version.txt
PATH_SUFFIXES share/arduino
Arduino.app/Contents/Resources/Java/
${ARDUINO_PATHS}
DOC "Arduino Development Kit path.")
include(Platform/ArduinoPaths)

2
libraries/APM_BMP085/APM_BMP085.h
View File

@ -2,7 +2,7 @@
#define APM_BMP085_h
#define TEMP_FILTER_SIZE 16
#define PRESS_FILTER_SIZE 8
#define PRESS_FILTER_SIZE 10
class APM_BMP085_Class
{

22
libraries/APM_BMP085/CMakeLists.txt Normal file
View File

@ -0,0 +1,22 @@
set(LIB_NAME APM_BMP085)
set(${LIB_NAME}_SRCS
APM_BMP085.cpp
) # Firmware sources
set(${LIB_NAME}_HDRS
APM_BMP085.h
)
include_directories(
-
#${CMAKE_SOURCE_DIR}/libraries/AP_Math
#${CMAKE_SOURCE_DIR}/libraries/AP_Common
#${CMAKE_SOURCE_DIR}/libraries/FastSerial
#
)
set(${LIB_NAME}_BOARD mega2560)
generate_arduino_library(${LIB_NAME})

12
libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde
View File

@ -12,9 +12,11 @@ unsigned long timer;
void setup()
{
APM_BMP085.Init(); // APM ADC initialization
Serial.begin(38400);
Serial.begin(115200);
Serial.println("ArduPilot Mega BMP085 library test");
Serial.println("Initialising barometer..."); delay(100);
APM_BMP085.Init(); // APM ADC initialization
Serial.println("initialisation complete."); delay(100);
delay(1000);
timer = millis();
}
@ -28,11 +30,11 @@ void loop()
if((millis()- timer) > 50){
timer = millis();
APM_BMP085.Read();
Serial.print("Pressure:");
Serial.print("Pressure:");
Serial.print(APM_BMP085.Press);
Serial.print(" Temperature:");
Serial.print(" Temperature:");
Serial.print(APM_BMP085.Temp / 10.0);
Serial.print(" Altitude:");
Serial.print(" Altitude:");
tmp_float = (APM_BMP085.Press / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);

24
libraries/APM_PI/CMakeLists.txt Normal file
View File

@ -0,0 +1,24 @@
set(LIB_NAME APM_PI)
set(${LIB_NAME}_SRCS
APM_PI.cpp
#AP_OpticalFlow_ADNS3080.cpp
) # Firmware sources
set(${LIB_NAME}_HDRS
APM_PI.h
#AP_OpticalFlow_ADNS3080.h
)
include_directories(
-
#${CMAKE_SOURCE_DIR}/libraries/AP_Math
${CMAKE_SOURCE_DIR}/libraries/AP_Common
#${CMAKE_SOURCE_DIR}/libraries/FastSerial
#
)
set(${LIB_NAME}_BOARD mega2560)
generate_arduino_library(${LIB_NAME})

13
libraries/APM_RC/APM_RC.h
View File

@ -5,6 +5,19 @@
#define MIN_PULSEWIDTH 900
#define MAX_PULSEWIDTH 2100
// Radio channels
// Note channels are from 0!
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define CH_10 9 //PB5
#define CH_11 10 //PE3
#include <inttypes.h>
class APM_RC_Class

22
libraries/APM_RC/CMakeLists.txt Normal file
View File

@ -0,0 +1,22 @@
set(LIB_NAME APM_RC)
set(${LIB_NAME}_SRCS
APM_RC.cpp
) # Firmware sources
set(${LIB_NAME}_HDRS
APM_RC.h
)
include_directories(
-
#${CMAKE_SOURCE_DIR}/libraries/AP_Math
#${CMAKE_SOURCE_DIR}/libraries/AP_Common
#${CMAKE_SOURCE_DIR}/libraries/FastSerial
#
)
set(${LIB_NAME}_BOARD mega2560)
generate_arduino_library(${LIB_NAME})

21
libraries/AP_ADC/AP_ADC.h
View File

@ -1,6 +1,8 @@
#ifndef AP_ADC_H
#define AP_ADC_H
#include <stdint.h>
/*
AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
Code by James Goppert. DIYDrones.com
@ -13,7 +15,7 @@
Methods:
Init() : Initialization of ADC. (interrupts etc)
Ch(ch_num) : Return the ADC channel value
Ch6(channel_numbers, result) : Return 6 ADC channel values
*/
class AP_ADC
@ -21,8 +23,21 @@ class AP_ADC
public:
AP_ADC() {}; // Constructor
virtual void Init() {};
virtual int Ch(unsigned char ch_num) = 0;
virtual int Ch_raw(unsigned char ch_num) = 0;
/* read one channel value */
virtual uint16_t Ch(uint8_t ch_num) = 0;
/* read 6 channels values as a set, used by IMU for 3 gyros
and 3 accelerometeres.
Pass in an array of 6 channel numbers and results are
returned in result[]
The function returns the amount of time (in microseconds)
since the last call to Ch6().
*/
virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0;
private:
};

190
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -19,9 +19,11 @@
XCK2 = SCK = pin PH2
Chip Select pin is PC4 (33) [PH6 (9)]
We are using the 16 clocks per conversion timming to increase efficiency (fast)
The sampling frequency is 400Hz (Timer2 overflow interrupt)
The sampling frequency is 1kHz (Timer2 overflow interrupt)
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
we have an 4x oversampling and averaging.
we have an 10x oversampling and averaging.
Methods:
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
@ -44,6 +46,7 @@
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <stdint.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
@ -53,13 +56,25 @@ extern "C" {
// Commands for reading ADC channels on ADS7844
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
static volatile uint16_t _filter[8][ADC_FILTER_SIZE];
static volatile uint8_t _filter_index;
static unsigned char ADC_SPI_transfer(unsigned char data)
// the sum of the values since last read
static volatile uint32_t _sum[8];
// how many values we've accumulated since last read
static volatile uint16_t _count[8];
static uint32_t last_ch6_micros;
// TCNT2 values for various interrupt rates,
// assuming 256 prescaler. Note that these values
// assume a zero-time ISR. The actual rate will be a
// bit lower than this
#define TCNT2_781_HZ (256-80)
#define TCNT2_1008_HZ (256-62)
#define TCNT2_1302_HZ (256-48)
static inline unsigned char ADC_SPI_transfer(unsigned char data)
{
/* Wait for empty transmit buffer */
while ( !( UCSR2A & (1 << UDRE2)) );
/* Put data into buffer, sends the data */
UDR2 = data;
/* Wait for data to be received */
@ -72,32 +87,43 @@ static unsigned char ADC_SPI_transfer(unsigned char data)
ISR (TIMER2_OVF_vect)
{
uint8_t ch;
uint16_t adc_tmp;
static uint8_t timer_offset;
//bit_set(PORTL,6); // To test performance
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
for (ch = 0; ch < 8; ch++) {
uint16_t v;
for (ch = 0; ch < 8; ch++){
adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
v = ADC_SPI_transfer(0) << 8; // Read first byte
v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
// Fill our Moving average filter
_filter[ch][_filter_index] = adc_tmp >> 3;
if (v & 0x8007) {
// this is a 12-bit ADC, shifted by 3 bits.
// if we get other bits set then the value is
// bogus and should be ignored
continue;
}
if (++_count[ch] == 0) {
// overflow ... shouldn't happen too often
// unless we're just not using the
// channel. Notice that we overflow the count
// to 1 here, not zero, as otherwise the
// reader below could get a division by zero
_sum[ch] = 0;
_count[ch] = 1;
last_ch6_micros = micros();
}
_sum[ch] += (v >> 3);
}
// increment our filter
_filter_index++;
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
// loop our filter
if(_filter_index == ADC_FILTER_SIZE)
_filter_index = 0;
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
//bit_clear(PORTL,6); // To test performance
TCNT2 = 104; // 400 Hz
// this gives us a sample rate between 781Hz and 1302Hz. We
// randomise it to try to minimise aliasing effects
timer_offset = (timer_offset + 49) % 32;
TCNT2 = TCNT2_781_HZ + timer_offset;
}
@ -111,54 +137,96 @@ void AP_ADC_ADS7844::Init(void)
{
pinMode(ADC_CHIP_SELECT, OUTPUT);
digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low)
digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low)
// Setup Serial Port2 in SPI mode
UBRR2 = 0;
DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
// Set MSPI mode of operation and SPI data mode 0.
UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
// Enable receiver and transmitter.
UCSR2B = (1 << RXEN2) | (1 << TXEN2);
// Set Baud rate
UBRR2 = 2; // SPI clock running at 2.6MHz
UBRR2 = 2; // SPI clock running at 2.6MHz
// Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256
TCNT2 = 0;
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
}
// Read one channel value
int AP_ADC_ADS7844::Ch(unsigned char ch_num)
{
uint16_t result = 0;
//while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator
// stop interrupts
cli();
// sum our filter
for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){
result += _filter[ch_num][i];
// get an initial value for each channel. This ensures
// _count[] is never zero
for (uint8_t i=0; i<8; i++) {
uint16_t adc_tmp;
adc_tmp = ADC_SPI_transfer(0) << 8;
adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]);
_count[i] = 1;
_sum[i] = adc_tmp;
}
// resume interrupts
sei();
last_ch6_micros = micros();
// average our sampels
result /= ADC_FILTER_SIZE;
return(result);
// Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256
TCNT2 = 0;
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
}
// Read one channel value
int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num)
uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
{
return _filter[ch_num][_filter_index]; // close enough
uint16_t count;
uint32_t sum;
// ensure we have at least one value
while (_count[ch_num] == 0) /* noop */ ;
// grab the value with interrupts disabled, and clear the count
cli();
count = _count[ch_num];
sum = _sum[ch_num];
_count[ch_num] = 0;
_sum[ch_num] = 0;
sei();
return sum/count;
}
// Read 6 channel values
// this assumes that the counts for all of the 6 channels are
// equal. This will only be true if we always consistently access a
// sensor by either Ch6() or Ch() and never mix them. If you mix them
// then you will get very strange results
uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
{
uint16_t count[6];
uint32_t sum[6];
uint8_t i;
// ensure we have at least one value
for (i=0; i<6; i++) {
while (_count[channel_numbers[i]] == 0) /* noop */;
}
// grab the values with interrupts disabled, and clear the counts
cli();
for (i=0; i<6; i++) {
count[i] = _count[channel_numbers[i]];
sum[i] = _sum[channel_numbers[i]];
_count[channel_numbers[i]] = 0;
_sum[channel_numbers[i]] = 0;
}
sei();
// calculate averages. We keep this out of the cli region
// to prevent us stalling the ISR while doing the
// division. That costs us 36 bytes of stack, but I think its
// worth it.
for (i=0; i<6; i++) {
result[i] = sum[i] / count[i];
}
// return number of microseconds since last call
uint32_t us = micros();
uint32_t ret = us - last_ch6_micros;
last_ch6_micros = us;
return ret;
}

10
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -9,7 +9,7 @@
#define ADC_DATAIN 50 // MISO
#define ADC_SPICLOCK 52 // SCK
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
#define ADC_FILTER_SIZE 6
#define ADC_FILTER_SIZE 3
#include "AP_ADC.h"
#include <inttypes.h>
@ -19,8 +19,12 @@ class AP_ADC_ADS7844 : public AP_ADC
public:
AP_ADC_ADS7844(); // Constructor
void Init();
int Ch(unsigned char ch_num);
int Ch_raw(unsigned char ch_num);
// Read 1 sensor value
uint16_t Ch(unsigned char ch_num);
// Read 6 sensors at once
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
private:
};

16
libraries/AP_ADC/AP_ADC_HIL.cpp
View File

@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL()
// set diff press and temp to zero
setGyroTemp(0);
setPressure(0);
last_hil_time = millis();
}
void AP_ADC_HIL::Init(void)
@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void)
}
// Read one channel value
int AP_ADC_HIL::Ch(unsigned char ch_num)
uint16_t AP_ADC_HIL::Ch(unsigned char ch_num)
{
return adcValue[ch_num];
return adcValue[ch_num];
}
// Read one channel value
int AP_ADC_HIL::Ch_raw(unsigned char ch_num)
// Read 6 channel values
uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result)
{
return adcValue[ch_num];
for (uint8_t i=0; i<6; i++) {
result[i] = Ch(channel_numbers[i]);
}
return ((millis() - last_hil_time)*2)/5;
}
// Set one channel value

20
libraries/AP_ADC/AP_ADC_HIL.h
View File

@ -32,10 +32,11 @@ class AP_ADC_HIL : public AP_ADC
///
// Read the sensor, part of public AP_ADC interface
int Ch(unsigned char ch_num);
uint16_t Ch(unsigned char ch_num);
///
// Read the sensor, part of public AP_ADC interface
int Ch_raw(unsigned char ch_num);
// Read 6 sensors at once
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
///
// Set the adc raw values given the current rotations rates,
@ -49,16 +50,19 @@ class AP_ADC_HIL : public AP_ADC
// The raw adc array
uint16_t adcValue[8];
// the time in milliseconds when we last got a HIL update
uint32_t last_hil_time;
///
// sensor constants
// constants declared in cpp file
// @see AP_ADC_HIL.cpp
static const uint8_t sensors[6];
static const float gyroBias[3];
static const float gyroScale[3];
static const uint8_t sensors[6];
static const float gyroBias[3];
static const float gyroScale[3];
static const float accelBias[3];
static const float accelScale[3];
static const int8_t sensorSign[6];
static const float accelScale[3];
static const int8_t sensorSign[6];
///
// gyro set function

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