mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
a52eff740e
|
@ -0,0 +1,4 @@
|
||||||
|
.metadata/
|
||||||
|
Tools/ArdupilotMegaPlanner/bin/Release/logs/
|
||||||
|
config.mk
|
||||||
|
ArduCopter/Debug/
|
|
@ -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
|
// GPS is auto-selected
|
||||||
|
|
||||||
|
@ -46,10 +48,15 @@
|
||||||
CH6_TRAVERSE_SPEED
|
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!
|
// 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
|
// lets use Manual throttle during Loiter
|
||||||
//#define LOITER_THR THROTTLE_MANUAL
|
//#define LOITER_THR THROTTLE_MANUAL
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
/// -*- 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 -*-
|
||||||
|
|
||||||
|
#define THISFIRMWARE "ArduCopter V2.0.43 Beta"
|
||||||
/*
|
/*
|
||||||
ArduCopter Version 2.0 Beta
|
ArduCopter Version 2.0 Beta
|
||||||
Authors: Jason Short
|
Authors: Jason Short
|
||||||
|
@ -127,9 +128,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||||
APM_BMP085_Class barometer;
|
APM_BMP085_Class barometer;
|
||||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||||
|
|
||||||
#ifdef OPTFLOW_ENABLED
|
|
||||||
AP_OpticalFlow_ADNS3080 optflow;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// real GPS selection
|
// real GPS selection
|
||||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||||
|
@ -201,6 +199,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||||
#endif
|
#endif
|
||||||
// normal dcm
|
// normal dcm
|
||||||
AP_DCM dcm(&imu, g_gps);
|
AP_DCM dcm(&imu, g_gps);
|
||||||
|
|
||||||
|
#ifdef OPTFLOW_ENABLED
|
||||||
|
AP_OpticalFlow_ADNS3080 optflow;
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -237,7 +239,6 @@ static const char *comma = ",";
|
||||||
static const char* flight_mode_strings[] = {
|
static const char* flight_mode_strings[] = {
|
||||||
"STABILIZE",
|
"STABILIZE",
|
||||||
"ACRO",
|
"ACRO",
|
||||||
"SIMPLE",
|
|
||||||
"ALT_HOLD",
|
"ALT_HOLD",
|
||||||
"AUTO",
|
"AUTO",
|
||||||
"GUIDED",
|
"GUIDED",
|
||||||
|
@ -275,6 +276,7 @@ static byte control_mode = STABILIZE;
|
||||||
static byte old_control_mode = STABILIZE;
|
static byte old_control_mode = STABILIZE;
|
||||||
static byte oldSwitchPosition; // for remembering the control mode switch
|
static byte oldSwitchPosition; // for remembering the control mode switch
|
||||||
static int motor_out[8];
|
static int motor_out[8];
|
||||||
|
static bool do_simple = false;
|
||||||
|
|
||||||
// Heli
|
// 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 radius_of_earth = 6378100; // meters
|
||||||
static const float gravity = 9.81; // meters/ sec^2
|
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 long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||||
|
|
||||||
static bool xtrack_enabled = false;
|
//static bool xtrack_enabled = false;
|
||||||
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
|
//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 int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
|
||||||
static long circle_angle = 0;
|
static long circle_angle = 0;
|
||||||
static byte wp_control; // used to control - navgation or loiter
|
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 cos_yaw_x = 1;
|
||||||
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
||||||
static long initial_simple_bearing; // used for Simple mode
|
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
|
// Acro
|
||||||
#if CH7_OPTION == DO_FLIP
|
#if CH7_OPTION == DO_FLIP
|
||||||
|
@ -349,7 +354,7 @@ static int airspeed; // m/s * 100
|
||||||
|
|
||||||
// Location Errors
|
// 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 altitude_error; // meters * 100 we are off in altitude
|
||||||
static long old_altitude;
|
static long old_altitude;
|
||||||
static long yaw_error; // how off are we pointed
|
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 struct Location guided_WP; // guided mode waypoint
|
||||||
static long target_altitude; // used for
|
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 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
|
static boolean new_location; // flag to tell us if location has been updated
|
||||||
|
|
||||||
|
|
||||||
// IMU variables
|
// IMU variables
|
||||||
// -------------
|
// -------------
|
||||||
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
||||||
|
|
||||||
|
|
||||||
// Performance monitoring
|
// Performance monitoring
|
||||||
// ----------------------
|
// ----------------------
|
||||||
static long perf_mon_timer;
|
static long perf_mon_timer;
|
||||||
//static float imu_health; // Metric based on accel gain deweighting
|
//static float imu_health; // Metric based on accel gain deweighting
|
||||||
static int G_Dt_max; // Max main loop cycle time in milliseconds
|
static int G_Dt_max; // Max main loop cycle time in milliseconds
|
||||||
static int gps_fix_count;
|
static int gps_fix_count;
|
||||||
|
static byte gps_watchdog;
|
||||||
// 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
|
|
||||||
|
|
||||||
// System Timers
|
// System Timers
|
||||||
// --------------
|
// --------------
|
||||||
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
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 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 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
|
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 byte slow_loopCounter;
|
||||||
static int superslow_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
|
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 byte counter_one_herz;
|
||||||
static bool GPS_enabled = false;
|
static bool GPS_enabled = false;
|
||||||
static byte loop_step;
|
static byte loop_step;
|
||||||
|
static bool new_radio_frame;
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
// Top-level logic
|
// Top-level logic
|
||||||
|
@ -531,17 +527,11 @@ void loop()
|
||||||
//PORTK |= B00010000;
|
//PORTK |= B00010000;
|
||||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||||
fast_loopTimer = millis();
|
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
|
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
|
// Execute the fast loop
|
||||||
// ---------------------
|
// ---------------------
|
||||||
fast_loop();
|
fast_loop();
|
||||||
fast_loopTimeStamp = millis();
|
|
||||||
}
|
}
|
||||||
//PORTK &= B11101111;
|
//PORTK &= B11101111;
|
||||||
|
|
||||||
|
@ -566,15 +556,12 @@ void loop()
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis() - perf_mon_timer > 20000) {
|
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)
|
if (g.log_bitmask & MASK_LOG_PM)
|
||||||
Log_Write_Performance();
|
Log_Write_Performance();
|
||||||
|
|
||||||
resetPerfData();
|
resetPerfData();
|
||||||
}
|
|
||||||
}
|
}
|
||||||
//PORTK &= B10111111;
|
//PORTK &= B10111111;
|
||||||
}
|
}
|
||||||
|
@ -608,7 +595,6 @@ static void fast_loop()
|
||||||
// ---------------------------------------
|
// ---------------------------------------
|
||||||
update_yaw_mode();
|
update_yaw_mode();
|
||||||
update_roll_pitch_mode();
|
update_roll_pitch_mode();
|
||||||
update_throttle_mode();
|
|
||||||
|
|
||||||
// write out the servo PWM values
|
// write out the servo PWM values
|
||||||
// ------------------------------
|
// ------------------------------
|
||||||
|
@ -628,6 +614,18 @@ static void medium_loop()
|
||||||
|
|
||||||
medium_loopCounter++;
|
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){
|
if(GPS_enabled){
|
||||||
update_GPS();
|
update_GPS();
|
||||||
}
|
}
|
||||||
|
@ -644,6 +642,10 @@ static void medium_loop()
|
||||||
|
|
||||||
// auto_trim, uses an auto_level algorithm
|
// auto_trim, uses an auto_level algorithm
|
||||||
auto_trim();
|
auto_trim();
|
||||||
|
|
||||||
|
// record throttle output
|
||||||
|
// ------------------------------
|
||||||
|
throttle_integrator += g.rc_3.servo_out;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// This case performs some navigation computations
|
// This case performs some navigation computations
|
||||||
|
@ -652,13 +654,6 @@ static void medium_loop()
|
||||||
loop_step = 2;
|
loop_step = 2;
|
||||||
medium_loopCounter++;
|
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:
|
// Auto control modes:
|
||||||
if(g_gps->new_data && g_gps->fix){
|
if(g_gps->new_data && g_gps->fix){
|
||||||
loop_step = 11;
|
loop_step = 11;
|
||||||
|
@ -674,8 +669,8 @@ static void medium_loop()
|
||||||
// ------------------------------------------------------
|
// ------------------------------------------------------
|
||||||
navigate();
|
navigate();
|
||||||
|
|
||||||
// control mode specific updates to nav_bearing
|
// control mode specific updates
|
||||||
// --------------------------------------------
|
// -----------------------------
|
||||||
update_navigation();
|
update_navigation();
|
||||||
|
|
||||||
if (g.log_bitmask & MASK_LOG_NTUN)
|
if (g.log_bitmask & MASK_LOG_NTUN)
|
||||||
|
@ -694,17 +689,14 @@ static void medium_loop()
|
||||||
// --------------------------
|
// --------------------------
|
||||||
update_altitude();
|
update_altitude();
|
||||||
|
|
||||||
// altitude smoothing
|
|
||||||
// ------------------
|
|
||||||
//calc_altitude_smoothing_error();
|
|
||||||
|
|
||||||
altitude_error = get_altitude_error();
|
|
||||||
|
|
||||||
//camera_stabilization();
|
|
||||||
|
|
||||||
// invalidate the throttle hold value
|
// invalidate the throttle hold value
|
||||||
// ----------------------------------
|
// ----------------------------------
|
||||||
invalid_throttle = true;
|
invalid_throttle = true;
|
||||||
|
|
||||||
|
// calc boost
|
||||||
|
// ----------
|
||||||
|
boost = get_angle_boost();
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// This case deals with sending high rate telemetry
|
// This case deals with sending high rate telemetry
|
||||||
|
@ -720,11 +712,13 @@ static void medium_loop()
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
if(motor_armed){
|
||||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
||||||
Log_Write_Attitude();
|
Log_Write_Attitude();
|
||||||
|
|
||||||
if (g.log_bitmask & MASK_LOG_CTUN && motor_armed)
|
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||||
Log_Write_Control_Tuning();
|
Log_Write_Control_Tuning();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||||||
|
@ -783,9 +777,9 @@ static void medium_loop()
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
static void fifty_hz_loop()
|
static void fifty_hz_loop()
|
||||||
{
|
{
|
||||||
// record throttle output
|
// moved to slower loop
|
||||||
// ------------------------------
|
// --------------------
|
||||||
throttle_integrator += g.rc_3.servo_out;
|
update_throttle_mode();
|
||||||
|
|
||||||
// Read Sonar
|
// Read Sonar
|
||||||
// ----------
|
// ----------
|
||||||
|
@ -798,19 +792,8 @@ static void fifty_hz_loop()
|
||||||
hil.send_message(MSG_RADIO_OUT);
|
hil.send_message(MSG_RADIO_OUT);
|
||||||
#endif
|
#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();
|
camera_stabilization();
|
||||||
|
|
||||||
|
|
||||||
//throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
|
|
||||||
|
|
||||||
# if HIL_MODE == HIL_MODE_DISABLED
|
# if HIL_MODE == HIL_MODE_DISABLED
|
||||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||||
Log_Write_Attitude();
|
Log_Write_Attitude();
|
||||||
|
@ -885,10 +868,10 @@ static void slow_loop()
|
||||||
|
|
||||||
#if AUTO_RESET_LOITER == 1
|
#if AUTO_RESET_LOITER == 1
|
||||||
if(control_mode == LOITER){
|
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
|
// reset LOITER to current position
|
||||||
//next_WP = current_loc;
|
//next_WP = current_loc;
|
||||||
}
|
//}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -918,11 +901,6 @@ static void slow_loop()
|
||||||
if(g.radio_tuning > 0)
|
if(g.radio_tuning > 0)
|
||||||
tuning();
|
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
|
#if MOTOR_LEDS == 1
|
||||||
update_motor_leds();
|
update_motor_leds();
|
||||||
#endif
|
#endif
|
||||||
|
@ -960,8 +938,16 @@ static void update_GPS(void)
|
||||||
//current_loc.lat = -1224318000; // Lat * 10 * *7
|
//current_loc.lat = -1224318000; // Lat * 10 * *7
|
||||||
//current_loc.alt = 100; // alt * 10 * *7
|
//current_loc.alt = 100; // alt * 10 * *7
|
||||||
//return;
|
//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) {
|
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
|
// XXX We should be sending GPS data off one of the regular loops so that we send
|
||||||
// no-GPS-fix data too
|
// 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)
|
void update_yaw_mode(void)
|
||||||
{
|
{
|
||||||
|
@ -1098,8 +1005,11 @@ void update_yaw_mode(void)
|
||||||
|
|
||||||
case YAW_HOLD:
|
case YAW_HOLD:
|
||||||
// calcualte new nav_yaw offset
|
// calcualte new nav_yaw offset
|
||||||
|
if (control_mode <= STABILIZE){
|
||||||
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
|
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);
|
}else{
|
||||||
|
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1);
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case YAW_LOOK_AT_HOME:
|
case YAW_LOOK_AT_HOME:
|
||||||
|
@ -1134,6 +1044,32 @@ void update_roll_pitch_mode(void)
|
||||||
|
|
||||||
int control_roll = 0, control_pitch = 0;
|
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){
|
switch(roll_pitch_mode){
|
||||||
case ROLL_PITCH_ACRO:
|
case ROLL_PITCH_ACRO:
|
||||||
|
@ -1142,76 +1078,57 @@ void update_roll_pitch_mode(void)
|
||||||
|
|
||||||
// Pitch control
|
// Pitch control
|
||||||
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
|
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
|
||||||
return;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case ROLL_PITCH_STABLE:
|
case ROLL_PITCH_STABLE:
|
||||||
control_roll = g.rc_1.control_in;
|
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||||
control_pitch = g.rc_2.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;
|
break;
|
||||||
|
|
||||||
case ROLL_PITCH_AUTO:
|
case ROLL_PITCH_AUTO:
|
||||||
// mix in user control with Nav control
|
// mix in user control with Nav control
|
||||||
control_roll = g.rc_1.control_mix(nav_roll);
|
control_roll = g.rc_1.control_mix(nav_roll);
|
||||||
control_pitch = g.rc_2.control_mix(nav_pitch);
|
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;
|
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)
|
void update_throttle_mode(void)
|
||||||
{
|
{
|
||||||
switch(throttle_mode){
|
switch(throttle_mode){
|
||||||
case THROTTLE_MANUAL:
|
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;
|
break;
|
||||||
|
|
||||||
case THROTTLE_HOLD:
|
case THROTTLE_HOLD:
|
||||||
// allow interactive changing of atitude
|
// allow interactive changing of atitude
|
||||||
adjust_altitude();
|
adjust_altitude();
|
||||||
|
|
||||||
// fall through
|
// fall through
|
||||||
|
|
||||||
case THROTTLE_AUTO:
|
case THROTTLE_AUTO:
|
||||||
|
// 10hz, don't run up i term
|
||||||
if(invalid_throttle && motor_auto_armed == true){
|
if(invalid_throttle && motor_auto_armed == true){
|
||||||
|
// how far off are we
|
||||||
|
altitude_error = get_altitude_error();
|
||||||
|
|
||||||
// get the AP throttle
|
// get the AP throttle
|
||||||
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s
|
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
|
// clear the new data flag
|
||||||
invalid_throttle = false;
|
invalid_throttle = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// apply throttle control at 200 hz
|
||||||
|
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1235,6 +1152,11 @@ static void update_navigation()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GUIDED:
|
case GUIDED:
|
||||||
|
wp_control = WP_MODE;
|
||||||
|
update_auto_yaw();
|
||||||
|
update_nav_wp();
|
||||||
|
break;
|
||||||
|
|
||||||
case RTL:
|
case RTL:
|
||||||
if(wp_distance > 4){
|
if(wp_distance > 4){
|
||||||
// calculates desired Yaw
|
// calculates desired Yaw
|
||||||
|
@ -1246,10 +1168,9 @@ static void update_navigation()
|
||||||
wp_control = WP_MODE;
|
wp_control = WP_MODE;
|
||||||
}else{
|
}else{
|
||||||
set_mode(LOITER);
|
set_mode(LOITER);
|
||||||
xtrack_enabled = false;
|
//xtrack_enabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// calculates the desired Roll and Pitch
|
// calculates the desired Roll and Pitch
|
||||||
update_nav_wp();
|
update_nav_wp();
|
||||||
break;
|
break;
|
||||||
|
@ -1269,10 +1190,10 @@ static void update_navigation()
|
||||||
update_auto_yaw();
|
update_auto_yaw();
|
||||||
{
|
{
|
||||||
//circle_angle += dTnav; //1000 * (dTnav/1000);
|
//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.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));
|
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100)));
|
||||||
}
|
}
|
||||||
|
|
||||||
// calc the lat and long error to the target
|
// calc the lat and long error to the target
|
||||||
|
@ -1298,7 +1219,7 @@ static void read_AHRS(void)
|
||||||
hil.update();
|
hil.update();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
dcm.update_DCM_fast(G_Dt);//, _tog);
|
dcm.update_DCM_fast();
|
||||||
omega = dcm.get_gyro();
|
omega = dcm.get_gyro();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1310,8 +1231,6 @@ static void update_trig(void){
|
||||||
yawvector.y = temp.b.x; // cos
|
yawvector.y = temp.b.x; // cos
|
||||||
yawvector.normalize();
|
yawvector.normalize();
|
||||||
|
|
||||||
cos_yaw_x = yawvector.y; // 0 x = north
|
|
||||||
sin_yaw_y = yawvector.x; // 1 y
|
|
||||||
|
|
||||||
sin_pitch_y = -temp.c.x;
|
sin_pitch_y = -temp.c.x;
|
||||||
cos_pitch_x = sqrt(1 - (temp.c.x * 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;
|
cos_roll_x = temp.c.z / cos_pitch_x;
|
||||||
sin_roll_y = temp.c.y / 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:
|
//flat:
|
||||||
// 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00,
|
// 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,
|
// 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00,
|
||||||
|
@ -1377,10 +1299,6 @@ static void update_altitude()
|
||||||
static void
|
static void
|
||||||
adjust_altitude()
|
adjust_altitude()
|
||||||
{
|
{
|
||||||
alt_timer++;
|
|
||||||
if(alt_timer >= 2){
|
|
||||||
alt_timer = 0;
|
|
||||||
|
|
||||||
if(g.rc_3.control_in <= 200){
|
if(g.rc_3.control_in <= 200){
|
||||||
next_WP.alt -= 1; // 1 meter per second
|
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, (current_loc.alt - 400)); // don't go less than 4 meters below current location
|
||||||
|
@ -1390,7 +1308,6 @@ adjust_altitude()
|
||||||
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
|
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
static void tuning(){
|
static void tuning(){
|
||||||
tuning_value = (float)g.rc_6.control_in / 1000.0;
|
tuning_value = (float)g.rc_6.control_in / 1000.0;
|
||||||
|
@ -1443,8 +1360,8 @@ static void tuning(){
|
||||||
|
|
||||||
case CH6_RELAY:
|
case CH6_RELAY:
|
||||||
g.rc_6.set_range(0,1000);
|
g.rc_6.set_range(0,1000);
|
||||||
if (g.rc_6.control_in <= 600) relay_on();
|
if (g.rc_6.control_in > 525) relay_on();
|
||||||
if (g.rc_6.control_in >= 400) relay_off();
|
if (g.rc_6.control_in < 475) relay_off();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case CH6_TRAVERSE_SPEED:
|
case CH6_TRAVERSE_SPEED:
|
||||||
|
@ -1462,7 +1379,6 @@ static void tuning(){
|
||||||
|
|
||||||
static void update_nav_wp()
|
static void update_nav_wp()
|
||||||
{
|
{
|
||||||
// XXX Guided mode!!!
|
|
||||||
if(wp_control == LOITER_MODE){
|
if(wp_control == LOITER_MODE){
|
||||||
|
|
||||||
// calc a pitch to the target
|
// calc a pitch to the target
|
||||||
|
|
|
@ -91,7 +91,6 @@ static int
|
||||||
get_nav_throttle(long z_error, int target_speed)
|
get_nav_throttle(long z_error, int target_speed)
|
||||||
{
|
{
|
||||||
int rate_error;
|
int rate_error;
|
||||||
int throttle;
|
|
||||||
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
|
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
|
||||||
|
|
||||||
// limit error to prevent I term run up
|
// 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 = target_speed - altitude_rate;
|
||||||
rate_error = constrain(rate_error, -110, 110);
|
rate_error = constrain(rate_error, -110, 110);
|
||||||
|
|
||||||
throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||||
return g.throttle_cruise + throttle;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static int
|
static int
|
||||||
get_rate_roll(long target_rate)
|
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.
|
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||||
// Keeps outdated data out of our calculations
|
// 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_lat.reset_I();
|
||||||
g.pi_nav_lon.reset_I();
|
g.pi_nav_lon.reset_I();
|
||||||
|
|
||||||
|
long_error = 0;
|
||||||
|
lat_error = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -169,11 +172,11 @@ throttle control
|
||||||
|
|
||||||
// user input:
|
// user input:
|
||||||
// -----------
|
// -----------
|
||||||
static int get_throttle(int throttle_input)
|
//static int get_throttle(int throttle_input)
|
||||||
{
|
//{
|
||||||
throttle_input = (float)throttle_input * angle_boost();
|
// throttle_input = (float)throttle_input * angle_boost();
|
||||||
return max(throttle_input, 0);
|
// return max(throttle_input, 0);
|
||||||
}
|
//}
|
||||||
|
|
||||||
static long
|
static long
|
||||||
get_nav_yaw_offset(int yaw_input, int reset)
|
get_nav_yaw_offset(int yaw_input, int reset)
|
||||||
|
@ -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;
|
float temp = cos_pitch_x * cos_roll_x;
|
||||||
temp = 2.0 - constrain(temp, .5, 1.0);
|
temp = 2.0 - constrain(temp, .5, 1.0);
|
||||||
|
|
|
@ -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})
|
|
@ -4,19 +4,9 @@
|
||||||
|
|
||||||
static void init_camera()
|
static void init_camera()
|
||||||
{
|
{
|
||||||
|
// ch 6 high(right) is down.
|
||||||
g.rc_camera_pitch.set_angle(4500);
|
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.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_roll.set_type(RC_CHANNEL_ANGLE_RAW);
|
||||||
g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
|
g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
|
||||||
|
|
|
@ -51,6 +51,7 @@ GCS_MAVLINK::update(void)
|
||||||
// receive new packets
|
// receive new packets
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_status_t status;
|
mavlink_status_t status;
|
||||||
|
status.packet_rx_drop_count = 0;
|
||||||
|
|
||||||
// process received bytes
|
// process received bytes
|
||||||
while(comm_get_available(chan))
|
while(comm_get_available(chan))
|
||||||
|
|
|
@ -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_3.servo_out)); // 2 bytes 4, 5
|
||||||
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
|
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
|
||||||
output_int((int)wp_distance); // 4 bytes 8,9
|
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)altitude_error); // 6 bytes 12, 13
|
||||||
output_int((int)energy_error); // 7 bytes 14,15
|
output_int((int)energy_error); // 7 bytes 14,15
|
||||||
output_byte((int)g.waypoint_index); // 8 bytes 16
|
output_byte((int)g.waypoint_index); // 8 bytes 16
|
||||||
|
@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
|
||||||
break;
|
break;
|
||||||
case MSG_LOCATION:
|
case MSG_LOCATION:
|
||||||
break;
|
break;
|
||||||
case MSG_LOCAL_LOCATION:
|
|
||||||
break;
|
|
||||||
case MSG_GPS_RAW:
|
case MSG_GPS_RAW:
|
||||||
break;
|
break;
|
||||||
case MSG_SERVO_OUT:
|
case MSG_SERVO_OUT:
|
||||||
|
|
|
@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||||
int dump_log_start;
|
int dump_log_start;
|
||||||
int dump_log_end;
|
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
|
// check that the requested log number can be read
|
||||||
dump_log = argv[1].i;
|
dump_log = argv[1].i;
|
||||||
|
|
||||||
|
@ -365,16 +362,15 @@ static void Log_Write_GPS()
|
||||||
DataFlash.WriteByte(LOG_GPS_MSG);
|
DataFlash.WriteByte(LOG_GPS_MSG);
|
||||||
|
|
||||||
DataFlash.WriteLong(g_gps->time); // 1
|
DataFlash.WriteLong(g_gps->time); // 1
|
||||||
DataFlash.WriteByte(g_gps->fix); // 2
|
DataFlash.WriteByte(g_gps->num_sats); // 2
|
||||||
DataFlash.WriteByte(g_gps->num_sats); // 3
|
|
||||||
|
|
||||||
DataFlash.WriteLong(current_loc.lat); // 4
|
DataFlash.WriteLong(current_loc.lat); // 3
|
||||||
DataFlash.WriteLong(current_loc.lng); // 5
|
DataFlash.WriteLong(current_loc.lng); // 4
|
||||||
DataFlash.WriteLong(current_loc.alt); // 7
|
DataFlash.WriteLong(current_loc.alt); // 5
|
||||||
DataFlash.WriteLong(g_gps->altitude); // 6
|
DataFlash.WriteLong(g_gps->altitude); // 6
|
||||||
|
|
||||||
DataFlash.WriteInt(g_gps->ground_speed); // 8
|
DataFlash.WriteInt(g_gps->ground_speed); // 7
|
||||||
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9
|
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8
|
||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
@ -382,21 +378,20 @@ static void Log_Write_GPS()
|
||||||
// Read a GPS packet
|
// Read a GPS packet
|
||||||
static void Log_Read_GPS()
|
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, "
|
"%4.7f, %4.7f, %4.4f, %4.4f, "
|
||||||
"%d, %u\n"),
|
"%d, %u\n"),
|
||||||
|
|
||||||
DataFlash.ReadLong(), // 1 time
|
DataFlash.ReadLong(), // 1 time
|
||||||
(int)DataFlash.ReadByte(), // 2 fix
|
(int)DataFlash.ReadByte(), // 2 sats
|
||||||
(int)DataFlash.ReadByte(), // 3 sats
|
|
||||||
|
|
||||||
(float)DataFlash.ReadLong() / t7, // 4 lat
|
(float)DataFlash.ReadLong() / t7, // 3 lat
|
||||||
(float)DataFlash.ReadLong() / t7, // 5 lon
|
(float)DataFlash.ReadLong() / t7, // 4 lon
|
||||||
(float)DataFlash.ReadLong() / 100.0, // 6 gps alt
|
(float)DataFlash.ReadLong() / 100.0, // 5 gps alt
|
||||||
(float)DataFlash.ReadLong() / 100.0, // 7 sensor alt
|
(float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
|
||||||
|
|
||||||
DataFlash.ReadInt(), // 8 ground speed
|
DataFlash.ReadInt(), // 7 ground speed
|
||||||
(uint16_t)DataFlash.ReadInt()); // 9 ground course
|
(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.dx);
|
||||||
DataFlash.WriteInt((int)optflow.dy);
|
DataFlash.WriteInt((int)optflow.dy);
|
||||||
DataFlash.WriteInt((int)optflow.surface_quality);
|
DataFlash.WriteInt((int)optflow.surface_quality);
|
||||||
DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat);
|
DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
|
||||||
DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng);
|
DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Read an attitude packet
|
|
||||||
static void Log_Read_Optflow()
|
static void Log_Read_Optflow()
|
||||||
{
|
{
|
||||||
Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
|
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.WriteInt((int)wp_distance); // 1
|
||||||
DataFlash.WriteByte(wp_verify_byte); // 2
|
DataFlash.WriteByte(wp_verify_byte); // 2
|
||||||
DataFlash.WriteInt((int)(target_bearing/100)); // 3
|
DataFlash.WriteInt((int)(target_bearing/100)); // 3
|
||||||
DataFlash.WriteInt((int)(nav_bearing/100)); // 4
|
DataFlash.WriteInt((int)long_error); // 4
|
||||||
DataFlash.WriteInt((int)long_error); // 5
|
DataFlash.WriteInt((int)lat_error); // 5
|
||||||
DataFlash.WriteInt((int)lat_error); // 6
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -564,11 +558,10 @@ static void Log_Write_Nav_Tuning()
|
||||||
|
|
||||||
static void Log_Read_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.ReadInt(), // distance
|
||||||
DataFlash.ReadByte(), // wp_verify_byte
|
DataFlash.ReadByte(), // wp_verify_byte
|
||||||
DataFlash.ReadInt(), // target_bearing
|
DataFlash.ReadInt(), // target_bearing
|
||||||
DataFlash.ReadInt(), // nav_bearing
|
|
||||||
|
|
||||||
DataFlash.ReadInt(), // long_error
|
DataFlash.ReadInt(), // long_error
|
||||||
DataFlash.ReadInt()); // lat_error
|
DataFlash.ReadInt()); // lat_error
|
||||||
|
@ -598,8 +591,6 @@ static void Log_Read_Nav_Tuning()
|
||||||
DataFlash.ReadInt(),
|
DataFlash.ReadInt(),
|
||||||
DataFlash.ReadInt()); //nav bearing
|
DataFlash.ReadInt()); //nav bearing
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -611,23 +602,18 @@ static void Log_Write_Control_Tuning()
|
||||||
DataFlash.WriteByte(HEAD_BYTE2);
|
DataFlash.WriteByte(HEAD_BYTE2);
|
||||||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
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
|
// yaw
|
||||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
|
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
|
||||||
DataFlash.WriteInt((int)(nav_yaw/100));
|
DataFlash.WriteInt((int)(nav_yaw/100)); //2
|
||||||
DataFlash.WriteInt((int)yaw_error/100);
|
DataFlash.WriteInt((int)yaw_error/100); //3
|
||||||
|
|
||||||
// Alt hold
|
// Alt hold
|
||||||
DataFlash.WriteInt(g.rc_3.servo_out);
|
DataFlash.WriteInt(g.rc_3.servo_out); //4
|
||||||
DataFlash.WriteInt(sonar_alt); //
|
DataFlash.WriteInt(sonar_alt); //5
|
||||||
DataFlash.WriteInt(baro_alt); //
|
DataFlash.WriteInt(baro_alt); //6
|
||||||
|
|
||||||
DataFlash.WriteInt((int)next_WP.alt); //
|
DataFlash.WriteInt((int)next_WP.alt); //7
|
||||||
//DataFlash.WriteInt((int)altitude_error); //
|
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
|
||||||
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
|
|
||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
@ -637,14 +623,13 @@ static void Log_Write_Control_Tuning()
|
||||||
static void Log_Read_Control_Tuning()
|
static void Log_Read_Control_Tuning()
|
||||||
{
|
{
|
||||||
Serial.printf_P(PSTR( "CTUN, "
|
Serial.printf_P(PSTR( "CTUN, "
|
||||||
//"%d, %d, "
|
|
||||||
"%d, %d, %d, "
|
"%d, %d, %d, "
|
||||||
"%d, %d, %d, "
|
"%d, %d, %d, "
|
||||||
"%d, %d, %d\n"),
|
"%d, %d\n"),
|
||||||
|
|
||||||
// Control
|
// Control
|
||||||
DataFlash.ReadInt(),
|
//DataFlash.ReadInt(),
|
||||||
DataFlash.ReadInt(),
|
//DataFlash.ReadInt(),
|
||||||
|
|
||||||
// yaw
|
// yaw
|
||||||
DataFlash.ReadInt(),
|
DataFlash.ReadInt(),
|
||||||
|
@ -672,21 +657,18 @@ static void Log_Write_Performance()
|
||||||
|
|
||||||
|
|
||||||
//*
|
//*
|
||||||
DataFlash.WriteLong( millis()- perf_mon_timer);
|
//DataFlash.WriteLong( millis()- perf_mon_timer);
|
||||||
DataFlash.WriteInt ( mainLoop_count);
|
//DataFlash.WriteInt ( mainLoop_count);
|
||||||
DataFlash.WriteInt ( G_Dt_max);
|
DataFlash.WriteInt ( G_Dt_max); //1
|
||||||
|
|
||||||
DataFlash.WriteByte( dcm.gyro_sat_count);
|
DataFlash.WriteByte( dcm.gyro_sat_count); //2
|
||||||
DataFlash.WriteByte( imu.adc_constraints);
|
DataFlash.WriteByte( imu.adc_constraints); //3
|
||||||
DataFlash.WriteByte( dcm.renorm_sqrt_count);
|
DataFlash.WriteByte( dcm.renorm_sqrt_count); //4
|
||||||
DataFlash.WriteByte( dcm.renorm_blowup_count);
|
DataFlash.WriteByte( dcm.renorm_blowup_count); //5
|
||||||
DataFlash.WriteByte( gps_fix_count);
|
DataFlash.WriteByte( gps_fix_count); //6
|
||||||
|
|
||||||
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000));
|
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7
|
||||||
DataFlash.WriteLong ( throttle_integrator);
|
DataFlash.WriteLong ( throttle_integrator); //8
|
||||||
|
|
||||||
//*/
|
|
||||||
//PM, 20005, 3742, 10,0,0,0,0,89,1000,
|
|
||||||
|
|
||||||
DataFlash.WriteByte(END_BYTE);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
@ -694,24 +676,23 @@ static void Log_Write_Performance()
|
||||||
// Read a performance packet
|
// Read a performance packet
|
||||||
static void Log_Read_Performance()
|
static void Log_Read_Performance()
|
||||||
{
|
{
|
||||||
Serial.printf_P(PSTR( "PM, %ld, %d, %d, "
|
Serial.printf_P(PSTR( "PM, %d, %d, %d, "
|
||||||
"%d, %d, %d, %d, %d, "
|
"%d, %d, %d, "
|
||||||
"%d, %ld\n"),
|
"%d, %ld\n"),
|
||||||
|
|
||||||
// Control
|
// Control
|
||||||
DataFlash.ReadLong(),
|
//DataFlash.ReadLong(),
|
||||||
DataFlash.ReadInt(),
|
//DataFlash.ReadInt(),
|
||||||
DataFlash.ReadInt(),
|
DataFlash.ReadInt(), //1
|
||||||
|
DataFlash.ReadByte(), //2
|
||||||
|
DataFlash.ReadByte(), //3
|
||||||
|
|
||||||
DataFlash.ReadByte(),
|
DataFlash.ReadByte(), //4
|
||||||
DataFlash.ReadByte(),
|
DataFlash.ReadByte(), //5
|
||||||
|
DataFlash.ReadByte(), //6
|
||||||
|
|
||||||
DataFlash.ReadByte(),
|
DataFlash.ReadInt(), //7
|
||||||
DataFlash.ReadByte(),
|
DataFlash.ReadLong()); //8
|
||||||
DataFlash.ReadByte(),
|
|
||||||
|
|
||||||
DataFlash.ReadInt(),
|
|
||||||
DataFlash.ReadLong());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Write a command processing packet.
|
// Write a command processing packet.
|
||||||
|
@ -762,19 +743,28 @@ static void Log_Write_Attitude()
|
||||||
DataFlash.WriteByte(HEAD_BYTE1);
|
DataFlash.WriteByte(HEAD_BYTE1);
|
||||||
DataFlash.WriteByte(HEAD_BYTE2);
|
DataFlash.WriteByte(HEAD_BYTE2);
|
||||||
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
|
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
|
||||||
|
|
||||||
DataFlash.WriteInt((int)dcm.roll_sensor);
|
DataFlash.WriteInt((int)dcm.roll_sensor);
|
||||||
DataFlash.WriteInt((int)dcm.pitch_sensor);
|
DataFlash.WriteInt((int)dcm.pitch_sensor);
|
||||||
DataFlash.WriteInt((uint16_t)dcm.yaw_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);
|
DataFlash.WriteByte(END_BYTE);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read an attitude packet
|
// Read an attitude packet
|
||||||
static void Log_Read_Attitude()
|
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(),
|
||||||
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
|
// Write a mode packet. Total length : 5 bytes
|
||||||
|
@ -834,8 +824,10 @@ static void Log_Read(int start_page, int end_page)
|
||||||
case 1:
|
case 1:
|
||||||
if(data == HEAD_BYTE2) // Head byte 2
|
if(data == HEAD_BYTE2) // Head byte 2
|
||||||
log_step++;
|
log_step++;
|
||||||
else
|
else{
|
||||||
log_step = 0;
|
log_step = 0;
|
||||||
|
Serial.println(".");
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
|
|
|
@ -25,36 +25,43 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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
|
#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
|
!!NOTE!!
|
||||||
// bootup, to try to prevent bricking of Xbees
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch(id) {
|
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
|
||||||
|
*/
|
||||||
|
|
||||||
case MSG_HEARTBEAT:
|
#define NOINLINE __attribute__((noinline))
|
||||||
|
|
||||||
|
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
|
||||||
mavlink_msg_heartbeat_send(
|
mavlink_msg_heartbeat_send(
|
||||||
chan,
|
chan,
|
||||||
mavlink_system.type,
|
mavlink_system.type,
|
||||||
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS1:
|
static NOINLINE void send_attitude(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
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 mode = MAV_MODE_UNINIT;
|
||||||
uint8_t nav_mode = MAV_NAV_VECTOR;
|
uint8_t nav_mode = MAV_NAV_VECTOR;
|
||||||
|
|
||||||
|
@ -76,7 +83,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
mode = control_mode + 100;
|
mode = control_mode + 100;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t status = MAV_STATE_ACTIVE;
|
uint8_t status = MAV_STATE_ACTIVE;
|
||||||
|
@ -91,89 +97,46 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
battery_voltage * 1000,
|
battery_voltage * 1000,
|
||||||
battery_remaining,
|
battery_remaining,
|
||||||
packet_drops);
|
packet_drops);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS2:
|
static void NOINLINE send_meminfo(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(MEMINFO);
|
|
||||||
extern unsigned __brkval;
|
extern unsigned __brkval;
|
||||||
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_ATTITUDE:
|
static void NOINLINE send_location(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
//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_LOCATION:
|
|
||||||
{
|
|
||||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
|
||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
||||||
mavlink_msg_global_position_int_send(
|
mavlink_msg_global_position_int_send(
|
||||||
chan,
|
chan,
|
||||||
current_loc.lat,
|
current_loc.lat,
|
||||||
current_loc.lng,
|
current_loc.lng,
|
||||||
/*current_loc.alt * 10,*/ // changed to absolute altitude
|
current_loc.alt * 10,
|
||||||
g_gps->altitude,
|
|
||||||
g_gps->ground_speed * rot.a.x,
|
g_gps->ground_speed * rot.a.x,
|
||||||
g_gps->ground_speed * rot.b.x,
|
g_gps->ground_speed * rot.b.x,
|
||||||
g_gps->ground_speed * rot.c.x);
|
g_gps->ground_speed * rot.c.x);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_NAV_CONTROLLER_OUTPUT:
|
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
//if (control_mode != MANUAL) {
|
|
||||||
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
|
||||||
mavlink_msg_nav_controller_output_send(
|
mavlink_msg_nav_controller_output_send(
|
||||||
chan,
|
chan,
|
||||||
nav_roll / 1.0e2,
|
nav_roll / 1.0e2,
|
||||||
nav_pitch / 1.0e2,
|
nav_pitch / 1.0e2,
|
||||||
nav_bearing / 1.0e2,
|
target_bearing / 1.0e2,
|
||||||
target_bearing / 1.0e2,
|
target_bearing / 1.0e2,
|
||||||
wp_distance,
|
wp_distance,
|
||||||
altitude_error / 1.0e2,
|
altitude_error / 1.0e2,
|
||||||
0,
|
0,
|
||||||
0);
|
0);
|
||||||
//}
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_LOCAL_LOCATION:
|
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
|
|
||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
||||||
mavlink_msg_local_position_send(
|
|
||||||
chan,
|
|
||||||
timeStamp,
|
|
||||||
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
|
|
||||||
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
|
|
||||||
(current_loc.alt - home.alt) / 1.0e2,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.a.x,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.b.x,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.c.x);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case MSG_GPS_RAW:
|
|
||||||
{
|
|
||||||
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
|
||||||
mavlink_msg_gps_raw_send(
|
mavlink_msg_gps_raw_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
g_gps->status(),
|
g_gps->status(),
|
||||||
g_gps->latitude / 1.0e7,
|
g_gps->latitude / 1.0e7,
|
||||||
g_gps->longitude / 1.0e7,
|
g_gps->longitude / 1.0e7,
|
||||||
|
@ -182,13 +145,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
0.0,
|
0.0,
|
||||||
g_gps->ground_speed / 100.0,
|
g_gps->ground_speed / 100.0,
|
||||||
g_gps->ground_course / 100.0);
|
g_gps->ground_course / 100.0);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_SERVO_OUT:
|
static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
const uint8_t rssi = 1;
|
||||||
uint8_t rssi = 1;
|
|
||||||
// normalized values scaled to -10000 to 10000
|
// normalized values scaled to -10000 to 10000
|
||||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||||
mavlink_msg_rc_channels_scaled_send(
|
mavlink_msg_rc_channels_scaled_send(
|
||||||
|
@ -202,13 +163,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
0,
|
0,
|
||||||
0,
|
0,
|
||||||
rssi);
|
rssi);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RADIO_IN:
|
static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
const uint8_t rssi = 1;
|
||||||
uint8_t rssi = 1;
|
|
||||||
mavlink_msg_rc_channels_raw_send(
|
mavlink_msg_rc_channels_raw_send(
|
||||||
chan,
|
chan,
|
||||||
g.rc_1.radio_in,
|
g.rc_1.radio_in,
|
||||||
|
@ -220,12 +179,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
g.rc_7.radio_in,
|
g.rc_7.radio_in,
|
||||||
g.rc_8.radio_in,
|
g.rc_8.radio_in,
|
||||||
rssi);
|
rssi);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RADIO_OUT:
|
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
|
||||||
mavlink_msg_servo_output_raw_send(
|
mavlink_msg_servo_output_raw_send(
|
||||||
chan,
|
chan,
|
||||||
motor_out[0],
|
motor_out[0],
|
||||||
|
@ -236,35 +193,28 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
motor_out[5],
|
motor_out[5],
|
||||||
motor_out[6],
|
motor_out[6],
|
||||||
motor_out[7]);
|
motor_out[7]);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_VFR_HUD:
|
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
|
||||||
mavlink_msg_vfr_hud_send(
|
mavlink_msg_vfr_hud_send(
|
||||||
chan,
|
chan,
|
||||||
(float)airspeed / 100.0,
|
(float)airspeed / 100.0,
|
||||||
(float)g_gps->ground_speed / 100.0,
|
(float)g_gps->ground_speed / 100.0,
|
||||||
(dcm.yaw_sensor / 100) % 360,
|
(dcm.yaw_sensor / 100) % 360,
|
||||||
g.rc_3.servo_out/10,
|
g.rc_3.servo_out/10,
|
||||||
/*current_loc.alt / 100.0,*/ // changed to absolute altitude
|
current_loc.alt / 100.0,
|
||||||
g_gps->altitude/100.0,
|
|
||||||
climb_rate);
|
climb_rate);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
case MSG_RAW_IMU1:
|
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
|
||||||
Vector3f accel = imu.get_accel();
|
Vector3f accel = imu.get_accel();
|
||||||
Vector3f gyro = imu.get_gyro();
|
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(
|
mavlink_msg_raw_imu_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
accel.x * 1000.0 / gravity,
|
accel.x * 1000.0 / gravity,
|
||||||
accel.y * 1000.0 / gravity,
|
accel.y * 1000.0 / gravity,
|
||||||
accel.z * 1000.0 / gravity,
|
accel.z * 1000.0 / gravity,
|
||||||
|
@ -274,24 +224,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
compass.mag_x,
|
compass.mag_x,
|
||||||
compass.mag_y,
|
compass.mag_y,
|
||||||
compass.mag_z);
|
compass.mag_z);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RAW_IMU2:
|
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
|
||||||
mavlink_msg_scaled_pressure_send(
|
mavlink_msg_scaled_pressure_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
(float)barometer.Press/100.0,
|
(float)barometer.Press/100.0,
|
||||||
(float)(barometer.Press-ground_pressure)/100.0,
|
(float)(barometer.Press-ground_pressure)/100.0,
|
||||||
(int)(barometer.Temp*10));
|
(int)(barometer.Temp*10));
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RAW_IMU3:
|
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
|
||||||
Vector3f mag_offsets = compass.get_offsets();
|
Vector3f mag_offsets = compass.get_offsets();
|
||||||
|
|
||||||
mavlink_msg_sensor_offsets_send(chan,
|
mavlink_msg_sensor_offsets_send(chan,
|
||||||
|
@ -303,13 +249,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
barometer.RawTemp,
|
barometer.RawTemp,
|
||||||
imu.gx(), imu.gy(), imu.gz(),
|
imu.gx(), imu.gy(), imu.gz(),
|
||||||
imu.ax(), imu.ay(), imu.az());
|
imu.ax(), imu.ay(), imu.az());
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
case MSG_GPS_STATUS:
|
static void NOINLINE send_gps_status(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
|
||||||
mavlink_msg_gps_status_send(
|
mavlink_msg_gps_status_send(
|
||||||
chan,
|
chan,
|
||||||
g_gps->num_sats,
|
g_gps->num_sats,
|
||||||
|
@ -318,18 +262,109 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
NULL,
|
NULL,
|
||||||
NULL,
|
NULL,
|
||||||
NULL);
|
NULL);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_CURRENT_WAYPOINT:
|
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
|
||||||
mavlink_msg_waypoint_current_send(
|
mavlink_msg_waypoint_current_send(
|
||||||
chan,
|
chan,
|
||||||
g.waypoint_index);
|
g.waypoint_index);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
|
|
||||||
|
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) {
|
||||||
|
case MSG_HEARTBEAT:
|
||||||
|
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
||||||
|
send_heartbeat(chan);
|
||||||
|
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:
|
||||||
|
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
||||||
|
send_nav_controller_output(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_GPS_RAW:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
||||||
|
send_gps_raw(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_SERVO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
||||||
|
send_servo_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_IN:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
||||||
|
send_radio_in(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
||||||
|
send_radio_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_VFR_HUD:
|
||||||
|
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
||||||
|
send_vfr_hud(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
case MSG_RAW_IMU1:
|
||||||
|
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
||||||
|
send_raw_imu1(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU2:
|
||||||
|
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
||||||
|
send_raw_imu2(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU3:
|
||||||
|
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
||||||
|
send_raw_imu3(chan);
|
||||||
|
break;
|
||||||
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
case MSG_GPS_STATUS:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
||||||
|
send_gps_status(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_CURRENT_WAYPOINT:
|
||||||
|
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
||||||
|
send_current_waypoint(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,7 +17,7 @@ public:
|
||||||
// The increment will prevent old parameters from being used incorrectly
|
// The increment will prevent old parameters from being used incorrectly
|
||||||
// by newer code.
|
// 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
|
// The parameter software_type is set up solely for ground station use
|
||||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||||
|
@ -78,12 +78,12 @@ public:
|
||||||
k_param_top_bottom_ratio,
|
k_param_top_bottom_ratio,
|
||||||
k_param_optflow_enabled,
|
k_param_optflow_enabled,
|
||||||
k_param_input_voltage,
|
k_param_input_voltage,
|
||||||
|
k_param_low_voltage,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 160: Navigation parameters
|
// 160: Navigation parameters
|
||||||
//
|
//
|
||||||
k_param_crosstrack_entry_angle = 160,
|
k_param_crosstrack_entry_angle = 160,
|
||||||
k_param_pitch_max,
|
|
||||||
k_param_RTL_altitude,
|
k_param_RTL_altitude,
|
||||||
|
|
||||||
//
|
//
|
||||||
|
@ -139,7 +139,7 @@ public:
|
||||||
k_param_flight_mode4,
|
k_param_flight_mode4,
|
||||||
k_param_flight_mode5,
|
k_param_flight_mode5,
|
||||||
k_param_flight_mode6,
|
k_param_flight_mode6,
|
||||||
|
k_param_simple_modes,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 220: Waypoint data
|
// 220: Waypoint data
|
||||||
|
@ -213,6 +213,7 @@ public:
|
||||||
AP_Int8 flight_mode4;
|
AP_Int8 flight_mode4;
|
||||||
AP_Int8 flight_mode5;
|
AP_Int8 flight_mode5;
|
||||||
AP_Int8 flight_mode6;
|
AP_Int8 flight_mode6;
|
||||||
|
AP_Int8 simple_modes;
|
||||||
|
|
||||||
// Radio settings
|
// Radio settings
|
||||||
//
|
//
|
||||||
|
@ -221,8 +222,6 @@ public:
|
||||||
//AP_Var_group pwm_throttle;
|
//AP_Var_group pwm_throttle;
|
||||||
//AP_Var_group pwm_yaw;
|
//AP_Var_group pwm_yaw;
|
||||||
|
|
||||||
AP_Int16 pitch_max;
|
|
||||||
|
|
||||||
// Misc
|
// Misc
|
||||||
//
|
//
|
||||||
AP_Int16 log_bitmask;
|
AP_Int16 log_bitmask;
|
||||||
|
@ -239,6 +238,7 @@ public:
|
||||||
AP_Float top_bottom_ratio;
|
AP_Float top_bottom_ratio;
|
||||||
AP_Int8 optflow_enabled;
|
AP_Int8 optflow_enabled;
|
||||||
AP_Float input_voltage;
|
AP_Float input_voltage;
|
||||||
|
AP_Float low_voltage;
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
// Heli
|
// Heli
|
||||||
|
@ -302,13 +302,14 @@ public:
|
||||||
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
|
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
|
||||||
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
|
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
|
||||||
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")),
|
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_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
||||||
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
||||||
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
|
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
|
||||||
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_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")),
|
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")),
|
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")),
|
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_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
|
||||||
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
|
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
|
||||||
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
|
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
|
||||||
|
simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")),
|
||||||
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
|
|
||||||
|
|
||||||
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
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")),
|
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
|
||||||
|
|
|
@ -20,18 +20,6 @@ static void clear_command_queue(){
|
||||||
next_command.id = NO_COMMAND;
|
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
|
// Getters
|
||||||
// -------
|
// -------
|
||||||
static struct Location get_command_with_index(int i)
|
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
|
// Find out proper location in memory by using the start_byte position + the index
|
||||||
// --------------------------------------------------------------------------------
|
// --------------------------------------------------------------------------------
|
||||||
if (i > g.waypoint_total) {
|
if (i > g.waypoint_total) {
|
||||||
Serial.println("XCD");
|
|
||||||
|
|
||||||
// we do not have a valid command to load
|
// we do not have a valid command to load
|
||||||
// return a WP with a "Blank" id
|
// return a WP with a "Blank" id
|
||||||
temp.id = CMD_BLANK;
|
temp.id = CMD_BLANK;
|
||||||
|
@ -51,7 +37,6 @@ static struct Location get_command_with_index(int i)
|
||||||
return temp;
|
return temp;
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
//Serial.println("LD");
|
|
||||||
// we can load a command, we don't process it yet
|
// we can load a command, we don't process it yet
|
||||||
// read WP position
|
// read WP position
|
||||||
long mem = (WP_START_BYTE) + (i * WP_SIZE);
|
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
|
// 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;
|
//temp.alt += home.alt;
|
||||||
}
|
//}
|
||||||
//Serial.println("ADD ALT");
|
|
||||||
|
|
||||||
if(temp.options & WP_OPTION_RELATIVE){
|
if(temp.options & WP_OPTION_RELATIVE){
|
||||||
// If were relative, just offset from home
|
// If were relative, just offset from home
|
||||||
|
@ -190,7 +174,6 @@ static void set_next_WP(struct Location *wp)
|
||||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||||
wp_distance = wp_totalDistance;
|
wp_distance = wp_totalDistance;
|
||||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||||
nav_bearing = target_bearing;
|
|
||||||
|
|
||||||
// to check if we have missed the WP
|
// 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
|
// 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();
|
gcs.print_current_waypoints();
|
||||||
}
|
}
|
||||||
|
@ -218,14 +201,12 @@ static void init_home()
|
||||||
home.lng = g_gps->longitude; // Lon * 10**7
|
home.lng = g_gps->longitude; // Lon * 10**7
|
||||||
home.lat = g_gps->latitude; // Lat * 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 = 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;
|
home_is_set = true;
|
||||||
|
|
||||||
// to point yaw towards home until we set it with Mavlink
|
// to point yaw towards home until we set it with Mavlink
|
||||||
target_WP = home;
|
target_WP = home;
|
||||||
|
|
||||||
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
|
|
||||||
|
|
||||||
// Save Home to EEPROM
|
// Save Home to EEPROM
|
||||||
// -------------------
|
// -------------------
|
||||||
// no need to save this to EPROM
|
// 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
|
// Save prev loc this makes the calcs look better before commands are loaded
|
||||||
prev_WP = home;
|
prev_WP = home;
|
||||||
|
|
||||||
// this is dangerous since we can get GPS lock at any time.
|
// this is dangerous since we can get GPS lock at any time.
|
||||||
//next_WP = home;
|
//next_WP = home;
|
||||||
|
|
||||||
|
// Load home for a default guided_WP
|
||||||
|
// -------------
|
||||||
|
guided_WP = home;
|
||||||
|
guided_WP.alt += g.RTL_altitude;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -105,17 +105,14 @@ static void handle_process_now()
|
||||||
|
|
||||||
static void handle_no_commands()
|
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){
|
switch (control_mode){
|
||||||
default:
|
default:
|
||||||
//set_mode(RTL);
|
set_mode(RTL);
|
||||||
break;
|
break;
|
||||||
}
|
}*/
|
||||||
return;
|
//return;
|
||||||
*/
|
//Serial.println("Handle No CMDs");
|
||||||
Serial.println("Handle No CMDs");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
@ -314,13 +311,10 @@ static void do_loiter_turns()
|
||||||
|
|
||||||
static void do_loiter_time()
|
static void do_loiter_time()
|
||||||
{
|
{
|
||||||
///*
|
|
||||||
wp_control = LOITER_MODE;
|
wp_control = LOITER_MODE;
|
||||||
set_next_WP(¤t_loc);
|
set_next_WP(¤t_loc);
|
||||||
loiter_time = millis();
|
loiter_time = millis();
|
||||||
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
|
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;
|
Location temp = next_WP;
|
||||||
condition_start = current_loc.alt;
|
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;
|
condition_value = next_command.alt;
|
||||||
}
|
temp.alt = next_command.alt;
|
||||||
temp.alt = condition_value;
|
|
||||||
set_next_WP(&temp);
|
set_next_WP(&temp);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -210,6 +210,8 @@
|
||||||
//#define OPTFLOW_ENABLED
|
//#define OPTFLOW_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
//#define OPTFLOW_ENABLED
|
||||||
|
|
||||||
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
|
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
|
||||||
# define OPTFLOW DISABLED
|
# define OPTFLOW DISABLED
|
||||||
#endif
|
#endif
|
||||||
|
@ -310,7 +312,7 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef SIMPLE_RP
|
#ifndef SIMPLE_RP
|
||||||
# define SIMPLE_RP ROLL_PITCH_SIMPLE
|
# define SIMPLE_RP ROLL_PITCH_STABLE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef SIMPLE_THR
|
#ifndef SIMPLE_THR
|
||||||
|
@ -390,7 +392,7 @@
|
||||||
// Attitude Control
|
// Attitude Control
|
||||||
//
|
//
|
||||||
#ifndef STABILIZE_ROLL_P
|
#ifndef STABILIZE_ROLL_P
|
||||||
# define STABILIZE_ROLL_P 3.6
|
# define STABILIZE_ROLL_P 4.0
|
||||||
#endif
|
#endif
|
||||||
#ifndef STABILIZE_ROLL_I
|
#ifndef STABILIZE_ROLL_I
|
||||||
# define STABILIZE_ROLL_I 0.02
|
# define STABILIZE_ROLL_I 0.02
|
||||||
|
@ -400,7 +402,7 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef STABILIZE_PITCH_P
|
#ifndef STABILIZE_PITCH_P
|
||||||
# define STABILIZE_PITCH_P 3.6
|
# define STABILIZE_PITCH_P 4.0
|
||||||
#endif
|
#endif
|
||||||
#ifndef STABILIZE_PITCH_I
|
#ifndef STABILIZE_PITCH_I
|
||||||
# define STABILIZE_PITCH_I 0.02
|
# define STABILIZE_PITCH_I 0.02
|
||||||
|
@ -413,7 +415,7 @@
|
||||||
// Rate Control
|
// Rate Control
|
||||||
//
|
//
|
||||||
#ifndef RATE_ROLL_P
|
#ifndef RATE_ROLL_P
|
||||||
# define RATE_ROLL_P .13
|
# define RATE_ROLL_P 0.13
|
||||||
#endif
|
#endif
|
||||||
#ifndef RATE_ROLL_I
|
#ifndef RATE_ROLL_I
|
||||||
# define RATE_ROLL_I 0.0
|
# define RATE_ROLL_I 0.0
|
||||||
|
@ -472,7 +474,7 @@
|
||||||
# define LOITER_P .4 //
|
# define LOITER_P .4 //
|
||||||
#endif
|
#endif
|
||||||
#ifndef LOITER_I
|
#ifndef LOITER_I
|
||||||
# define LOITER_I 0.01 //
|
# define LOITER_I 0.04 //
|
||||||
#endif
|
#endif
|
||||||
#ifndef LOITER_IMAX
|
#ifndef LOITER_IMAX
|
||||||
# define LOITER_IMAX 12 // degrees°
|
# define LOITER_IMAX 12 // degrees°
|
||||||
|
@ -482,28 +484,12 @@
|
||||||
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
|
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_I
|
#ifndef NAV_I
|
||||||
# define NAV_I 0.1 // this
|
# define NAV_I 0.15 // this
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_IMAX
|
#ifndef NAV_IMAX
|
||||||
# define NAV_IMAX 16 // degrees
|
# define NAV_IMAX 20 // degrees
|
||||||
#endif
|
#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
|
#ifndef WAYPOINT_SPEED_MAX
|
||||||
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph
|
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph
|
||||||
#endif
|
#endif
|
||||||
|
@ -518,9 +504,6 @@
|
||||||
#ifndef THROTTLE_I
|
#ifndef THROTTLE_I
|
||||||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||||
#endif
|
#endif
|
||||||
//#ifndef THROTTLE_D
|
|
||||||
//# define THROTTLE_D 0.6 // upped with filter
|
|
||||||
//#endif
|
|
||||||
#ifndef THROTTLE_IMAX
|
#ifndef THROTTLE_IMAX
|
||||||
# define THROTTLE_IMAX 300
|
# define THROTTLE_IMAX 300
|
||||||
#endif
|
#endif
|
||||||
|
@ -579,7 +562,7 @@
|
||||||
# define LOG_ATTITUDE_FAST DISABLED
|
# define LOG_ATTITUDE_FAST DISABLED
|
||||||
#endif
|
#endif
|
||||||
#ifndef LOG_ATTITUDE_MED
|
#ifndef LOG_ATTITUDE_MED
|
||||||
# define LOG_ATTITUDE_MED DISABLED
|
# define LOG_ATTITUDE_MED ENABLED
|
||||||
#endif
|
#endif
|
||||||
#ifndef LOG_GPS
|
#ifndef LOG_GPS
|
||||||
# define LOG_GPS ENABLED
|
# define LOG_GPS ENABLED
|
||||||
|
@ -671,8 +654,8 @@
|
||||||
# define WP_RADIUS_DEFAULT 3
|
# define WP_RADIUS_DEFAULT 3
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef LOITER_RADIUS_DEFAULT
|
#ifndef LOITER_RADIUS
|
||||||
# define LOITER_RADIUS_DEFAULT 10
|
# define LOITER_RADIUS 10
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef ALT_HOLD_HOME
|
#ifndef ALT_HOLD_HOME
|
||||||
|
|
|
@ -13,6 +13,15 @@ static void read_control_switch()
|
||||||
switch_debouncer = false;
|
switch_debouncer = false;
|
||||||
|
|
||||||
set_mode(flight_modes[switchPosition]);
|
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{
|
}else{
|
||||||
switch_debouncer = true;
|
switch_debouncer = true;
|
||||||
}
|
}
|
||||||
|
@ -47,6 +56,11 @@ static void read_trim_switch()
|
||||||
do_flip = true;
|
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
|
#elif CH7_OPTION == DO_SET_HOVER
|
||||||
// switch is engaged
|
// switch is engaged
|
||||||
if (g.rc_7.control_in > 800){
|
if (g.rc_7.control_in > 800){
|
||||||
|
|
|
@ -14,8 +14,7 @@
|
||||||
|
|
||||||
#define ROLL_PITCH_STABLE 0
|
#define ROLL_PITCH_STABLE 0
|
||||||
#define ROLL_PITCH_ACRO 1
|
#define ROLL_PITCH_ACRO 1
|
||||||
#define ROLL_PITCH_SIMPLE 2
|
#define ROLL_PITCH_AUTO 2
|
||||||
#define ROLL_PITCH_AUTO 3
|
|
||||||
|
|
||||||
#define THROTTLE_MANUAL 0
|
#define THROTTLE_MANUAL 0
|
||||||
#define THROTTLE_HOLD 1
|
#define THROTTLE_HOLD 1
|
||||||
|
@ -30,6 +29,7 @@
|
||||||
// CH 7 control
|
// CH 7 control
|
||||||
#define DO_SET_HOVER 0
|
#define DO_SET_HOVER 0
|
||||||
#define DO_FLIP 1
|
#define DO_FLIP 1
|
||||||
|
#define SIMPLE_MODE_CONTROL 2
|
||||||
|
|
||||||
// Frame types
|
// Frame types
|
||||||
#define QUAD_FRAME 0
|
#define QUAD_FRAME 0
|
||||||
|
@ -83,21 +83,6 @@
|
||||||
#define MAX_SONAR_UNKNOWN 0
|
#define MAX_SONAR_UNKNOWN 0
|
||||||
#define MAX_SONAR_XL 1
|
#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_ROLL CH_1
|
||||||
#define CH_PITCH CH_2
|
#define CH_PITCH CH_2
|
||||||
#define CH_THROTTLE CH_3
|
#define CH_THROTTLE CH_3
|
||||||
|
@ -129,14 +114,20 @@
|
||||||
// ----------------
|
// ----------------
|
||||||
#define STABILIZE 0 // hold level position
|
#define STABILIZE 0 // hold level position
|
||||||
#define ACRO 1 // rate control
|
#define ACRO 1 // rate control
|
||||||
#define SIMPLE 2 //
|
#define ALT_HOLD 2 // AUTO control
|
||||||
#define ALT_HOLD 3 // AUTO control
|
#define AUTO 3 // AUTO control
|
||||||
#define AUTO 4 // AUTO control
|
#define GUIDED 4 // AUTO control
|
||||||
#define GUIDED 5 // AUTO control
|
#define LOITER 5 // Hold a single location
|
||||||
#define LOITER 6 // Hold a single location
|
#define RTL 6 // AUTO control
|
||||||
#define RTL 7 // AUTO control
|
#define CIRCLE 7 // AUTO control
|
||||||
#define CIRCLE 8 // AUTO control
|
#define NUM_MODES 8
|
||||||
#define NUM_MODES 9
|
|
||||||
|
#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
|
// CH_6 Tuning
|
||||||
// -----------
|
// -----------
|
||||||
|
@ -252,7 +243,6 @@
|
||||||
#define MSG_ATTITUDE_CORRECT 0xb1
|
#define MSG_ATTITUDE_CORRECT 0xb1
|
||||||
#define MSG_POSITION_SET 0xb2
|
#define MSG_POSITION_SET 0xb2
|
||||||
#define MSG_ATTITUDE_SET 0xb3
|
#define MSG_ATTITUDE_SET 0xb3
|
||||||
#define MSG_LOCAL_LOCATION 0xb4
|
|
||||||
#define MSG_RETRY_DEFERRED 0xff
|
#define MSG_RETRY_DEFERRED 0xff
|
||||||
|
|
||||||
#define SEVERITY_LOW 1
|
#define SEVERITY_LOW 1
|
||||||
|
|
|
@ -96,6 +96,8 @@ static void clear_leds()
|
||||||
#if MOTOR_LEDS == 1
|
#if MOTOR_LEDS == 1
|
||||||
static void update_motor_leds(void)
|
static void update_motor_leds(void)
|
||||||
{
|
{
|
||||||
|
if (motor_armed == true){
|
||||||
|
if (low_batt == true){
|
||||||
// blink rear
|
// blink rear
|
||||||
static bool blink = false;
|
static bool blink = false;
|
||||||
|
|
||||||
|
@ -110,12 +112,19 @@ static void update_motor_leds(void)
|
||||||
digitalWrite(RI_LED, HIGH);
|
digitalWrite(RI_LED, HIGH);
|
||||||
digitalWrite(LE_LED, HIGH);
|
digitalWrite(LE_LED, HIGH);
|
||||||
}
|
}
|
||||||
|
|
||||||
blink = !blink;
|
blink = !blink;
|
||||||
|
}else{
|
||||||
// the variable low_batt is here to let people know the voltage is low or the pack capacity is finished
|
digitalWrite(RE_LED, HIGH);
|
||||||
// I don't know what folks want here.
|
digitalWrite(FR_LED, HIGH);
|
||||||
// low_batt
|
digitalWrite(RI_LED, HIGH);
|
||||||
|
digitalWrite(LE_LED, HIGH);
|
||||||
|
}
|
||||||
|
}else {
|
||||||
|
digitalWrite(RE_LED, LOW);
|
||||||
|
digitalWrite(FR_LED, LOW);
|
||||||
|
digitalWrite(RI_LED, LOW);
|
||||||
|
digitalWrite(LE_LED, LOW);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
/// -*- 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 -*-
|
||||||
|
|
||||||
#define ARM_DELAY 10 // one secon
|
#define ARM_DELAY 10 // one second
|
||||||
#define DISARM_DELAY 10 // one secon
|
#define DISARM_DELAY 10 // one second
|
||||||
#define LEVEL_DELAY 120 // twelve seconds
|
#define LEVEL_DELAY 120 // twelve seconds
|
||||||
#define AUTO_LEVEL_DELAY 150 // twentyfive seconds
|
#define AUTO_LEVEL_DELAY 150 // twentyfive seconds
|
||||||
|
|
||||||
|
|
|
@ -138,21 +138,21 @@ static void output_motor_test()
|
||||||
// 31
|
// 31
|
||||||
// 24
|
// 24
|
||||||
if(g.rc_1.control_in > 3000){ // right
|
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
|
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
|
if(g.rc_2.control_in > 3000){ // back
|
||||||
motor_out[CH_8] += 50;
|
motor_out[CH_8] += 100;
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_2.control_in < -3000){ // front
|
if(g.rc_2.control_in < -3000){ // front
|
||||||
motor_out[CH_7] += 50;
|
motor_out[CH_7] += 100;
|
||||||
motor_out[CH_3] += 50;
|
motor_out[CH_3] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
|
@ -160,21 +160,21 @@ static void output_motor_test()
|
||||||
// 2 1
|
// 2 1
|
||||||
// 4
|
// 4
|
||||||
if(g.rc_1.control_in > 3000){ // right
|
if(g.rc_1.control_in > 3000){ // right
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
motor_out[CH_8] += 50;
|
motor_out[CH_8] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_1.control_in < -3000){ // left
|
if(g.rc_1.control_in < -3000){ // left
|
||||||
motor_out[CH_7] += 50;
|
motor_out[CH_7] += 100;
|
||||||
motor_out[CH_3] += 50;
|
motor_out[CH_3] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_2.control_in > 3000){ // back
|
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
|
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_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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
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);
|
delay(1000);
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -181,39 +181,77 @@ static void output_motors_disarmed()
|
||||||
}
|
}
|
||||||
|
|
||||||
static void output_motor_test()
|
static void output_motor_test()
|
||||||
|
{
|
||||||
|
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_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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_1, 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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 50);
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_11, 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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_4, 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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_2, 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);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 50);
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
APM_RC.OutputCh(CH_10, 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);
|
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
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -149,8 +149,37 @@ static void output_motors_disarmed()
|
||||||
|
|
||||||
static void output_motor_test()
|
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
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -4,6 +4,7 @@
|
||||||
|
|
||||||
static void output_motors_armed()
|
static void output_motors_armed()
|
||||||
{
|
{
|
||||||
|
|
||||||
int roll_out, pitch_out;
|
int roll_out, pitch_out;
|
||||||
int out_min = g.rc_3.radio_min;
|
int out_min = g.rc_3.radio_min;
|
||||||
int out_max = g.rc_3.radio_max;
|
int out_max = g.rc_3.radio_max;
|
||||||
|
@ -139,23 +140,23 @@ static void output_motor_test()
|
||||||
// 31
|
// 31
|
||||||
// 24
|
// 24
|
||||||
if(g.rc_1.control_in > 3000){
|
if(g.rc_1.control_in > 3000){
|
||||||
motor_out[CH_1] += 50;
|
motor_out[CH_1] += 100;
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_1.control_in < -3000){
|
if(g.rc_1.control_in < -3000){
|
||||||
motor_out[CH_2] += 50;
|
motor_out[CH_2] += 100;
|
||||||
motor_out[CH_3] += 50;
|
motor_out[CH_3] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_2.control_in > 3000){
|
if(g.rc_2.control_in > 3000){
|
||||||
motor_out[CH_2] += 50;
|
motor_out[CH_2] += 100;
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_2.control_in < -3000){
|
if(g.rc_2.control_in < -3000){
|
||||||
motor_out[CH_1] += 50;
|
motor_out[CH_1] += 100;
|
||||||
motor_out[CH_3] += 50;
|
motor_out[CH_3] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
|
@ -163,16 +164,16 @@ static void output_motor_test()
|
||||||
// 2 1
|
// 2 1
|
||||||
// 4
|
// 4
|
||||||
if(g.rc_1.control_in > 3000)
|
if(g.rc_1.control_in > 3000)
|
||||||
motor_out[CH_1] += 50;
|
motor_out[CH_1] += 100;
|
||||||
|
|
||||||
if(g.rc_1.control_in < -3000)
|
if(g.rc_1.control_in < -3000)
|
||||||
motor_out[CH_2] += 50;
|
motor_out[CH_2] += 100;
|
||||||
|
|
||||||
if(g.rc_2.control_in > 3000)
|
if(g.rc_2.control_in > 3000)
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
|
|
||||||
if(g.rc_2.control_in < -3000)
|
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]);
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||||
|
|
|
@ -96,15 +96,15 @@ static void output_motor_test()
|
||||||
|
|
||||||
|
|
||||||
if(g.rc_1.control_in > 3000){ // right
|
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
|
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
|
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]);
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||||
|
|
|
@ -144,18 +144,18 @@ static void output_motor_test()
|
||||||
|
|
||||||
|
|
||||||
if(g.rc_1.control_in > 3000){ // right
|
if(g.rc_1.control_in > 3000){ // right
|
||||||
motor_out[CH_1] += 50;
|
motor_out[CH_1] += 100;
|
||||||
motor_out[CH_7] += 50;
|
motor_out[CH_7] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_1.control_in < -3000){ // left
|
if(g.rc_1.control_in < -3000){ // left
|
||||||
motor_out[CH_2] += 50;
|
motor_out[CH_2] += 100;
|
||||||
motor_out[CH_3] += 50;
|
motor_out[CH_3] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(g.rc_2.control_in > 3000){ // back
|
if(g.rc_2.control_in > 3000){ // back
|
||||||
motor_out[CH_8] += 50;
|
motor_out[CH_8] += 100;
|
||||||
motor_out[CH_4] += 50;
|
motor_out[CH_4] += 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||||
|
|
|
@ -30,15 +30,6 @@ static void navigate()
|
||||||
// target_bearing is where we should be heading
|
// target_bearing is where we should be heading
|
||||||
// --------------------------------------------
|
// --------------------------------------------
|
||||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||||
|
|
||||||
// nav_bearing will include xtrac correction
|
|
||||||
// -----------------------------------------
|
|
||||||
//xtrack_enabled = false;
|
|
||||||
if(xtrack_enabled){
|
|
||||||
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction());
|
|
||||||
}else{
|
|
||||||
nav_bearing = target_bearing;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool check_missed_wp()
|
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
|
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
|
#define NAV_ERR_MAX 400
|
||||||
static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
|
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:
|
// find the rates:
|
||||||
|
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
|
// 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));
|
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 = 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);
|
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());
|
//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 = 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);
|
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;
|
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()
|
static long get_altitude_error()
|
||||||
{
|
{
|
||||||
return next_WP.alt - current_loc.alt;
|
return next_WP.alt - current_loc.alt;
|
||||||
|
@ -189,6 +188,7 @@ static long wrap_180(long error)
|
||||||
return error;
|
return error;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
static long get_crosstrack_correction(void)
|
static long get_crosstrack_correction(void)
|
||||||
{
|
{
|
||||||
// Crosstrack Error
|
// Crosstrack Error
|
||||||
|
@ -206,19 +206,20 @@ static long get_crosstrack_correction(void)
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
/*
|
||||||
static long cross_track_test()
|
static long cross_track_test()
|
||||||
{
|
{
|
||||||
long temp = wrap_180(target_bearing - crosstrack_bearing);
|
long temp = wrap_180(target_bearing - crosstrack_bearing);
|
||||||
return abs(temp);
|
return abs(temp);
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
/*
|
||||||
static void reset_crosstrack()
|
static void reset_crosstrack()
|
||||||
{
|
{
|
||||||
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
static long get_altitude_above_home(void)
|
static long get_altitude_above_home(void)
|
||||||
{
|
{
|
||||||
// This is the altitude above the home location
|
// This is the altitude above the home location
|
||||||
|
|
|
@ -101,7 +101,7 @@ void output_min()
|
||||||
static void read_radio()
|
static void read_radio()
|
||||||
{
|
{
|
||||||
if (APM_RC.GetState() == 1){
|
if (APM_RC.GetState() == 1){
|
||||||
|
new_radio_frame = true;
|
||||||
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
||||||
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
||||||
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
||||||
|
|
|
@ -113,17 +113,20 @@ static void read_battery(void)
|
||||||
|
|
||||||
if(g.battery_monitoring == 1)
|
if(g.battery_monitoring == 1)
|
||||||
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
|
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
|
||||||
|
|
||||||
if(g.battery_monitoring == 2)
|
if(g.battery_monitoring == 2)
|
||||||
battery_voltage = battery_voltage4;
|
battery_voltage = battery_voltage4;
|
||||||
|
|
||||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
|
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
|
||||||
battery_voltage = battery_voltage1;
|
battery_voltage = battery_voltage1;
|
||||||
|
|
||||||
if(g.battery_monitoring == 4) {
|
if(g.battery_monitoring == 4) {
|
||||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
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;
|
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if BATTERY_EVENT == 1
|
#if BATTERY_EVENT == 1
|
||||||
if(battery_voltage < LOW_VOLTAGE)
|
if(battery_voltage < g.low_voltage)
|
||||||
low_battery_event();
|
low_battery_event();
|
||||||
|
|
||||||
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)
|
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)
|
||||||
|
|
|
@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||||
report_flight_modes();
|
report_flight_modes();
|
||||||
report_imu();
|
report_imu();
|
||||||
report_compass();
|
report_compass();
|
||||||
|
|
||||||
#ifdef OPTFLOW_ENABLED
|
#ifdef OPTFLOW_ENABLED
|
||||||
report_optflow();
|
report_optflow();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
report_heli();
|
report_heli();
|
||||||
report_gyro();
|
report_gyro();
|
||||||
|
@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||||
byte _oldSwitchPosition = 0;
|
byte _oldSwitchPosition = 0;
|
||||||
byte mode = 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();
|
print_hit_enter();
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
|
@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||||
mode = constrain(mode, 0, NUM_MODES-1);
|
mode = constrain(mode, 0, NUM_MODES-1);
|
||||||
|
|
||||||
// update the user
|
// update the user
|
||||||
print_switch(_switchPosition, mode);
|
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
||||||
|
|
||||||
// Remember switch position
|
// Remember switch position
|
||||||
_oldSwitchPosition = _switchPosition;
|
_oldSwitchPosition = _switchPosition;
|
||||||
}
|
}
|
||||||
|
|
||||||
// look for stick input
|
// look for stick input
|
||||||
if (radio_input_switch() == true){
|
if (abs(g.rc_1.control_in) > 3000){
|
||||||
mode++;
|
mode++;
|
||||||
if(mode >= NUM_MODES)
|
if(mode >= NUM_MODES)
|
||||||
mode = 0;
|
mode = 0;
|
||||||
|
@ -334,13 +336,32 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||||
flight_modes[_switchPosition] = mode;
|
flight_modes[_switchPosition] = mode;
|
||||||
|
|
||||||
// print new 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
|
// escape hatch
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
for (mode = 0; mode < 6; mode++)
|
for (mode = 0; mode < 6; mode++)
|
||||||
flight_modes[mode].save();
|
flight_modes[mode].save();
|
||||||
|
|
||||||
|
g.simple_modes.save();
|
||||||
print_done();
|
print_done();
|
||||||
report_flight_modes();
|
report_flight_modes();
|
||||||
return (0);
|
return (0);
|
||||||
|
@ -740,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
|
||||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||||
g.optflow_enabled = false;
|
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{
|
}else{
|
||||||
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
||||||
report_optflow();
|
report_optflow();
|
||||||
|
@ -872,7 +889,7 @@ static void report_flight_modes()
|
||||||
print_divider();
|
print_divider();
|
||||||
|
|
||||||
for(int i = 0; i < 6; i++ ){
|
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);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
@ -956,10 +973,15 @@ print_radio_values()
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
static void
|
||||||
print_switch(byte p, byte m)
|
print_switch(byte p, byte m, bool b)
|
||||||
{
|
{
|
||||||
Serial.printf_P(PSTR("Pos %d: "),p);
|
Serial.printf_P(PSTR("Pos %d:\t"),p);
|
||||||
Serial.println(flight_mode_strings[m]);
|
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
|
static void
|
||||||
|
@ -968,32 +990,6 @@ print_done()
|
||||||
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
|
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)
|
static void zero_eeprom(void)
|
||||||
{
|
{
|
||||||
|
|
|
@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// 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
|
#endif // CLI_ENABLED
|
||||||
|
|
||||||
|
@ -73,8 +73,8 @@ static void init_ardupilot()
|
||||||
Serial1.begin(38400, 128, 16);
|
Serial1.begin(38400, 128, 16);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
Serial.printf_P(PSTR("\n\nInit ACM"
|
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||||
"\n\nRAM: %lu\n"),
|
"\n\nFree RAM: %lu\n"),
|
||||||
freeRAM());
|
freeRAM());
|
||||||
|
|
||||||
|
|
||||||
|
@ -249,11 +249,6 @@ static void init_ardupilot()
|
||||||
start_new_log();
|
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;
|
GPS_enabled = false;
|
||||||
|
|
||||||
// Read in the GPS
|
// Read in the GPS
|
||||||
|
@ -292,10 +287,8 @@ static void init_ardupilot()
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
reset_control_switch();
|
reset_control_switch();
|
||||||
|
|
||||||
//delay(100);
|
|
||||||
startup_ground();
|
startup_ground();
|
||||||
|
|
||||||
//Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max);
|
|
||||||
Log_Write_Startup();
|
Log_Write_Startup();
|
||||||
|
|
||||||
SendDebug("\nReady to FLY ");
|
SendDebug("\nReady to FLY ");
|
||||||
|
@ -312,8 +305,10 @@ static void startup_ground(void)
|
||||||
// Warm up and read Gyro offsets
|
// Warm up and read Gyro offsets
|
||||||
// -----------------------------
|
// -----------------------------
|
||||||
imu.init_gyro(mavlink_delay);
|
imu.init_gyro(mavlink_delay);
|
||||||
|
#if CLI_ENABLED == ENABLED
|
||||||
report_imu();
|
report_imu();
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
// reset the leds
|
// reset the leds
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
|
@ -328,8 +323,7 @@ static void startup_ground(void)
|
||||||
|
|
||||||
#define ROLL_PITCH_STABLE 0
|
#define ROLL_PITCH_STABLE 0
|
||||||
#define ROLL_PITCH_ACRO 1
|
#define ROLL_PITCH_ACRO 1
|
||||||
#define ROLL_PITCH_SIMPLE 2
|
#define ROLL_PITCH_AUTO 2
|
||||||
#define ROLL_PITCH_AUTO 3
|
|
||||||
|
|
||||||
#define THROTTLE_MANUAL 0
|
#define THROTTLE_MANUAL 0
|
||||||
#define THROTTLE_HOLD 1
|
#define THROTTLE_HOLD 1
|
||||||
|
@ -361,9 +355,7 @@ static void set_mode(byte mode)
|
||||||
// report the GPS and Motor arming status
|
// report the GPS and Motor arming status
|
||||||
led_mode = NORMAL_LEDS;
|
led_mode = NORMAL_LEDS;
|
||||||
|
|
||||||
// most modes do not calculate crosstrack correction
|
reset_nav();
|
||||||
xtrack_enabled = false;
|
|
||||||
reset_nav_I();
|
|
||||||
|
|
||||||
switch(control_mode)
|
switch(control_mode)
|
||||||
{
|
{
|
||||||
|
@ -381,13 +373,6 @@ static void set_mode(byte mode)
|
||||||
reset_hold_I();
|
reset_hold_I();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SIMPLE:
|
|
||||||
yaw_mode = SIMPLE_YAW;
|
|
||||||
roll_pitch_mode = SIMPLE_RP;
|
|
||||||
throttle_mode = SIMPLE_THR;
|
|
||||||
reset_hold_I();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case ALT_HOLD:
|
case ALT_HOLD:
|
||||||
yaw_mode = ALT_HOLD_YAW;
|
yaw_mode = ALT_HOLD_YAW;
|
||||||
roll_pitch_mode = ALT_HOLD_RP;
|
roll_pitch_mode = ALT_HOLD_RP;
|
||||||
|
@ -395,7 +380,7 @@ static void set_mode(byte mode)
|
||||||
reset_hold_I();
|
reset_hold_I();
|
||||||
|
|
||||||
init_throttle_cruise();
|
init_throttle_cruise();
|
||||||
next_WP.alt = current_loc.alt;
|
next_WP = current_loc;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AUTO:
|
case AUTO:
|
||||||
|
@ -407,11 +392,7 @@ static void set_mode(byte mode)
|
||||||
init_throttle_cruise();
|
init_throttle_cruise();
|
||||||
|
|
||||||
// loads the commands from where we left off
|
// loads the commands from where we left off
|
||||||
init_auto();
|
init_commands();
|
||||||
|
|
||||||
// do crosstrack correction
|
|
||||||
// XXX move to flight commands
|
|
||||||
xtrack_enabled = true;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case CIRCLE:
|
case CIRCLE:
|
||||||
|
@ -437,9 +418,10 @@ static void set_mode(byte mode)
|
||||||
roll_pitch_mode = ROLL_PITCH_AUTO;
|
roll_pitch_mode = ROLL_PITCH_AUTO;
|
||||||
throttle_mode = THROTTLE_AUTO;
|
throttle_mode = THROTTLE_AUTO;
|
||||||
|
|
||||||
xtrack_enabled = true;
|
//xtrack_enabled = true;
|
||||||
init_throttle_cruise();
|
init_throttle_cruise();
|
||||||
next_WP = current_loc;
|
next_WP = current_loc;
|
||||||
|
set_next_WP(&guided_WP);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case RTL:
|
case RTL:
|
||||||
|
@ -447,7 +429,7 @@ static void set_mode(byte mode)
|
||||||
roll_pitch_mode = RTL_RP;
|
roll_pitch_mode = RTL_RP;
|
||||||
throttle_mode = RTL_THR;
|
throttle_mode = RTL_THR;
|
||||||
|
|
||||||
xtrack_enabled = true;
|
//xtrack_enabled = true;
|
||||||
init_throttle_cruise();
|
init_throttle_cruise();
|
||||||
do_RTL();
|
do_RTL();
|
||||||
break;
|
break;
|
||||||
|
@ -489,7 +471,7 @@ static void set_failsafe(boolean mode)
|
||||||
|
|
||||||
|
|
||||||
static void resetPerfData(void) {
|
static void resetPerfData(void) {
|
||||||
mainLoop_count = 0;
|
//mainLoop_count = 0;
|
||||||
G_Dt_max = 0;
|
G_Dt_max = 0;
|
||||||
gps_fix_count = 0;
|
gps_fix_count = 0;
|
||||||
perf_mon_timer = millis();
|
perf_mon_timer = millis();
|
||||||
|
@ -508,7 +490,10 @@ init_compass()
|
||||||
static void
|
static void
|
||||||
init_optflow()
|
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_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
|
optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view
|
||||||
}
|
}
|
||||||
|
@ -539,7 +524,7 @@ static void
|
||||||
init_throttle_cruise()
|
init_throttle_cruise()
|
||||||
{
|
{
|
||||||
// are we moving from manual throttle to auto_throttle?
|
// 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.pi_throttle.reset_I();
|
||||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||||
}
|
}
|
||||||
|
|
|
@ -362,7 +362,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
for(int i = 0; i < 9; i++){
|
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();
|
Serial.println();
|
||||||
delay(20);
|
delay(20);
|
||||||
|
@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||||
static int8_t
|
static int8_t
|
||||||
test_optflow(uint8_t argc, const Menu::arg *argv)
|
test_optflow(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
|
///*
|
||||||
if(g.optflow_enabled) {
|
if(g.optflow_enabled) {
|
||||||
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
|
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
|
|
|
@ -1,4 +1,3 @@
|
||||||
|
|
||||||
// -*- 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
|
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
|
||||||
|
|
|
@ -304,19 +304,6 @@
|
||||||
//#define FLIGHT_MODE_6 MANUAL
|
//#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
|
// 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
|
// 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
|
// also means that you should avoid switching out of MANUAL while you have
|
||||||
// any control stick deflection.
|
// 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
|
||||||
//
|
//
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
|
@ -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 unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||||
static float load; // % MCU cycles used
|
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
|
// Top-level logic
|
||||||
|
@ -501,7 +502,7 @@ static void fast_loop()
|
||||||
hil.update();
|
hil.update();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
dcm.update_DCM(G_Dt);
|
dcm.update_DCM();
|
||||||
|
|
||||||
// uses the yaw from the DCM to give more accurate turns
|
// uses the yaw from the DCM to give more accurate turns
|
||||||
calc_bearing_error();
|
calc_bearing_error();
|
||||||
|
@ -710,6 +711,8 @@ static void slow_loop()
|
||||||
// ----------------------------------
|
// ----------------------------------
|
||||||
update_servo_switches();
|
update_servo_switches();
|
||||||
|
|
||||||
|
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 2:
|
case 2:
|
||||||
|
|
|
@ -61,7 +61,7 @@ static void stabilize()
|
||||||
|
|
||||||
// Mix Stick input to allow users to override control surfaces
|
// 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?
|
// 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
|
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
|
||||||
// important for steering on the ground during landing
|
// 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 = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
|
||||||
ch4_inf = fabs(ch4_inf);
|
ch4_inf = fabs(ch4_inf);
|
||||||
ch4_inf = min(ch4_inf, 400.0);
|
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_throttle.radio_out = g.channel_throttle.radio_in;
|
||||||
g.channel_rudder.radio_out = g.channel_rudder.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;
|
G_RC_AUX(k_aileron)->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (g.mix_mode == 0) {
|
if (g.mix_mode == 0) {
|
||||||
g.channel_roll.calc_pwm();
|
g.channel_roll.calc_pwm();
|
||||||
g.channel_pitch.calc_pwm();
|
g.channel_pitch.calc_pwm();
|
||||||
g.channel_rudder.calc_pwm();
|
g.channel_rudder.calc_pwm();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
|
if (g_rc_function[RC_Channel_aux::k_aileron]) {
|
||||||
g.rc_5.servo_out = g.channel_roll.servo_out;
|
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
|
||||||
g.rc_5.calc_pwm();
|
g_rc_function[RC_Channel_aux::k_aileron]->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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
|
@ -320,8 +315,7 @@ static void set_servos(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if(control_mode <= FLY_BY_WIRE_B) {
|
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;
|
G_RC_AUX(k_flap_auto)->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
|
|
||||||
} else if (control_mode >= FLY_BY_WIRE_C) {
|
} else if (control_mode >= FLY_BY_WIRE_C) {
|
||||||
if (g.airspeed_enabled == true) {
|
if (g.airspeed_enabled == true) {
|
||||||
flapSpeedSource = g.airspeed_cruise;
|
flapSpeedSource = g.airspeed_cruise;
|
||||||
|
@ -329,14 +323,11 @@ static void set_servos(void)
|
||||||
flapSpeedSource = g.throttle_cruise;
|
flapSpeedSource = g.throttle_cruise;
|
||||||
}
|
}
|
||||||
if ( flapSpeedSource > g.flap_1_speed) {
|
if ( flapSpeedSource > g.flap_1_speed) {
|
||||||
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
|
G_RC_AUX(k_flap_auto)->servo_out = 0;
|
||||||
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
|
|
||||||
} else if (flapSpeedSource > g.flap_2_speed) {
|
} 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;
|
G_RC_AUX(k_flap_auto)->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;
|
|
||||||
} else {
|
} else {
|
||||||
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
|
G_RC_AUX(k_flap_auto)->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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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_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_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_4, g.channel_rudder.radio_out); // send to Servos
|
||||||
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
|
// Route configurable aux. functions to their respective servos
|
||||||
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to 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
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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})
|
|
@ -51,6 +51,7 @@ GCS_MAVLINK::update(void)
|
||||||
// receive new packets
|
// receive new packets
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
mavlink_status_t status;
|
mavlink_status_t status;
|
||||||
|
status.packet_rx_drop_count = 0;
|
||||||
|
|
||||||
// process received bytes
|
// process received bytes
|
||||||
while(comm_get_available(chan))
|
while(comm_get_available(chan))
|
||||||
|
@ -1084,15 +1085,6 @@ GCS_MAVLINK::_queued_send()
|
||||||
|
|
||||||
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
|
#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
|
a delay() callback that processes MAVLink packets. We set this as the
|
||||||
callback in long running library initialisation routines to allow
|
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)
|
static void mavlink_delay(unsigned long t)
|
||||||
{
|
{
|
||||||
unsigned long tstart;
|
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) {
|
if (in_mavlink_delay) {
|
||||||
// this should never happen, but let's not tempt fate by
|
// 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) {
|
if (tnow - last_1hz > 1000) {
|
||||||
last_1hz = tnow;
|
last_1hz = tnow;
|
||||||
gcs.send_message(MSG_HEARTBEAT);
|
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)
|
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
|
||||||
hil.send_message(MSG_HEARTBEAT);
|
hil.send_message(MSG_HEARTBEAT);
|
||||||
|
hil.send_message(MSG_EXTENDED_STATUS1);
|
||||||
#endif
|
#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) {
|
if (tnow - last_50hz > 20) {
|
||||||
last_50hz = tnow;
|
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)
|
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
|
||||||
hil.update();
|
hil.update();
|
||||||
#endif
|
#endif
|
||||||
send_rate(45, 1000);
|
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
} while (millis() - tstart < t);
|
} while (millis() - tstart < t);
|
||||||
|
|
|
@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
|
||||||
break;
|
break;
|
||||||
case MSG_LOCATION:
|
case MSG_LOCATION:
|
||||||
break;
|
break;
|
||||||
case MSG_LOCAL_LOCATION:
|
|
||||||
break;
|
|
||||||
case MSG_GPS_RAW:
|
case MSG_GPS_RAW:
|
||||||
break;
|
break;
|
||||||
case MSG_SERVO_OUT:
|
case MSG_SERVO_OUT:
|
||||||
|
|
|
@ -25,36 +25,44 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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
|
#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
|
!!NOTE!!
|
||||||
// bootup, to try to prevent bricking of Xbees
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch(id) {
|
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
|
||||||
|
*/
|
||||||
|
|
||||||
case MSG_HEARTBEAT:
|
#define NOINLINE __attribute__((noinline))
|
||||||
|
|
||||||
|
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
|
||||||
mavlink_msg_heartbeat_send(
|
mavlink_msg_heartbeat_send(
|
||||||
chan,
|
chan,
|
||||||
mavlink_system.type,
|
mavlink_system.type,
|
||||||
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS1:
|
static NOINLINE void send_attitude(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
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 mode = MAV_MODE_UNINIT;
|
||||||
uint8_t nav_mode = MAV_NAV_VECTOR;
|
uint8_t nav_mode = MAV_NAV_VECTOR;
|
||||||
|
|
||||||
|
@ -106,36 +114,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
battery_voltage * 1000,
|
battery_voltage * 1000,
|
||||||
battery_remaining,
|
battery_remaining,
|
||||||
packet_drops);
|
packet_drops);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS2:
|
static void NOINLINE send_meminfo(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(MEMINFO);
|
|
||||||
extern unsigned __brkval;
|
extern unsigned __brkval;
|
||||||
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_ATTITUDE:
|
static void NOINLINE send_location(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
|
||||||
Vector3f omega = dcm.get_gyro();
|
|
||||||
mavlink_msg_attitude_send(
|
|
||||||
chan,
|
|
||||||
timeStamp,
|
|
||||||
dcm.roll,
|
|
||||||
dcm.pitch,
|
|
||||||
dcm.yaw,
|
|
||||||
omega.x,
|
|
||||||
omega.y,
|
|
||||||
omega.z);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case MSG_LOCATION:
|
|
||||||
{
|
|
||||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
|
||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
||||||
mavlink_msg_global_position_int_send(
|
mavlink_msg_global_position_int_send(
|
||||||
chan,
|
chan,
|
||||||
|
@ -145,13 +133,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
g_gps->ground_speed * rot.a.x,
|
g_gps->ground_speed * rot.a.x,
|
||||||
g_gps->ground_speed * rot.b.x,
|
g_gps->ground_speed * rot.b.x,
|
||||||
g_gps->ground_speed * rot.c.x);
|
g_gps->ground_speed * rot.c.x);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_NAV_CONTROLLER_OUTPUT:
|
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
if (control_mode != MANUAL) {
|
|
||||||
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
|
||||||
mavlink_msg_nav_controller_output_send(
|
mavlink_msg_nav_controller_output_send(
|
||||||
chan,
|
chan,
|
||||||
nav_roll / 1.0e2,
|
nav_roll / 1.0e2,
|
||||||
|
@ -163,31 +148,12 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
airspeed_error,
|
airspeed_error,
|
||||||
crosstrack_error);
|
crosstrack_error);
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case MSG_LOCAL_LOCATION:
|
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
|
|
||||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
|
||||||
mavlink_msg_local_position_send(
|
|
||||||
chan,
|
|
||||||
timeStamp,
|
|
||||||
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
|
|
||||||
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
|
|
||||||
(current_loc.alt - home.alt) / 1.0e2,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.a.x,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.b.x,
|
|
||||||
g_gps->ground_speed / 1.0e2 * rot.c.x);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case MSG_GPS_RAW:
|
|
||||||
{
|
|
||||||
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
|
||||||
mavlink_msg_gps_raw_send(
|
mavlink_msg_gps_raw_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
g_gps->status(),
|
g_gps->status(),
|
||||||
g_gps->latitude / 1.0e7,
|
g_gps->latitude / 1.0e7,
|
||||||
g_gps->longitude / 1.0e7,
|
g_gps->longitude / 1.0e7,
|
||||||
|
@ -196,13 +162,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
0.0,
|
0.0,
|
||||||
g_gps->ground_speed / 100.0,
|
g_gps->ground_speed / 100.0,
|
||||||
g_gps->ground_course / 100.0);
|
g_gps->ground_course / 100.0);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_SERVO_OUT:
|
static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
const uint8_t rssi = 1;
|
||||||
uint8_t rssi = 1;
|
|
||||||
// normalized values scaled to -10000 to 10000
|
// normalized values scaled to -10000 to 10000
|
||||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||||
mavlink_msg_rc_channels_scaled_send(
|
mavlink_msg_rc_channels_scaled_send(
|
||||||
|
@ -216,12 +180,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
0,
|
0,
|
||||||
0,
|
0,
|
||||||
rssi);
|
rssi);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RADIO_IN:
|
static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
|
||||||
uint8_t rssi = 1;
|
uint8_t rssi = 1;
|
||||||
mavlink_msg_rc_channels_raw_send(
|
mavlink_msg_rc_channels_raw_send(
|
||||||
chan,
|
chan,
|
||||||
|
@ -234,12 +196,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
g.rc_7.radio_in,
|
g.rc_7.radio_in,
|
||||||
g.rc_8.radio_in,
|
g.rc_8.radio_in,
|
||||||
rssi);
|
rssi);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RADIO_OUT:
|
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
|
||||||
mavlink_msg_servo_output_raw_send(
|
mavlink_msg_servo_output_raw_send(
|
||||||
chan,
|
chan,
|
||||||
g.channel_roll.radio_out,
|
g.channel_roll.radio_out,
|
||||||
|
@ -250,12 +210,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
g.rc_6.radio_out,
|
g.rc_6.radio_out,
|
||||||
g.rc_7.radio_out,
|
g.rc_7.radio_out,
|
||||||
g.rc_8.radio_out);
|
g.rc_8.radio_out);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_VFR_HUD:
|
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
|
||||||
mavlink_msg_vfr_hud_send(
|
mavlink_msg_vfr_hud_send(
|
||||||
chan,
|
chan,
|
||||||
(float)airspeed / 100.0,
|
(float)airspeed / 100.0,
|
||||||
|
@ -264,21 +222,17 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
(int)g.channel_throttle.servo_out,
|
(int)g.channel_throttle.servo_out,
|
||||||
current_loc.alt / 100.0,
|
current_loc.alt / 100.0,
|
||||||
climb_rate);
|
climb_rate);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
case MSG_RAW_IMU1:
|
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
|
||||||
Vector3f accel = imu.get_accel();
|
Vector3f accel = imu.get_accel();
|
||||||
Vector3f gyro = imu.get_gyro();
|
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(
|
mavlink_msg_raw_imu_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
accel.x * 1000.0 / gravity,
|
accel.x * 1000.0 / gravity,
|
||||||
accel.y * 1000.0 / gravity,
|
accel.y * 1000.0 / gravity,
|
||||||
accel.z * 1000.0 / gravity,
|
accel.z * 1000.0 / gravity,
|
||||||
|
@ -288,24 +242,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
compass.mag_x,
|
compass.mag_x,
|
||||||
compass.mag_y,
|
compass.mag_y,
|
||||||
compass.mag_z);
|
compass.mag_z);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RAW_IMU2:
|
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
|
||||||
mavlink_msg_scaled_pressure_send(
|
mavlink_msg_scaled_pressure_send(
|
||||||
chan,
|
chan,
|
||||||
timeStamp,
|
micros(),
|
||||||
(float)barometer.Press/100.0,
|
(float)barometer.Press/100.0,
|
||||||
(float)(barometer.Press-g.ground_pressure)/100.0,
|
(float)(barometer.Press-g.ground_pressure)/100.0,
|
||||||
(int)(barometer.Temp*10));
|
(int)(barometer.Temp*10));
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_RAW_IMU3:
|
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
|
||||||
Vector3f mag_offsets = compass.get_offsets();
|
Vector3f mag_offsets = compass.get_offsets();
|
||||||
|
|
||||||
mavlink_msg_sensor_offsets_send(chan,
|
mavlink_msg_sensor_offsets_send(chan,
|
||||||
|
@ -317,13 +267,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
barometer.RawTemp,
|
barometer.RawTemp,
|
||||||
imu.gx(), imu.gy(), imu.gz(),
|
imu.gx(), imu.gy(), imu.gz(),
|
||||||
imu.ax(), imu.ay(), imu.az());
|
imu.ax(), imu.ay(), imu.az());
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
case MSG_GPS_STATUS:
|
static void NOINLINE send_gps_status(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
|
||||||
mavlink_msg_gps_status_send(
|
mavlink_msg_gps_status_send(
|
||||||
chan,
|
chan,
|
||||||
g_gps->num_sats,
|
g_gps->num_sats,
|
||||||
|
@ -332,18 +280,112 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
|
||||||
NULL,
|
NULL,
|
||||||
NULL,
|
NULL,
|
||||||
NULL);
|
NULL);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case MSG_CURRENT_WAYPOINT:
|
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
|
||||||
mavlink_msg_waypoint_current_send(
|
mavlink_msg_waypoint_current_send(
|
||||||
chan,
|
chan,
|
||||||
g.waypoint_index);
|
g.waypoint_index);
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
|
|
||||||
|
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) {
|
||||||
|
case MSG_HEARTBEAT:
|
||||||
|
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
||||||
|
send_heartbeat(chan);
|
||||||
|
return true;
|
||||||
|
|
||||||
|
case MSG_EXTENDED_STATUS1:
|
||||||
|
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
||||||
|
send_extended_status1(chan, packet_drops);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_EXTENDED_STATUS2:
|
||||||
|
CHECK_PAYLOAD_SIZE(MEMINFO);
|
||||||
|
send_meminfo(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_ATTITUDE:
|
||||||
|
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
||||||
|
send_attitude(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_LOCATION:
|
||||||
|
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
||||||
|
send_location(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_NAV_CONTROLLER_OUTPUT:
|
||||||
|
if (control_mode != MANUAL) {
|
||||||
|
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
||||||
|
send_nav_controller_output(chan);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_GPS_RAW:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
||||||
|
send_gps_raw(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_SERVO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
||||||
|
send_servo_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_IN:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
||||||
|
send_radio_in(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
||||||
|
send_radio_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_VFR_HUD:
|
||||||
|
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
||||||
|
send_vfr_hud(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
case MSG_RAW_IMU1:
|
||||||
|
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
||||||
|
send_raw_imu1(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU2:
|
||||||
|
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
||||||
|
send_raw_imu2(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU3:
|
||||||
|
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
||||||
|
send_raw_imu3(chan);
|
||||||
|
break;
|
||||||
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
case MSG_GPS_STATUS:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
||||||
|
send_gps_status(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_CURRENT_WAYPOINT:
|
||||||
|
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
||||||
|
send_current_waypoint(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,7 +17,7 @@ public:
|
||||||
// The increment will prevent old parameters from being used incorrectly
|
// The increment will prevent old parameters from being used incorrectly
|
||||||
// by newer code.
|
// 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
|
// The parameter software_type is set up solely for ground station use
|
||||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||||
|
@ -95,6 +95,9 @@ public:
|
||||||
k_param_compass_enabled,
|
k_param_compass_enabled,
|
||||||
k_param_compass,
|
k_param_compass,
|
||||||
k_param_battery_monitoring,
|
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_pack_capacity,
|
||||||
k_param_airspeed_offset,
|
k_param_airspeed_offset,
|
||||||
k_param_sonar_enabled,
|
k_param_sonar_enabled,
|
||||||
|
@ -135,11 +138,6 @@ public:
|
||||||
k_param_gcs_heartbeat_fs_enabled,
|
k_param_gcs_heartbeat_fs_enabled,
|
||||||
k_param_throttle_slewrate,
|
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
|
// 200: Feed-forward gains
|
||||||
//
|
//
|
||||||
|
@ -313,6 +311,9 @@ public:
|
||||||
AP_Int8 compass_enabled;
|
AP_Int8 compass_enabled;
|
||||||
AP_Int16 angle_of_attack;
|
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_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_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 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
|
||||||
AP_Int8 sonar_enabled;
|
AP_Int8 sonar_enabled;
|
||||||
|
@ -327,14 +328,10 @@ public:
|
||||||
RC_Channel channel_pitch;
|
RC_Channel channel_pitch;
|
||||||
RC_Channel channel_throttle;
|
RC_Channel channel_throttle;
|
||||||
RC_Channel channel_rudder;
|
RC_Channel channel_rudder;
|
||||||
RC_Channel rc_5;
|
RC_Channel_aux rc_5;
|
||||||
RC_Channel rc_6;
|
RC_Channel_aux rc_6;
|
||||||
RC_Channel rc_7;
|
RC_Channel_aux rc_7;
|
||||||
RC_Channel rc_8;
|
RC_Channel_aux rc_8;
|
||||||
AP_Int8 rc_5_funct;
|
|
||||||
AP_Int8 rc_6_funct;
|
|
||||||
AP_Int8 rc_7_funct;
|
|
||||||
AP_Int8 rc_8_funct;
|
|
||||||
|
|
||||||
// PID controllers
|
// PID controllers
|
||||||
//
|
//
|
||||||
|
@ -424,14 +421,13 @@ public:
|
||||||
|
|
||||||
|
|
||||||
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
|
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")),
|
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
|
||||||
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
|
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
|
||||||
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
|
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
|
||||||
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_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!
|
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
|
||||||
|
|
||||||
|
|
|
@ -226,19 +226,6 @@
|
||||||
# define CH8_MAX 2000
|
# define CH8_MAX 2000
|
||||||
#endif
|
#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
|
#ifndef FLAP_1_PERCENT
|
||||||
# define FLAP_1_PERCENT 0
|
# define FLAP_1_PERCENT 0
|
||||||
|
@ -313,7 +300,7 @@
|
||||||
// AUTO_TRIM
|
// AUTO_TRIM
|
||||||
//
|
//
|
||||||
#ifndef AUTO_TRIM
|
#ifndef AUTO_TRIM
|
||||||
# define AUTO_TRIM ENABLED
|
# define AUTO_TRIM DISABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -41,39 +41,12 @@
|
||||||
#define GPS_PROTOCOL_MTK16 6
|
#define GPS_PROTOCOL_MTK16 6
|
||||||
#define GPS_PROTOCOL_AUTO 7
|
#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_ROLL CH_1
|
||||||
#define CH_PITCH CH_2
|
#define CH_PITCH CH_2
|
||||||
#define CH_THROTTLE CH_3
|
#define CH_THROTTLE CH_3
|
||||||
#define CH_RUDDER CH_4
|
#define CH_RUDDER CH_4
|
||||||
#define CH_YAW 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
|
// HIL enumerations
|
||||||
#define HIL_PROTOCOL_XPLANE 1
|
#define HIL_PROTOCOL_XPLANE 1
|
||||||
#define HIL_PROTOCOL_MAVLINK 2
|
#define HIL_PROTOCOL_MAVLINK 2
|
||||||
|
@ -190,7 +163,6 @@
|
||||||
#define MSG_ATTITUDE_CORRECT 0xb1
|
#define MSG_ATTITUDE_CORRECT 0xb1
|
||||||
#define MSG_POSITION_SET 0xb2
|
#define MSG_POSITION_SET 0xb2
|
||||||
#define MSG_ATTITUDE_SET 0xb3
|
#define MSG_ATTITUDE_SET 0xb3
|
||||||
#define MSG_LOCAL_LOCATION 0xb4
|
|
||||||
#define MSG_RETRY_DEFERRED 0xff
|
#define MSG_RETRY_DEFERRED 0xff
|
||||||
|
|
||||||
#define SEVERITY_LOW 1
|
#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 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 AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
|
||||||
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
|
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
|
||||||
|
|
|
@ -43,12 +43,12 @@ static void failsafe_long_on_event()
|
||||||
case STABILIZE:
|
case STABILIZE:
|
||||||
case FLY_BY_WIRE_A: // middle position
|
case FLY_BY_WIRE_A: // middle position
|
||||||
case FLY_BY_WIRE_B: // middle position
|
case FLY_BY_WIRE_B: // middle position
|
||||||
|
case CIRCLE:
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AUTO:
|
case AUTO:
|
||||||
case LOITER:
|
case LOITER:
|
||||||
case CIRCLE:
|
|
||||||
if(g.long_fs_action == 1) {
|
if(g.long_fs_action == 1) {
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,24 +23,7 @@ static void init_rc_in()
|
||||||
g.channel_throttle.dead_zone = 6;
|
g.channel_throttle.dead_zone = 6;
|
||||||
|
|
||||||
//set auxiliary ranges
|
//set auxiliary ranges
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
|
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static void init_rc_out()
|
static void init_rc_out()
|
||||||
|
@ -173,8 +156,7 @@ static void trim_control_surfaces()
|
||||||
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
||||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
||||||
g.channel_rudder.radio_trim = g.channel_rudder.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
|
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->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
|
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
elevon1_trim = ch1_temp;
|
elevon1_trim = ch1_temp;
|
||||||
|
@ -191,8 +173,7 @@ static void trim_control_surfaces()
|
||||||
g.channel_pitch.save_eeprom();
|
g.channel_pitch.save_eeprom();
|
||||||
g.channel_throttle.save_eeprom();
|
g.channel_throttle.save_eeprom();
|
||||||
g.channel_rudder.save_eeprom();
|
g.channel_rudder.save_eeprom();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
|
G_RC_AUX(k_aileron)->save_eeprom();
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static void trim_radio()
|
static void trim_radio()
|
||||||
|
@ -208,8 +189,7 @@ static void trim_radio()
|
||||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
||||||
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
|
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
|
||||||
g.channel_rudder.radio_trim = g.channel_rudder.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
|
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->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
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
elevon1_trim = ch1_temp;
|
elevon1_trim = ch1_temp;
|
||||||
|
@ -225,7 +205,5 @@ static void trim_radio()
|
||||||
g.channel_pitch.save_eeprom();
|
g.channel_pitch.save_eeprom();
|
||||||
//g.channel_throttle.save_eeprom();
|
//g.channel_throttle.save_eeprom();
|
||||||
g.channel_rudder.save_eeprom();
|
g.channel_rudder.save_eeprom();
|
||||||
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
|
G_RC_AUX(k_aileron)->save_eeprom();
|
||||||
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -41,7 +41,7 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// Create the top-level menu object.
|
||||||
MENU(main_menu, "APM trunk", main_menu_commands);
|
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
#endif // CLI_ENABLED
|
||||||
|
|
||||||
|
|
|
@ -418,7 +418,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
while(1){
|
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();
|
Serial.println();
|
||||||
delay(100);
|
delay(100);
|
||||||
if(Serial.available() > 0){
|
if(Serial.available() > 0){
|
||||||
|
@ -476,7 +476,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||||
|
|
||||||
// IMU
|
// IMU
|
||||||
// ---
|
// ---
|
||||||
dcm.update_DCM(G_Dt);
|
dcm.update_DCM();
|
||||||
|
|
||||||
if(g.compass_enabled) {
|
if(g.compass_enabled) {
|
||||||
medium_loopCounter++;
|
medium_loopCounter++;
|
||||||
|
@ -563,7 +563,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||||
|
|
||||||
// IMU
|
// IMU
|
||||||
// ---
|
// ---
|
||||||
dcm.update_DCM(G_Dt);
|
dcm.update_DCM();
|
||||||
|
|
||||||
medium_loopCounter++;
|
medium_loopCounter++;
|
||||||
if(medium_loopCounter == 5){
|
if(medium_loopCounter == 5){
|
||||||
|
|
|
@ -2,7 +2,7 @@
|
||||||
// Example and reference ArduPilot Mega configuration file
|
// 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.
|
// supported by the ArduPilot Mega software.
|
||||||
//
|
//
|
||||||
// Most of these options are just that - optional. You should create
|
// Most of these options are just that - optional. You should create
|
||||||
|
|
|
@ -188,6 +188,7 @@
|
||||||
<SubType>Component</SubType>
|
<SubType>Component</SubType>
|
||||||
</Compile>
|
</Compile>
|
||||||
<Compile Include="CommsUdpSerial.cs" />
|
<Compile Include="CommsUdpSerial.cs" />
|
||||||
|
<Compile Include="georefimage.cs" />
|
||||||
<Compile Include="HIL\Aircraft.cs" />
|
<Compile Include="HIL\Aircraft.cs" />
|
||||||
<Compile Include="HIL\Point3d.cs" />
|
<Compile Include="HIL\Point3d.cs" />
|
||||||
<Compile Include="HIL\QuadCopter.cs" />
|
<Compile Include="HIL\QuadCopter.cs" />
|
||||||
|
@ -232,7 +233,6 @@
|
||||||
<Compile Include="MyUserControl.cs">
|
<Compile Include="MyUserControl.cs">
|
||||||
<SubType>UserControl</SubType>
|
<SubType>UserControl</SubType>
|
||||||
</Compile>
|
</Compile>
|
||||||
<Compile Include="NetSerialServer.cs" />
|
|
||||||
<Compile Include="ArduinoSTKv2.cs">
|
<Compile Include="ArduinoSTKv2.cs">
|
||||||
<SubType>Component</SubType>
|
<SubType>Component</SubType>
|
||||||
</Compile>
|
</Compile>
|
||||||
|
@ -302,7 +302,6 @@
|
||||||
<DependentUpon>ElevationProfile.cs</DependentUpon>
|
<DependentUpon>ElevationProfile.cs</DependentUpon>
|
||||||
</Compile>
|
</Compile>
|
||||||
<Compile Include="MAVLink.cs" />
|
<Compile Include="MAVLink.cs" />
|
||||||
<Compile Include="NetSerial.cs" />
|
|
||||||
<Compile Include="ArduinoSTK.cs">
|
<Compile Include="ArduinoSTK.cs">
|
||||||
<SubType>Component</SubType>
|
<SubType>Component</SubType>
|
||||||
</Compile>
|
</Compile>
|
||||||
|
@ -338,6 +337,7 @@
|
||||||
<Compile Include="Splash.Designer.cs">
|
<Compile Include="Splash.Designer.cs">
|
||||||
<DependentUpon>Splash.cs</DependentUpon>
|
<DependentUpon>Splash.cs</DependentUpon>
|
||||||
</Compile>
|
</Compile>
|
||||||
|
<Compile Include="srtm.cs" />
|
||||||
<Compile Include="temp.cs">
|
<Compile Include="temp.cs">
|
||||||
<SubType>Form</SubType>
|
<SubType>Form</SubType>
|
||||||
</Compile>
|
</Compile>
|
||||||
|
@ -374,6 +374,7 @@
|
||||||
</EmbeddedResource>
|
</EmbeddedResource>
|
||||||
<EmbeddedResource Include="GCSViews\Firmware.zh-Hans.resx">
|
<EmbeddedResource Include="GCSViews\Firmware.zh-Hans.resx">
|
||||||
<DependentUpon>Firmware.cs</DependentUpon>
|
<DependentUpon>Firmware.cs</DependentUpon>
|
||||||
|
<SubType>Designer</SubType>
|
||||||
</EmbeddedResource>
|
</EmbeddedResource>
|
||||||
<EmbeddedResource Include="GCSViews\FlightData.zh-Hans.resx">
|
<EmbeddedResource Include="GCSViews\FlightData.zh-Hans.resx">
|
||||||
<DependentUpon>FlightData.cs</DependentUpon>
|
<DependentUpon>FlightData.cs</DependentUpon>
|
||||||
|
@ -424,6 +425,7 @@
|
||||||
<EmbeddedResource Include="GCSViews\Firmware.resx">
|
<EmbeddedResource Include="GCSViews\Firmware.resx">
|
||||||
<DependentUpon>Firmware.cs</DependentUpon>
|
<DependentUpon>Firmware.cs</DependentUpon>
|
||||||
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
|
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
|
||||||
|
<SubType>Designer</SubType>
|
||||||
</EmbeddedResource>
|
</EmbeddedResource>
|
||||||
<EmbeddedResource Include="GCSViews\FlightData.resx">
|
<EmbeddedResource Include="GCSViews\FlightData.resx">
|
||||||
<DependentUpon>FlightData.cs</DependentUpon>
|
<DependentUpon>FlightData.cs</DependentUpon>
|
||||||
|
|
|
@ -240,13 +240,12 @@ namespace ArdupilotMega
|
||||||
{
|
{
|
||||||
STABILIZE = 0, // hold level position
|
STABILIZE = 0, // hold level position
|
||||||
ACRO = 1, // rate control
|
ACRO = 1, // rate control
|
||||||
SIMPLE = 2, //
|
ALT_HOLD = 2, // AUTO control
|
||||||
ALT_HOLD = 3, // AUTO control
|
AUTO = 3, // AUTO control
|
||||||
AUTO = 4, // AUTO control
|
GUIDED = 4, // AUTO control
|
||||||
GUIDED = 5, // AUTO control
|
LOITER = 5, // Hold a single location
|
||||||
LOITER = 6, // Hold a single location
|
RTL = 6, // AUTO control
|
||||||
RTL = 7, // AUTO control
|
CIRCLE = 7
|
||||||
CIRCLE = 8
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode)
|
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode)
|
||||||
|
|
|
@ -6,7 +6,6 @@ using System.IO.Ports;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
using System.Net; // dns, ip address
|
using System.Net; // dns, ip address
|
||||||
using System.Net.Sockets; // tcplistner
|
using System.Net.Sockets; // tcplistner
|
||||||
using SerialProxy;
|
|
||||||
|
|
||||||
namespace System.IO.Ports
|
namespace System.IO.Ports
|
||||||
{
|
{
|
||||||
|
|
|
@ -65,7 +65,7 @@ namespace ArdupilotMega
|
||||||
public float gz { get; set; }
|
public float gz { get; set; }
|
||||||
|
|
||||||
// calced turn rate
|
// 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
|
//radio
|
||||||
public float ch1in { get; set; }
|
public float ch1in { get; set; }
|
||||||
|
@ -101,17 +101,17 @@ namespace ArdupilotMega
|
||||||
//nav state
|
//nav state
|
||||||
public float nav_roll { get; set; }
|
public float nav_roll { get; set; }
|
||||||
public float nav_pitch { get; set; }
|
public float nav_pitch { get; set; }
|
||||||
public short nav_bearing { get; set; }
|
public float nav_bearing { get; set; }
|
||||||
public short target_bearing { get; set; }
|
public float target_bearing { get; set; }
|
||||||
public ushort wp_dist { get { return (ushort)(_wpdist * multiplierdist); } set { _wpdist = value; } }
|
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 alt_error { get { return _alt_error * multiplierdist; } set { _alt_error = value; } }
|
||||||
public float ber_error { get { return (target_bearing - yaw); } set { } }
|
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 aspd_error { get { return _aspd_error * multiplierspeed; } set { _aspd_error = value; } }
|
||||||
public float xtrack_error { get; set; }
|
public float xtrack_error { get; set; }
|
||||||
public int wpno { get; set; }
|
public float wpno { get; set; }
|
||||||
public string mode { get; set; }
|
public string mode { get; set; }
|
||||||
public float climbrate { get; set; }
|
public float climbrate { get; set; }
|
||||||
ushort _wpdist;
|
float _wpdist;
|
||||||
float _aspd_error;
|
float _aspd_error;
|
||||||
float _alt_error;
|
float _alt_error;
|
||||||
|
|
||||||
|
@ -150,7 +150,8 @@ namespace ArdupilotMega
|
||||||
public float brklevel { get; set; }
|
public float brklevel { get; set; }
|
||||||
|
|
||||||
// stats
|
// stats
|
||||||
public ushort packetdrop { get; set; }
|
public ushort packetdropremote { get; set; }
|
||||||
|
public ushort linkqualitygcs { get; set; }
|
||||||
|
|
||||||
// requested stream rates
|
// requested stream rates
|
||||||
public byte rateattitude { get; set; }
|
public byte rateattitude { get; set; }
|
||||||
|
@ -284,24 +285,21 @@ namespace ArdupilotMega
|
||||||
mode = "Acro";
|
mode = "Acro";
|
||||||
break;
|
break;
|
||||||
case (byte)102:
|
case (byte)102:
|
||||||
mode = "Simple";
|
|
||||||
break;
|
|
||||||
case (byte)103:
|
|
||||||
mode = "Alt Hold";
|
mode = "Alt Hold";
|
||||||
break;
|
break;
|
||||||
case (byte)104:
|
case (byte)103:
|
||||||
mode = "Auto";
|
mode = "Auto";
|
||||||
break;
|
break;
|
||||||
case (byte)105:
|
case (byte)104:
|
||||||
mode = "Guided";
|
mode = "Guided";
|
||||||
break;
|
break;
|
||||||
case (byte)106:
|
case (byte)105:
|
||||||
mode = "Loiter";
|
mode = "Loiter";
|
||||||
break;
|
break;
|
||||||
case (byte)107:
|
case (byte)106:
|
||||||
mode = "RTL";
|
mode = "RTL";
|
||||||
break;
|
break;
|
||||||
case (byte)108:
|
case (byte)107:
|
||||||
mode = "Circle";
|
mode = "Circle";
|
||||||
break;
|
break;
|
||||||
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
|
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
|
||||||
|
@ -361,7 +359,7 @@ namespace ArdupilotMega
|
||||||
battery_voltage = sysstatus.vbat;
|
battery_voltage = sysstatus.vbat;
|
||||||
battery_remaining = sysstatus.battery_remaining;
|
battery_remaining = sysstatus.battery_remaining;
|
||||||
|
|
||||||
packetdrop = sysstatus.packet_drop;
|
packetdropremote = sysstatus.packet_drop;
|
||||||
|
|
||||||
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
|
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
|
||||||
{
|
{
|
||||||
|
@ -468,7 +466,7 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);
|
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);
|
||||||
|
|
||||||
int oldwp = wpno;
|
int oldwp = (int)wpno;
|
||||||
|
|
||||||
wpno = wpcur.seq;
|
wpno = wpcur.seq;
|
||||||
|
|
||||||
|
@ -545,6 +543,27 @@ namespace ArdupilotMega
|
||||||
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
|
//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)
|
if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
|
||||||
{
|
{
|
||||||
MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();
|
MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();
|
||||||
|
|
|
@ -30,8 +30,8 @@
|
||||||
{
|
{
|
||||||
this.components = new System.ComponentModel.Container();
|
this.components = new System.ComponentModel.Container();
|
||||||
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration));
|
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration));
|
||||||
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle();
|
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
|
||||||
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle();
|
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
|
||||||
this.Params = new System.Windows.Forms.DataGridView();
|
this.Params = new System.Windows.Forms.DataGridView();
|
||||||
this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn();
|
this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn();
|
||||||
this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn();
|
this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn();
|
||||||
|
@ -167,9 +167,6 @@
|
||||||
this.label21 = new System.Windows.Forms.Label();
|
this.label21 = new System.Windows.Forms.Label();
|
||||||
this.THR_HOLD_P = new System.Windows.Forms.DomainUpDown();
|
this.THR_HOLD_P = new System.Windows.Forms.DomainUpDown();
|
||||||
this.label22 = new System.Windows.Forms.Label();
|
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.groupBox19 = new System.Windows.Forms.GroupBox();
|
||||||
this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown();
|
this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown();
|
||||||
this.label28 = new System.Windows.Forms.Label();
|
this.label28 = new System.Windows.Forms.Label();
|
||||||
|
@ -285,7 +282,6 @@
|
||||||
this.groupBox4.SuspendLayout();
|
this.groupBox4.SuspendLayout();
|
||||||
this.groupBox6.SuspendLayout();
|
this.groupBox6.SuspendLayout();
|
||||||
this.groupBox7.SuspendLayout();
|
this.groupBox7.SuspendLayout();
|
||||||
this.groupBox18.SuspendLayout();
|
|
||||||
this.groupBox19.SuspendLayout();
|
this.groupBox19.SuspendLayout();
|
||||||
this.groupBox20.SuspendLayout();
|
this.groupBox20.SuspendLayout();
|
||||||
this.groupBox21.SuspendLayout();
|
this.groupBox21.SuspendLayout();
|
||||||
|
@ -302,14 +298,14 @@
|
||||||
this.Params.AllowUserToAddRows = false;
|
this.Params.AllowUserToAddRows = false;
|
||||||
this.Params.AllowUserToDeleteRows = false;
|
this.Params.AllowUserToDeleteRows = false;
|
||||||
resources.ApplyResources(this.Params, "Params");
|
resources.ApplyResources(this.Params, "Params");
|
||||||
dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
|
dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
|
||||||
dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon;
|
dataGridViewCellStyle1.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)));
|
dataGridViewCellStyle1.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;
|
dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White;
|
||||||
dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight;
|
dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight;
|
||||||
dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
|
dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
|
||||||
dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
|
dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
|
||||||
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3;
|
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1;
|
||||||
this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
|
this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize;
|
||||||
this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
|
this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] {
|
||||||
this.Command,
|
this.Command,
|
||||||
|
@ -318,14 +314,14 @@
|
||||||
this.mavScale,
|
this.mavScale,
|
||||||
this.RawValue});
|
this.RawValue});
|
||||||
this.Params.Name = "Params";
|
this.Params.Name = "Params";
|
||||||
dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
|
dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
|
||||||
dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption;
|
dataGridViewCellStyle2.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)));
|
dataGridViewCellStyle2.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;
|
dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText;
|
||||||
dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight;
|
dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight;
|
||||||
dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
|
dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
|
||||||
dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
|
dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
|
||||||
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4;
|
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2;
|
||||||
this.Params.RowHeadersVisible = false;
|
this.Params.RowHeadersVisible = false;
|
||||||
this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged);
|
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.groupBox4);
|
||||||
this.TabAC2.Controls.Add(this.groupBox6);
|
this.TabAC2.Controls.Add(this.groupBox6);
|
||||||
this.TabAC2.Controls.Add(this.groupBox7);
|
this.TabAC2.Controls.Add(this.groupBox7);
|
||||||
this.TabAC2.Controls.Add(this.groupBox18);
|
|
||||||
this.TabAC2.Controls.Add(this.groupBox19);
|
this.TabAC2.Controls.Add(this.groupBox19);
|
||||||
this.TabAC2.Controls.Add(this.groupBox20);
|
this.TabAC2.Controls.Add(this.groupBox20);
|
||||||
this.TabAC2.Controls.Add(this.groupBox21);
|
this.TabAC2.Controls.Add(this.groupBox21);
|
||||||
|
@ -1157,24 +1152,6 @@
|
||||||
resources.ApplyResources(this.label22, "label22");
|
resources.ApplyResources(this.label22, "label22");
|
||||||
this.label22.Name = "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
|
// groupBox19
|
||||||
//
|
//
|
||||||
this.groupBox19.Controls.Add(this.HLD_LAT_IMAX);
|
this.groupBox19.Controls.Add(this.HLD_LAT_IMAX);
|
||||||
|
@ -1887,7 +1864,6 @@
|
||||||
this.groupBox4.ResumeLayout(false);
|
this.groupBox4.ResumeLayout(false);
|
||||||
this.groupBox6.ResumeLayout(false);
|
this.groupBox6.ResumeLayout(false);
|
||||||
this.groupBox7.ResumeLayout(false);
|
this.groupBox7.ResumeLayout(false);
|
||||||
this.groupBox18.ResumeLayout(false);
|
|
||||||
this.groupBox19.ResumeLayout(false);
|
this.groupBox19.ResumeLayout(false);
|
||||||
this.groupBox20.ResumeLayout(false);
|
this.groupBox20.ResumeLayout(false);
|
||||||
this.groupBox21.ResumeLayout(false);
|
this.groupBox21.ResumeLayout(false);
|
||||||
|
@ -2031,9 +2007,6 @@
|
||||||
private System.Windows.Forms.Label label21;
|
private System.Windows.Forms.Label label21;
|
||||||
private System.Windows.Forms.DomainUpDown THR_HOLD_P;
|
private System.Windows.Forms.DomainUpDown THR_HOLD_P;
|
||||||
private System.Windows.Forms.Label label22;
|
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.GroupBox groupBox19;
|
||||||
private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX;
|
private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX;
|
||||||
private System.Windows.Forms.Label label28;
|
private System.Windows.Forms.Label label28;
|
||||||
|
|
|
@ -257,6 +257,8 @@ namespace ArdupilotMega.GCSViews
|
||||||
|
|
||||||
internal void processToScreen()
|
internal void processToScreen()
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
Params.Rows.Clear();
|
Params.Rows.Clear();
|
||||||
|
|
||||||
// process hashdefines and update display
|
// process hashdefines and update display
|
||||||
|
@ -541,7 +543,10 @@ namespace ArdupilotMega.GCSViews
|
||||||
|
|
||||||
private void BUT_writePIDS_Click(object sender, EventArgs e)
|
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
|
try
|
||||||
{
|
{
|
||||||
|
@ -566,6 +571,7 @@ namespace ArdupilotMega.GCSViews
|
||||||
if (row.Cells[0].Value.ToString() == value)
|
if (row.Cells[0].Value.ToString() == value)
|
||||||
{
|
{
|
||||||
row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
|
row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45);
|
||||||
|
changes.Remove(value);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -575,8 +581,6 @@ namespace ArdupilotMega.GCSViews
|
||||||
}
|
}
|
||||||
catch { MessageBox.Show("Set " + value + " Failed"); }
|
catch { MessageBox.Show("Set " + value + " Failed"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
changes.Clear();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const float rad2deg = (float)(180 / Math.PI);
|
const float rad2deg = (float)(180 / Math.PI);
|
||||||
|
@ -600,9 +604,11 @@ namespace ArdupilotMega.GCSViews
|
||||||
|
|
||||||
MainV2.fixtheme(temp);
|
MainV2.fixtheme(temp);
|
||||||
|
|
||||||
TabSetup.Controls.Clear();
|
temp.ShowDialog();
|
||||||
|
|
||||||
TabSetup.Controls.Add(temp.Controls[0]);
|
startup = true;
|
||||||
|
processToScreen();
|
||||||
|
startup = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -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; }
|
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];
|
byte[] FLASH = new byte[1];
|
||||||
StreamReader sr = null;
|
StreamReader sr = null;
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
lbl_status.Text = "Reading Hex File";
|
lbl_status.Text = "Reading Hex File";
|
||||||
this.Refresh();
|
this.Refresh();
|
||||||
sr = new StreamReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex");
|
sr = new StreamReader(filename);
|
||||||
FLASH = readIntelHEXv2(sr);
|
FLASH = readIntelHEXv2(sr);
|
||||||
sr.Close();
|
sr.Close();
|
||||||
Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length);
|
Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length);
|
||||||
|
|
|
@ -343,16 +343,16 @@
|
||||||
<value>NoControl</value>
|
<value>NoControl</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
|
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
|
||||||
<value>101, 165</value>
|
<value>113, 167</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
|
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
|
||||||
<value>79, 13</value>
|
<value>56, 13</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.TabIndex" type="System.Int32, mscorlib">
|
<data name="label1.TabIndex" type="System.Int32, mscorlib">
|
||||||
<value>8</value>
|
<value>8</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Text" xml:space="preserve">
|
<data name="label1.Text" xml:space="preserve">
|
||||||
<value>ArduPilot Mega</value>
|
<value>ArduPlane</value>
|
||||||
</data>
|
</data>
|
||||||
<data name=">>label1.Name" xml:space="preserve">
|
<data name=">>label1.Name" xml:space="preserve">
|
||||||
<value>label1</value>
|
<value>label1</value>
|
||||||
|
@ -403,16 +403,16 @@
|
||||||
<value>NoControl</value>
|
<value>NoControl</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
|
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
|
||||||
<value>57, 362</value>
|
<value>74, 361</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
|
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
|
||||||
<value>168, 13</value>
|
<value>142, 13</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.TabIndex" type="System.Int32, mscorlib">
|
<data name="label3.TabIndex" type="System.Int32, mscorlib">
|
||||||
<value>10</value>
|
<value>10</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Text" xml:space="preserve">
|
<data name="label3.Text" xml:space="preserve">
|
||||||
<value>ArduPilot Mega (Xplane simulator)</value>
|
<value>ArduPlane (Xplane simulator)</value>
|
||||||
</data>
|
</data>
|
||||||
<data name=">>label3.Name" xml:space="preserve">
|
<data name=">>label3.Name" xml:space="preserve">
|
||||||
<value>label3</value>
|
<value>label3</value>
|
||||||
|
|
|
@ -435,11 +435,14 @@ namespace ArdupilotMega.GCSViews
|
||||||
private void updateLogPlayPosition()
|
private void updateLogPlayPosition()
|
||||||
{
|
{
|
||||||
this.BeginInvoke((MethodInvoker)delegate()
|
this.BeginInvoke((MethodInvoker)delegate()
|
||||||
|
{
|
||||||
|
try
|
||||||
{
|
{
|
||||||
tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100);
|
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%");
|
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)
|
if (MainV2.comPort.logplaybackfile != null)
|
||||||
MainV2.comPort.logplaybackfile.BaseStream.Position = (long)(MainV2.comPort.logplaybackfile.BaseStream.Length * (tracklog.Value / 100.0));
|
MainV2.comPort.logplaybackfile.BaseStream.Position = (long)(MainV2.comPort.logplaybackfile.BaseStream.Length * (tracklog.Value / 100.0));
|
||||||
|
|
||||||
|
updateLogPlayPosition();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool loaded = false;
|
bool loaded = false;
|
||||||
|
@ -1223,7 +1228,7 @@ namespace ArdupilotMega.GCSViews
|
||||||
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
||||||
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
||||||
|
|
||||||
if (!(typeCode == TypeCode.Single || typeCode == TypeCode.Double || typeCode == TypeCode.Int32))
|
if (!(typeCode == TypeCode.Single))
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
CheckBox chk_box = new CheckBox();
|
CheckBox chk_box = new CheckBox();
|
||||||
|
|
|
@ -347,7 +347,7 @@ namespace ArdupilotMega.GCSViews
|
||||||
{
|
{
|
||||||
TXT_mouselat.Text = lat.ToString();
|
TXT_mouselat.Text = lat.ToString();
|
||||||
TXT_mouselong.Text = lng.ToString();
|
TXT_mouselong.Text = lng.ToString();
|
||||||
TXT_mousealt.Text = alt.ToString();
|
TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0");
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
|
@ -1228,7 +1228,7 @@ namespace ArdupilotMega.GCSViews
|
||||||
|
|
||||||
if (CHK_holdalt.Checked)
|
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
|
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");
|
TXT_WPRad.Text = ((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist).ToString("0");
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
|
|
|
@ -23,7 +23,7 @@ using OpenTK.Graphics;
|
||||||
|
|
||||||
namespace hud
|
namespace hud
|
||||||
{
|
{
|
||||||
public partial class HUD : GLControl
|
public class HUD : GLControl// : Graphics
|
||||||
{
|
{
|
||||||
object paintlock = new object();
|
object paintlock = new object();
|
||||||
object streamlock = new object();
|
object streamlock = new object();
|
||||||
|
@ -38,7 +38,9 @@ namespace hud
|
||||||
int[] charbitmaptexid = new int[6000];
|
int[] charbitmaptexid = new int[6000];
|
||||||
int[] charwidth = new int[6000];
|
int[] charwidth = new int[6000];
|
||||||
|
|
||||||
int huddrawtime = 0;
|
public int huddrawtime = 0;
|
||||||
|
|
||||||
|
public bool opengl = true;
|
||||||
|
|
||||||
public HUD()
|
public HUD()
|
||||||
{
|
{
|
||||||
|
@ -46,7 +48,7 @@ namespace hud
|
||||||
return;
|
return;
|
||||||
|
|
||||||
InitializeComponent();
|
InitializeComponent();
|
||||||
graphicsObject = this;
|
//graphicsObject = this;
|
||||||
|
|
||||||
//graphicsObject = Graphics.FromImage(objBitmap);
|
//graphicsObject = Graphics.FromImage(objBitmap);
|
||||||
}
|
}
|
||||||
|
@ -150,7 +152,6 @@ System.ComponentModel.Category("Values")]
|
||||||
Bitmap objBitmap = new Bitmap(1024, 1024);
|
Bitmap objBitmap = new Bitmap(1024, 1024);
|
||||||
int count = 0;
|
int count = 0;
|
||||||
DateTime countdate = DateTime.Now;
|
DateTime countdate = DateTime.Now;
|
||||||
HUD graphicsObject; // Graphics
|
|
||||||
|
|
||||||
DateTime starttime = DateTime.MinValue;
|
DateTime starttime = DateTime.MinValue;
|
||||||
|
|
||||||
|
@ -236,40 +237,45 @@ System.ComponentModel.Category("Values")]
|
||||||
{
|
{
|
||||||
//Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds);
|
//Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds);
|
||||||
//e.Graphics.DrawImageUnscaled(objBitmap, 0, 0);
|
//e.Graphics.DrawImageUnscaled(objBitmap, 0, 0);
|
||||||
return;
|
//return;
|
||||||
}
|
}
|
||||||
|
|
||||||
starttime = DateTime.Now;
|
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();
|
MakeCurrent();
|
||||||
|
|
||||||
//GL.LoadIdentity();
|
//Console.Write(" b " + (DateTime.Now - starttime).TotalMilliseconds);
|
||||||
|
|
||||||
//GL.ClearColor(Color.Red);
|
|
||||||
|
|
||||||
GL.Clear(ClearBufferMask.ColorBufferBit);
|
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();
|
this.SwapBuffers();
|
||||||
|
|
||||||
|
//Console.Write(" e " + (DateTime.Now - starttime).TotalMilliseconds);
|
||||||
|
|
||||||
|
count++;
|
||||||
|
|
||||||
|
huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds;
|
||||||
|
|
||||||
if (DateTime.Now.Second != countdate.Second)
|
if (DateTime.Now.Second != countdate.Second)
|
||||||
{
|
{
|
||||||
countdate = DateTime.Now;
|
countdate = DateTime.Now;
|
||||||
Console.WriteLine("HUD " + count + " hz drawtime " + huddrawtime);
|
Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count));
|
||||||
count = 0;
|
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)
|
void Clear(Color color)
|
||||||
|
@ -282,7 +288,7 @@ System.ComponentModel.Category("Values")]
|
||||||
|
|
||||||
//graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90);
|
//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);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -309,7 +315,7 @@ System.ComponentModel.Category("Values")]
|
||||||
//GL.Enable(EnableCap.Texture2D);
|
//GL.Enable(EnableCap.Texture2D);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DrawEllipse(Pen penn, Rectangle rect)
|
public void DrawEllipse(Pen penn, Rectangle rect)
|
||||||
{
|
{
|
||||||
//GL.Disable(EnableCap.Texture2D);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -340,7 +346,7 @@ System.ComponentModel.Category("Values")]
|
||||||
int texture;
|
int texture;
|
||||||
Bitmap bitmap = new Bitmap(512,512);
|
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)
|
if (img == null)
|
||||||
return;
|
return;
|
||||||
|
@ -392,7 +398,7 @@ System.ComponentModel.Category("Values")]
|
||||||
GL.Disable(EnableCap.Texture2D);
|
GL.Disable(EnableCap.Texture2D);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DrawPath(Pen penn,GraphicsPath gp)
|
public void DrawPath(Pen penn, GraphicsPath gp)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
|
@ -401,7 +407,7 @@ System.ComponentModel.Category("Values")]
|
||||||
catch { }
|
catch { }
|
||||||
}
|
}
|
||||||
|
|
||||||
void FillPath(Brush brushh,GraphicsPath gp)
|
public void FillPath(Brush brushh, GraphicsPath gp)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
|
@ -410,32 +416,32 @@ System.ComponentModel.Category("Values")]
|
||||||
catch { }
|
catch { }
|
||||||
}
|
}
|
||||||
|
|
||||||
void SetClip(Rectangle rect)
|
public void SetClip(Rectangle rect)
|
||||||
{
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ResetClip()
|
public void ResetClip()
|
||||||
{
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ResetTransform()
|
public void ResetTransform()
|
||||||
{
|
{
|
||||||
GL.LoadIdentity();
|
GL.LoadIdentity();
|
||||||
}
|
}
|
||||||
|
|
||||||
void RotateTransform(float angle)
|
public void RotateTransform(float angle)
|
||||||
{
|
{
|
||||||
GL.Rotate(angle,0,0,1);
|
GL.Rotate(angle,0,0,1);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TranslateTransform(float x, float y)
|
public void TranslateTransform(float x, float y)
|
||||||
{
|
{
|
||||||
GL.Translate(x, y, 0f);
|
GL.Translate(x, y, 0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
void FillPolygon(Brush brushh, Point[] list)
|
public void FillPolygon(Brush brushh, Point[] list)
|
||||||
{
|
{
|
||||||
|
|
||||||
//GL.Disable(EnableCap.Texture2D);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
@ -456,7 +462,7 @@ System.ComponentModel.Category("Values")]
|
||||||
//GL.Enable(EnableCap.Texture2D);
|
//GL.Enable(EnableCap.Texture2D);
|
||||||
}
|
}
|
||||||
|
|
||||||
void FillPolygon(Brush brushh, PointF[] list)
|
public void FillPolygon(Brush brushh, PointF[] list)
|
||||||
{
|
{
|
||||||
//GL.Disable(EnableCap.Texture2D);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -478,7 +484,7 @@ System.ComponentModel.Category("Values")]
|
||||||
|
|
||||||
//graphicsObject.DrawPolygon(redPen, pointlist);
|
//graphicsObject.DrawPolygon(redPen, pointlist);
|
||||||
|
|
||||||
void DrawPolygon(Pen penn, Point[] list)
|
public void DrawPolygon(Pen penn, Point[] list)
|
||||||
{
|
{
|
||||||
//GL.Disable(EnableCap.Texture2D);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -499,7 +505,7 @@ System.ComponentModel.Category("Values")]
|
||||||
//GL.Enable(EnableCap.Texture2D);
|
//GL.Enable(EnableCap.Texture2D);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DrawPolygon(Pen penn, PointF[] list)
|
public void DrawPolygon(Pen penn, PointF[] list)
|
||||||
{
|
{
|
||||||
//GL.Disable(EnableCap.Texture2D);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -523,7 +529,7 @@ System.ComponentModel.Category("Values")]
|
||||||
|
|
||||||
//graphicsObject.FillRectangle(linearBrush, bg);
|
//graphicsObject.FillRectangle(linearBrush, bg);
|
||||||
|
|
||||||
void FillRectangle(Brush brushh,RectangleF rectf)
|
public void FillRectangle(Brush brushh, RectangleF rectf)
|
||||||
{
|
{
|
||||||
float x1 = rectf.X;
|
float x1 = rectf.X;
|
||||||
float y1 = rectf.Y;
|
float y1 = rectf.Y;
|
||||||
|
@ -571,12 +577,12 @@ System.ComponentModel.Category("Values")]
|
||||||
|
|
||||||
//graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height);
|
//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);
|
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);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -597,7 +603,7 @@ System.ComponentModel.Category("Values")]
|
||||||
//GL.Enable(EnableCap.Texture2D);
|
//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);
|
//GL.Disable(EnableCap.Texture2D);
|
||||||
|
|
||||||
|
@ -616,8 +622,13 @@ System.ComponentModel.Category("Values")]
|
||||||
//GL.Enable(EnableCap.Texture2D);
|
//GL.Enable(EnableCap.Texture2D);
|
||||||
}
|
}
|
||||||
|
|
||||||
void doPaint()
|
void doPaint(object e)
|
||||||
{
|
{
|
||||||
|
HUD graphicsObject = this;
|
||||||
|
//Graphics graphicsObject = ((PaintEventArgs)e).Graphics;
|
||||||
|
//graphicsObject.SmoothingMode = SmoothingMode.AntiAlias;
|
||||||
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
graphicsObject.Clear(Color.Gray);
|
graphicsObject.Clear(Color.Gray);
|
||||||
|
@ -1055,7 +1066,7 @@ System.ComponentModel.Category("Values")]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
greenPen.Width = 4;
|
||||||
|
|
||||||
// vsi
|
// vsi
|
||||||
|
|
||||||
|
@ -1131,6 +1142,20 @@ System.ComponentModel.Category("Values")]
|
||||||
drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5);
|
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);
|
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
|
// battery
|
||||||
|
|
||||||
graphicsObject.ResetTransform();
|
graphicsObject.ResetTransform();
|
||||||
|
@ -1191,8 +1216,6 @@ System.ComponentModel.Category("Values")]
|
||||||
Console.WriteLine("hud error "+ex.ToString());
|
Console.WriteLine("hud error "+ex.ToString());
|
||||||
//MessageBox.Show(ex.ToString());
|
//MessageBox.Show(ex.ToString());
|
||||||
}
|
}
|
||||||
|
|
||||||
count++;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
protected override void OnPaintBackground(PaintEventArgs e)
|
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)
|
protected override void OnResize(EventArgs e)
|
||||||
{
|
{
|
||||||
if (DesignMode)
|
if (DesignMode)
|
||||||
|
@ -1365,6 +1413,18 @@ System.ComponentModel.Category("Values")]
|
||||||
|
|
||||||
charbitmaps = new Bitmap[charbitmaps.Length];
|
charbitmaps = new Bitmap[charbitmaps.Length];
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
|
||||||
|
foreach (int texid in charbitmaptexid)
|
||||||
|
{
|
||||||
|
if (texid != 0)
|
||||||
|
GL.DeleteTexture(texid);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
catch { }
|
||||||
|
|
||||||
GC.Collect();
|
GC.Collect();
|
||||||
|
|
||||||
try
|
try
|
||||||
|
|
|
@ -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<string> modelist = new List<string>();
|
||||||
List<Core.Geometry.Point3D>[] position = new List<Core.Geometry.Point3D>[200];
|
List<Core.Geometry.Point3D>[] position = new List<Core.Geometry.Point3D>[200];
|
||||||
int positionindex = 0;
|
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 };
|
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();
|
KMLRoot kml = new KMLRoot();
|
||||||
Folder fldr = new Folder("Log");
|
Folder fldr = new Folder("Log");
|
||||||
|
|
||||||
|
@ -436,7 +404,7 @@ namespace ArdupilotMega
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
LineString ls = new LineString();
|
LineString ls = new LineString();
|
||||||
ls.AltitudeMode = AltitudeMode.absolute;
|
ls.AltitudeMode = altmode;
|
||||||
ls.Extrude = true;
|
ls.Extrude = true;
|
||||||
//ls.Tessellate = true;
|
//ls.Tessellate = true;
|
||||||
|
|
||||||
|
@ -523,7 +491,7 @@ namespace ArdupilotMega
|
||||||
pmplane.visibility = false;
|
pmplane.visibility = false;
|
||||||
|
|
||||||
Model model = mod.model;
|
Model model = mod.model;
|
||||||
model.AltitudeMode = AltitudeMode.absolute;
|
model.AltitudeMode = altmode;
|
||||||
model.Scale.x = 2;
|
model.Scale.x = 2;
|
||||||
model.Scale.y = 2;
|
model.Scale.y = 2;
|
||||||
model.Scale.z = 2;
|
model.Scale.z = 2;
|
||||||
|
@ -545,7 +513,7 @@ namespace ArdupilotMega
|
||||||
catch { }
|
catch { }
|
||||||
|
|
||||||
pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude);
|
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 link = new Link();
|
||||||
link.href = "block_plane_0.dae";
|
link.href = "block_plane_0.dae";
|
||||||
|
|
|
@ -16,6 +16,9 @@ namespace ArdupilotMega
|
||||||
{
|
{
|
||||||
public ICommsSerial BaseStream = new SerialPort();
|
public ICommsSerial BaseStream = new SerialPort();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// used for outbound packet sending
|
||||||
|
/// </summary>
|
||||||
byte packetcount = 0;
|
byte packetcount = 0;
|
||||||
public byte sysid = 0;
|
public byte sysid = 0;
|
||||||
public byte compid = 0;
|
public byte compid = 0;
|
||||||
|
@ -29,6 +32,8 @@ namespace ArdupilotMega
|
||||||
public DateTime lastvalidpacket = DateTime.Now;
|
public DateTime lastvalidpacket = DateTime.Now;
|
||||||
bool oldlogformat = false;
|
bool oldlogformat = false;
|
||||||
|
|
||||||
|
byte mavlinkversion = 0;
|
||||||
|
|
||||||
public PointLatLngAlt[] wps = new PointLatLngAlt[200];
|
public PointLatLngAlt[] wps = new PointLatLngAlt[200];
|
||||||
|
|
||||||
public bool debugmavlink = false;
|
public bool debugmavlink = false;
|
||||||
|
@ -45,7 +50,12 @@ namespace ArdupilotMega
|
||||||
public int bps = 0;
|
public int bps = 0;
|
||||||
public DateTime bpstime = DateTime.Now;
|
public DateTime bpstime = DateTime.Now;
|
||||||
int recvpacketcount = 0;
|
int recvpacketcount = 0;
|
||||||
int packetslost = 0;
|
|
||||||
|
float synclost;
|
||||||
|
float packetslost = 0;
|
||||||
|
float packetsnotlost = 0;
|
||||||
|
DateTime packetlosttimer = DateTime.Now;
|
||||||
|
|
||||||
|
|
||||||
//Stopwatch stopwatch = new Stopwatch();
|
//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])
|
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];
|
sysid = buffer[3];
|
||||||
compid = buffer[4];
|
compid = buffer[4];
|
||||||
recvpacketcount = buffer[2];
|
recvpacketcount = buffer[2];
|
||||||
Console.WriteLine("ID " + sysid + " " + compid);
|
Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + ") ";
|
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") ";
|
||||||
frm.Refresh();
|
frm.Refresh();
|
||||||
if (getparams == true)
|
if (getparams == true)
|
||||||
getParamList();
|
getParamList();
|
||||||
|
@ -190,6 +210,7 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
Console.WriteLine("Done open " + sysid + " " + compid);
|
Console.WriteLine("Done open " + sysid + " " + compid);
|
||||||
|
|
||||||
|
packetslost = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static byte[] StructureToByteArrayEndian(params object[] list)
|
public static byte[] StructureToByteArrayEndian(params object[] list)
|
||||||
|
@ -331,12 +352,28 @@ namespace ArdupilotMega
|
||||||
/// <param name="indata">struct of data</param>
|
/// <param name="indata">struct of data</param>
|
||||||
public void generatePacket(byte messageType, object indata)
|
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);
|
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
|
||||||
byte[] packet = new byte[data.Length + 6 + 2];
|
byte[] packet = new byte[data.Length + 6 + 2];
|
||||||
|
|
||||||
|
if (mavlinkversion == 3)
|
||||||
|
{
|
||||||
|
packet[0] = 254;
|
||||||
|
}
|
||||||
|
else if (mavlinkversion == 2)
|
||||||
|
{
|
||||||
packet[0] = (byte)'U';
|
packet[0] = (byte)'U';
|
||||||
|
}
|
||||||
packet[1] = (byte)data.Length;
|
packet[1] = (byte)data.Length;
|
||||||
packet[2] = packetcount;
|
packet[2] = packetcount;
|
||||||
packet[3] = 255; // this is always 255 - MYGCS
|
packet[3] = 255; // this is always 255 - MYGCS
|
||||||
|
@ -351,6 +388,12 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
|
|
||||||
ushort checksum = crc_calculate(packet, packet[1] + 6);
|
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_a = (byte)(checksum & 0xFF); ///< High byte
|
||||||
byte ck_b = (byte)(checksum >> 8); ///< Low byte
|
byte ck_b = (byte)(checksum >> 8); ///< Low byte
|
||||||
|
|
||||||
|
@ -425,6 +468,8 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
|
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
|
||||||
|
|
||||||
|
modifyParamForDisplay(false, paramname, ref value);
|
||||||
|
|
||||||
Array.Resize(ref temp, 15);
|
Array.Resize(ref temp, 15);
|
||||||
|
|
||||||
req.param_id = temp;
|
req.param_id = temp;
|
||||||
|
@ -458,7 +503,31 @@ namespace ArdupilotMega
|
||||||
{
|
{
|
||||||
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
|
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;
|
MainV2.givecomport = false;
|
||||||
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
|
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
|
||||||
return true;
|
return true;
|
||||||
|
@ -559,6 +628,8 @@ namespace ArdupilotMega
|
||||||
st = st.Substring(0, pos);
|
st = st.Substring(0, pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
modifyParamForDisplay(true, st, ref par.param_value);
|
||||||
|
|
||||||
param[st] = (par.param_value);
|
param[st] = (par.param_value);
|
||||||
|
|
||||||
a++;
|
a++;
|
||||||
|
@ -575,17 +646,21 @@ namespace ArdupilotMega
|
||||||
return param;
|
return param;
|
||||||
}
|
}
|
||||||
|
|
||||||
public _param getParam()
|
void modifyParamForDisplay(bool fromapm, string paramname, ref float value)
|
||||||
{
|
{
|
||||||
throw new Exception("getParam Not implemented");
|
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")
|
||||||
_param temp = new _param();
|
|| paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX"))
|
||||||
|
{
|
||||||
temp.name = "none";
|
if (fromapm)
|
||||||
temp.value = 0;
|
{
|
||||||
|
value /= 100.0f;
|
||||||
return temp;
|
}
|
||||||
*/
|
else
|
||||||
|
{
|
||||||
|
value *= 100.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
@ -951,6 +1026,8 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
req.seq = index;
|
req.seq = index;
|
||||||
|
|
||||||
|
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
|
||||||
|
|
||||||
// request
|
// request
|
||||||
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
|
||||||
|
|
||||||
|
@ -972,11 +1049,14 @@ namespace ArdupilotMega
|
||||||
MainV2.givecomport = false;
|
MainV2.givecomport = false;
|
||||||
throw new Exception("Timeout on read - getWP");
|
throw new Exception("Timeout on read - getWP");
|
||||||
}
|
}
|
||||||
|
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
|
||||||
byte[] buffer = readPacket();
|
byte[] buffer = readPacket();
|
||||||
|
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
|
||||||
if (buffer.Length > 5)
|
if (buffer.Length > 5)
|
||||||
{
|
{
|
||||||
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT)
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT)
|
||||||
{
|
{
|
||||||
|
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
|
||||||
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
|
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
|
||||||
|
|
||||||
object temp = (object)wp;
|
object temp = (object)wp;
|
||||||
|
@ -1070,7 +1150,7 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
|
Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1412,12 +1492,13 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
MainV2.cs.datetime = DateTime.Now;
|
||||||
temp[count] = (byte)BaseStream.ReadByte();
|
temp[count] = (byte)BaseStream.ReadByte();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; }
|
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')
|
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n')
|
||||||
{
|
{
|
||||||
|
@ -1432,7 +1513,7 @@ namespace ArdupilotMega
|
||||||
// reset count on valid packet
|
// reset count on valid packet
|
||||||
readcount = 0;
|
readcount = 0;
|
||||||
|
|
||||||
if (temp[0] == 'U')
|
if (temp[0] == 'U' || temp[0] == 254)
|
||||||
{
|
{
|
||||||
length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length
|
length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length
|
||||||
if (count >= 5 || logreadmode)
|
if (count >= 5 || logreadmode)
|
||||||
|
@ -1455,16 +1536,18 @@ namespace ArdupilotMega
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
int to = 0;
|
int to = 0;
|
||||||
while (BaseStream.BytesToRead < length)
|
while (BaseStream.BytesToRead < (length - 4))
|
||||||
{
|
{
|
||||||
if (to > 1000)
|
if (to > 1000)
|
||||||
{
|
{
|
||||||
Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
|
Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
System.Threading.Thread.Sleep(2);
|
System.Threading.Thread.Sleep(1);
|
||||||
System.Windows.Forms.Application.DoEvents();
|
System.Windows.Forms.Application.DoEvents();
|
||||||
to++;
|
to++;
|
||||||
|
|
||||||
|
//Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead);
|
||||||
}
|
}
|
||||||
int read = BaseStream.Read(temp, 6, length - 4);
|
int read = BaseStream.Read(temp, 6, length - 4);
|
||||||
}
|
}
|
||||||
|
@ -1484,9 +1567,18 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
Array.Resize<byte>(ref temp, count);
|
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)
|
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
|
bps2 = bps1; // prev sec
|
||||||
bps1 = 0; // current sec
|
bps1 = 0; // current sec
|
||||||
bpstime = DateTime.Now;
|
bpstime = DateTime.Now;
|
||||||
|
@ -1498,11 +1590,16 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet
|
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);
|
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))
|
if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff))
|
||||||
{
|
{
|
||||||
int packetno = 0;
|
int packetno = 0;
|
||||||
|
@ -1516,13 +1613,27 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
try
|
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))
|
if (temp[2] != ((recvpacketcount + 1) % 0x100))
|
||||||
{
|
{
|
||||||
Console.WriteLine("lost {0}", temp[2]);
|
synclost++; // actualy sync loss's
|
||||||
packetslost++; // 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];
|
recvpacketcount = temp[2];
|
||||||
|
|
||||||
//MAVLINK_MSG_ID_GPS_STATUS
|
//MAVLINK_MSG_ID_GPS_STATUS
|
||||||
|
@ -1672,6 +1783,8 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
lastlogread = date1;
|
lastlogread = date1;
|
||||||
|
|
||||||
|
MainV2.cs.datetime = lastlogread;
|
||||||
|
|
||||||
int length = 5;
|
int length = 5;
|
||||||
int a = 0;
|
int a = 0;
|
||||||
while (a < length)
|
while (a < length)
|
||||||
|
@ -1751,6 +1864,34 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
|
|
||||||
public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)
|
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);
|
int len = Marshal.SizeOf(obj);
|
||||||
|
|
||||||
|
@ -1779,51 +1920,12 @@ namespace ArdupilotMega
|
||||||
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
||||||
|
|
||||||
if (typeCode != TypeCode.Object)
|
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));
|
Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue));
|
||||||
reversestartoffset += Marshal.SizeOf(fieldValue);
|
reversestartoffset += Marshal.SizeOf(fieldValue);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
/*
|
|
||||||
for (int c = 0; c < ((byte[])fieldValue).Length;c++)
|
|
||||||
Marshal.WriteByte(i, c, bytearray[reversestartoffset + c]);
|
|
||||||
*/
|
|
||||||
reversestartoffset += ((byte[])fieldValue).Length;
|
reversestartoffset += ((byte[])fieldValue).Length;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -7,159 +7,7 @@ namespace ArdupilotMega
|
||||||
{
|
{
|
||||||
partial class MAVLink
|
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
|
|
||||||
};
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -76,6 +76,14 @@ namespace ArdupilotMega
|
||||||
//System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US");
|
//System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US");
|
||||||
//System.Threading.Thread.CurrentThread.CurrentCulture = 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");
|
var t = Type.GetType("Mono.Runtime");
|
||||||
MAC = (t != null);
|
MAC = (t != null);
|
||||||
|
|
||||||
|
@ -153,6 +161,15 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
catch (Exception e) { MessageBox.Show("A Major error has occured : " + e.ToString()); this.Close(); }
|
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();
|
changeunits();
|
||||||
|
|
||||||
try
|
try
|
||||||
|
@ -867,6 +884,8 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
DateTime speechcustomtime = DateTime.Now;
|
DateTime speechcustomtime = DateTime.Now;
|
||||||
|
|
||||||
|
DateTime linkqualitytime = DateTime.Now;
|
||||||
|
|
||||||
while (serialthread)
|
while (serialthread)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
|
@ -919,6 +938,21 @@ namespace ArdupilotMega
|
||||||
speechcustomtime = DateTime.Now;
|
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))
|
if (speechenable && talk != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
|
||||||
{
|
{
|
||||||
float warnalt = float.MaxValue;
|
float warnalt = float.MaxValue;
|
||||||
|
@ -939,7 +973,7 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
if (heatbeatsend.Second != DateTime.Now.Second)
|
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();
|
MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t();
|
||||||
|
|
||||||
|
@ -952,11 +986,13 @@ namespace ArdupilotMega
|
||||||
}
|
}
|
||||||
|
|
||||||
// data loss warning
|
// data loss warning
|
||||||
if (speechenable && talk != null && (DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
|
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
|
||||||
{
|
{
|
||||||
|
if (speechenable && talk != null) {
|
||||||
if (MainV2.talk.State == SynthesizerState.Ready)
|
if (MainV2.talk.State == SynthesizerState.Ready)
|
||||||
MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
|
MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
//Console.WriteLine(comPort.BaseStream.BytesToRead);
|
//Console.WriteLine(comPort.BaseStream.BytesToRead);
|
||||||
|
|
||||||
|
@ -1459,6 +1495,12 @@ namespace ArdupilotMega
|
||||||
MainV2.comPort.Open(false);
|
MainV2.comPort.Open(false);
|
||||||
return true;
|
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
|
if (keyData == (Keys.Control | Keys.J)) // for jani
|
||||||
{
|
{
|
||||||
string data = "!!";
|
string data = "!!";
|
||||||
|
|
|
@ -431,13 +431,14 @@ namespace ArdupilotMega
|
||||||
// bar moves to 100 % in this step
|
// bar moves to 100 % in this step
|
||||||
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
|
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();
|
byte[] packet = mine.readPacket();
|
||||||
string text = "";
|
string text = "";
|
||||||
mine.DebugPacket(packet, ref text);
|
mine.DebugPacket(packet, ref text);
|
||||||
|
|
||||||
sw.Write(text);
|
sw.Write(mine.lastlogread +" "+text);
|
||||||
}
|
}
|
||||||
|
|
||||||
sw.Close();
|
sw.Close();
|
||||||
|
|
|
@ -50,7 +50,7 @@ namespace ArdupilotMega
|
||||||
|
|
||||||
if (!this.Enabled)
|
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);
|
gr.FillRectangle(brush, 0, 0, this.Width, this.Height);
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,5 +34,5 @@ using System.Resources;
|
||||||
// by using the '*' as shown below:
|
// by using the '*' as shown below:
|
||||||
// [assembly: AssemblyVersion("1.0.*")]
|
// [assembly: AssemblyVersion("1.0.*")]
|
||||||
[assembly: AssemblyVersion("1.0.0.0")]
|
[assembly: AssemblyVersion("1.0.0.0")]
|
||||||
[assembly: AssemblyFileVersion("1.0.67")]
|
[assembly: AssemblyFileVersion("1.0.70")]
|
||||||
[assembly: NeutralResourcesLanguageAttribute("")]
|
[assembly: NeutralResourcesLanguageAttribute("")]
|
||||||
|
|
|
@ -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.
|
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_MREV|| || ||0|| || ||Description coming soon||
|
||||||
||SYSID_SW_TYPE|| || ||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.||
|
||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||
|
||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, 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, 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, 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, 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, 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, 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, 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, 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, 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, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL||
|
|
@ -32,22 +32,11 @@
|
||||||
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup));
|
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup));
|
||||||
this.tabControl1 = new System.Windows.Forms.TabControl();
|
this.tabControl1 = new System.Windows.Forms.TabControl();
|
||||||
this.tabReset = new System.Windows.Forms.TabPage();
|
this.tabReset = new System.Windows.Forms.TabPage();
|
||||||
this.BUT_reset = new ArdupilotMega.MyButton();
|
|
||||||
this.tabRadioIn = new System.Windows.Forms.TabPage();
|
this.tabRadioIn = new System.Windows.Forms.TabPage();
|
||||||
this.CHK_revch3 = new System.Windows.Forms.CheckBox();
|
this.CHK_revch3 = new System.Windows.Forms.CheckBox();
|
||||||
this.CHK_revch4 = new System.Windows.Forms.CheckBox();
|
this.CHK_revch4 = new System.Windows.Forms.CheckBox();
|
||||||
this.CHK_revch2 = new System.Windows.Forms.CheckBox();
|
this.CHK_revch2 = new System.Windows.Forms.CheckBox();
|
||||||
this.CHK_revch1 = 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.tabModes = new System.Windows.Forms.TabPage();
|
||||||
this.label14 = new System.Windows.Forms.Label();
|
this.label14 = new System.Windows.Forms.Label();
|
||||||
this.LBL_flightmodepwm = 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.CMB_fmode2 = new System.Windows.Forms.ComboBox();
|
||||||
this.label1 = new System.Windows.Forms.Label();
|
this.label1 = new System.Windows.Forms.Label();
|
||||||
this.CMB_fmode1 = new System.Windows.Forms.ComboBox();
|
this.CMB_fmode1 = new System.Windows.Forms.ComboBox();
|
||||||
this.BUT_SaveModes = new ArdupilotMega.MyButton();
|
|
||||||
this.tabHardware = new System.Windows.Forms.TabPage();
|
this.tabHardware = new System.Windows.Forms.TabPage();
|
||||||
this.linkLabelmagdec = new System.Windows.Forms.LinkLabel();
|
this.linkLabelmagdec = new System.Windows.Forms.LinkLabel();
|
||||||
this.label106 = new System.Windows.Forms.Label();
|
this.label106 = new System.Windows.Forms.Label();
|
||||||
|
@ -90,7 +78,6 @@
|
||||||
this.pictureBox1 = new System.Windows.Forms.PictureBox();
|
this.pictureBox1 = new System.Windows.Forms.PictureBox();
|
||||||
this.tabArducopter = new System.Windows.Forms.TabPage();
|
this.tabArducopter = new System.Windows.Forms.TabPage();
|
||||||
this.label28 = new System.Windows.Forms.Label();
|
this.label28 = new System.Windows.Forms.Label();
|
||||||
this.BUT_levelac2 = new ArdupilotMega.MyButton();
|
|
||||||
this.label16 = new System.Windows.Forms.Label();
|
this.label16 = new System.Windows.Forms.Label();
|
||||||
this.label15 = new System.Windows.Forms.Label();
|
this.label15 = new System.Windows.Forms.Label();
|
||||||
this.pictureBoxQuadX = new System.Windows.Forms.PictureBox();
|
this.pictureBoxQuadX = new System.Windows.Forms.PictureBox();
|
||||||
|
@ -119,6 +106,20 @@
|
||||||
this.HS2_REV = new System.Windows.Forms.CheckBox();
|
this.HS2_REV = new System.Windows.Forms.CheckBox();
|
||||||
this.HS1_REV = new System.Windows.Forms.CheckBox();
|
this.HS1_REV = new System.Windows.Forms.CheckBox();
|
||||||
this.label17 = new System.Windows.Forms.Label();
|
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_saveheliconfig = new ArdupilotMega.MyButton();
|
||||||
this.BUT_0collective = new ArdupilotMega.MyButton();
|
this.BUT_0collective = new ArdupilotMega.MyButton();
|
||||||
this.HS4 = new ArdupilotMega.VerticalProgressBar2();
|
this.HS4 = new ArdupilotMega.VerticalProgressBar2();
|
||||||
|
@ -128,11 +129,15 @@
|
||||||
this.HS2_TRIM = new ArdupilotMega.MyTrackBar();
|
this.HS2_TRIM = new ArdupilotMega.MyTrackBar();
|
||||||
this.HS1_TRIM = new ArdupilotMega.MyTrackBar();
|
this.HS1_TRIM = new ArdupilotMega.MyTrackBar();
|
||||||
this.Gservoloc = new AGaugeApp.AGauge();
|
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.tabControl1.SuspendLayout();
|
||||||
this.tabReset.SuspendLayout();
|
this.tabReset.SuspendLayout();
|
||||||
this.tabRadioIn.SuspendLayout();
|
this.tabRadioIn.SuspendLayout();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
|
|
||||||
this.tabModes.SuspendLayout();
|
this.tabModes.SuspendLayout();
|
||||||
this.tabHardware.SuspendLayout();
|
this.tabHardware.SuspendLayout();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit();
|
||||||
|
@ -143,6 +148,7 @@
|
||||||
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit();
|
||||||
this.tabHeli.SuspendLayout();
|
this.tabHeli.SuspendLayout();
|
||||||
|
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).BeginInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit();
|
((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit();
|
||||||
|
@ -169,14 +175,6 @@
|
||||||
this.tabReset.Name = "tabReset";
|
this.tabReset.Name = "tabReset";
|
||||||
this.tabReset.UseVisualStyleBackColor = true;
|
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
|
// tabRadioIn
|
||||||
//
|
//
|
||||||
this.tabRadioIn.Controls.Add(this.CHK_revch3);
|
this.tabRadioIn.Controls.Add(this.CHK_revch3);
|
||||||
|
@ -224,139 +222,14 @@
|
||||||
this.CHK_revch1.UseVisualStyleBackColor = true;
|
this.CHK_revch1.UseVisualStyleBackColor = true;
|
||||||
this.CHK_revch1.CheckedChanged += new System.EventHandler(this.CHK_revch1_CheckedChanged);
|
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
|
// 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.label14);
|
||||||
this.tabModes.Controls.Add(this.LBL_flightmodepwm);
|
this.tabModes.Controls.Add(this.LBL_flightmodepwm);
|
||||||
this.tabModes.Controls.Add(this.label13);
|
this.tabModes.Controls.Add(this.label13);
|
||||||
|
@ -519,13 +392,6 @@
|
||||||
resources.ApplyResources(this.CMB_fmode1, "CMB_fmode1");
|
resources.ApplyResources(this.CMB_fmode1, "CMB_fmode1");
|
||||||
this.CMB_fmode1.Name = "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
|
// tabHardware
|
||||||
//
|
//
|
||||||
this.tabHardware.BackColor = System.Drawing.Color.DarkRed;
|
this.tabHardware.BackColor = System.Drawing.Color.DarkRed;
|
||||||
|
@ -661,11 +527,11 @@
|
||||||
// tabArducopter
|
// tabArducopter
|
||||||
//
|
//
|
||||||
this.tabArducopter.Controls.Add(this.label28);
|
this.tabArducopter.Controls.Add(this.label28);
|
||||||
this.tabArducopter.Controls.Add(this.BUT_levelac2);
|
|
||||||
this.tabArducopter.Controls.Add(this.label16);
|
this.tabArducopter.Controls.Add(this.label16);
|
||||||
this.tabArducopter.Controls.Add(this.label15);
|
this.tabArducopter.Controls.Add(this.label15);
|
||||||
this.tabArducopter.Controls.Add(this.pictureBoxQuadX);
|
this.tabArducopter.Controls.Add(this.pictureBoxQuadX);
|
||||||
this.tabArducopter.Controls.Add(this.pictureBoxQuad);
|
this.tabArducopter.Controls.Add(this.pictureBoxQuad);
|
||||||
|
this.tabArducopter.Controls.Add(this.BUT_levelac2);
|
||||||
resources.ApplyResources(this.tabArducopter, "tabArducopter");
|
resources.ApplyResources(this.tabArducopter, "tabArducopter");
|
||||||
this.tabArducopter.Name = "tabArducopter";
|
this.tabArducopter.Name = "tabArducopter";
|
||||||
this.tabArducopter.UseVisualStyleBackColor = true;
|
this.tabArducopter.UseVisualStyleBackColor = true;
|
||||||
|
@ -675,13 +541,6 @@
|
||||||
resources.ApplyResources(this.label28, "label28");
|
resources.ApplyResources(this.label28, "label28");
|
||||||
this.label28.Name = "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
|
// label16
|
||||||
//
|
//
|
||||||
resources.ApplyResources(this.label16, "label16");
|
resources.ApplyResources(this.label16, "label16");
|
||||||
|
@ -879,6 +738,159 @@
|
||||||
resources.ApplyResources(this.label17, "label17");
|
resources.ApplyResources(this.label17, "label17");
|
||||||
this.label17.Name = "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
|
// BUT_saveheliconfig
|
||||||
//
|
//
|
||||||
resources.ApplyResources(this.BUT_saveheliconfig, "BUT_saveheliconfig");
|
resources.ApplyResources(this.BUT_saveheliconfig, "BUT_saveheliconfig");
|
||||||
|
@ -925,8 +937,8 @@
|
||||||
//
|
//
|
||||||
// HS4_TRIM
|
// HS4_TRIM
|
||||||
//
|
//
|
||||||
this.HS4_TRIM.LargeChange = 1000;
|
|
||||||
resources.ApplyResources(this.HS4_TRIM, "HS4_TRIM");
|
resources.ApplyResources(this.HS4_TRIM, "HS4_TRIM");
|
||||||
|
this.HS4_TRIM.LargeChange = 1000;
|
||||||
this.HS4_TRIM.Maximum = 2000D;
|
this.HS4_TRIM.Maximum = 2000D;
|
||||||
this.HS4_TRIM.Minimum = 1000D;
|
this.HS4_TRIM.Minimum = 1000D;
|
||||||
this.HS4_TRIM.Name = "HS4_TRIM";
|
this.HS4_TRIM.Name = "HS4_TRIM";
|
||||||
|
@ -937,8 +949,8 @@
|
||||||
//
|
//
|
||||||
// HS3_TRIM
|
// HS3_TRIM
|
||||||
//
|
//
|
||||||
this.HS3_TRIM.LargeChange = 1000;
|
|
||||||
resources.ApplyResources(this.HS3_TRIM, "HS3_TRIM");
|
resources.ApplyResources(this.HS3_TRIM, "HS3_TRIM");
|
||||||
|
this.HS3_TRIM.LargeChange = 1000;
|
||||||
this.HS3_TRIM.Maximum = 2000D;
|
this.HS3_TRIM.Maximum = 2000D;
|
||||||
this.HS3_TRIM.Minimum = 1000D;
|
this.HS3_TRIM.Minimum = 1000D;
|
||||||
this.HS3_TRIM.Name = "HS3_TRIM";
|
this.HS3_TRIM.Name = "HS3_TRIM";
|
||||||
|
@ -949,8 +961,8 @@
|
||||||
//
|
//
|
||||||
// HS2_TRIM
|
// HS2_TRIM
|
||||||
//
|
//
|
||||||
this.HS2_TRIM.LargeChange = 1000;
|
|
||||||
resources.ApplyResources(this.HS2_TRIM, "HS2_TRIM");
|
resources.ApplyResources(this.HS2_TRIM, "HS2_TRIM");
|
||||||
|
this.HS2_TRIM.LargeChange = 1000;
|
||||||
this.HS2_TRIM.Maximum = 2000D;
|
this.HS2_TRIM.Maximum = 2000D;
|
||||||
this.HS2_TRIM.Minimum = 1000D;
|
this.HS2_TRIM.Minimum = 1000D;
|
||||||
this.HS2_TRIM.Name = "HS2_TRIM";
|
this.HS2_TRIM.Name = "HS2_TRIM";
|
||||||
|
@ -961,8 +973,8 @@
|
||||||
//
|
//
|
||||||
// HS1_TRIM
|
// HS1_TRIM
|
||||||
//
|
//
|
||||||
this.HS1_TRIM.LargeChange = 1000;
|
|
||||||
resources.ApplyResources(this.HS1_TRIM, "HS1_TRIM");
|
resources.ApplyResources(this.HS1_TRIM, "HS1_TRIM");
|
||||||
|
this.HS1_TRIM.LargeChange = 1000;
|
||||||
this.HS1_TRIM.Maximum = 2000D;
|
this.HS1_TRIM.Maximum = 2000D;
|
||||||
this.HS1_TRIM.Minimum = 1000D;
|
this.HS1_TRIM.Minimum = 1000D;
|
||||||
this.HS1_TRIM.Name = "HS1_TRIM";
|
this.HS1_TRIM.Name = "HS1_TRIM";
|
||||||
|
@ -1114,6 +1126,42 @@
|
||||||
this.Gservoloc.Value2 = 180F;
|
this.Gservoloc.Value2 = 180F;
|
||||||
this.Gservoloc.Value3 = 0F;
|
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
|
// Setup
|
||||||
//
|
//
|
||||||
resources.ApplyResources(this, "$this");
|
resources.ApplyResources(this, "$this");
|
||||||
|
@ -1126,7 +1174,6 @@
|
||||||
this.tabReset.ResumeLayout(false);
|
this.tabReset.ResumeLayout(false);
|
||||||
this.tabRadioIn.ResumeLayout(false);
|
this.tabRadioIn.ResumeLayout(false);
|
||||||
this.tabRadioIn.PerformLayout();
|
this.tabRadioIn.PerformLayout();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
|
|
||||||
this.tabModes.ResumeLayout(false);
|
this.tabModes.ResumeLayout(false);
|
||||||
this.tabModes.PerformLayout();
|
this.tabModes.PerformLayout();
|
||||||
this.tabHardware.ResumeLayout(false);
|
this.tabHardware.ResumeLayout(false);
|
||||||
|
@ -1141,6 +1188,7 @@
|
||||||
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit();
|
((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit();
|
||||||
this.tabHeli.ResumeLayout(false);
|
this.tabHeli.ResumeLayout(false);
|
||||||
this.tabHeli.PerformLayout();
|
this.tabHeli.PerformLayout();
|
||||||
|
((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).EndInit();
|
((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).EndInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit();
|
((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit();
|
||||||
((System.ComponentModel.ISupportInitialize)(this.HS2_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_revch4;
|
||||||
private System.Windows.Forms.CheckBox CHK_revch2;
|
private System.Windows.Forms.CheckBox CHK_revch2;
|
||||||
private System.Windows.Forms.CheckBox CHK_revch1;
|
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;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
|
@ -231,7 +231,6 @@ namespace ArdupilotMega.Setup
|
||||||
|
|
||||||
MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc);
|
MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc);
|
||||||
|
|
||||||
MainV2.comPort.param = MainV2.comPort.getParamList();
|
|
||||||
if (Configuration != null)
|
if (Configuration != null)
|
||||||
{
|
{
|
||||||
Configuration.startup = true;
|
Configuration.startup = true;
|
||||||
|
@ -253,6 +252,13 @@ namespace ArdupilotMega.Setup
|
||||||
{
|
{
|
||||||
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) // APM
|
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_fmode1.Items.Clear();
|
||||||
CMB_fmode2.Items.Clear();
|
CMB_fmode2.Items.Clear();
|
||||||
CMB_fmode3.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("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("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));
|
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"); }
|
catch { MessageBox.Show("Failed to set Flight modes"); }
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -343,16 +343,16 @@
|
||||||
<value>NoControl</value>
|
<value>NoControl</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
|
<data name="label1.Location" type="System.Drawing.Point, System.Drawing">
|
||||||
<value>101, 165</value>
|
<value>113, 167</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
|
<data name="label1.Size" type="System.Drawing.Size, System.Drawing">
|
||||||
<value>79, 13</value>
|
<value>56, 13</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.TabIndex" type="System.Int32, mscorlib">
|
<data name="label1.TabIndex" type="System.Int32, mscorlib">
|
||||||
<value>8</value>
|
<value>8</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label1.Text" xml:space="preserve">
|
<data name="label1.Text" xml:space="preserve">
|
||||||
<value>ArduPilot Mega</value>
|
<value>ArduPlane</value>
|
||||||
</data>
|
</data>
|
||||||
<data name=">>label1.Name" xml:space="preserve">
|
<data name=">>label1.Name" xml:space="preserve">
|
||||||
<value>label1</value>
|
<value>label1</value>
|
||||||
|
@ -403,16 +403,16 @@
|
||||||
<value>NoControl</value>
|
<value>NoControl</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
|
<data name="label3.Location" type="System.Drawing.Point, System.Drawing">
|
||||||
<value>57, 362</value>
|
<value>74, 361</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
|
<data name="label3.Size" type="System.Drawing.Size, System.Drawing">
|
||||||
<value>168, 13</value>
|
<value>142, 13</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.TabIndex" type="System.Int32, mscorlib">
|
<data name="label3.TabIndex" type="System.Int32, mscorlib">
|
||||||
<value>10</value>
|
<value>10</value>
|
||||||
</data>
|
</data>
|
||||||
<data name="label3.Text" xml:space="preserve">
|
<data name="label3.Text" xml:space="preserve">
|
||||||
<value>ArduPilot Mega (Xplane simulator)</value>
|
<value>ArduPlane (Xplane simulator)</value>
|
||||||
</data>
|
</data>
|
||||||
<data name=">>label3.Name" xml:space="preserve">
|
<data name=">>label3.Name" xml:space="preserve">
|
||||||
<value>label3</value>
|
<value>label3</value>
|
||||||
|
|
|
@ -22,9 +22,8 @@
|
||||||
<F1>WP Dist</F1>
|
<F1>WP Dist</F1>
|
||||||
<F2>WP Verify</F2>
|
<F2>WP Verify</F2>
|
||||||
<F3>Target Bear</F3>
|
<F3>Target Bear</F3>
|
||||||
<F4>Nav Bear</F4>
|
<F4>Long Err</F4>
|
||||||
<F5>Long Err</F5>
|
<F5>Lat Err</F5>
|
||||||
<F6>Lat Err</F6>
|
|
||||||
</NTUN>
|
</NTUN>
|
||||||
<CTUN>
|
<CTUN>
|
||||||
<F1>Yaw Sensor</F1>
|
<F1>Yaw Sensor</F1>
|
||||||
|
@ -56,6 +55,13 @@
|
||||||
<F5>Accel Y</F5>
|
<F5>Accel Y</F5>
|
||||||
<F6>Accel Z</F6>
|
<F6>Accel Z</F6>
|
||||||
</RAW>
|
</RAW>
|
||||||
|
<CURR>
|
||||||
|
<F1>Throttle in</F1>
|
||||||
|
<F2>Throttle intergrator</F2>
|
||||||
|
<F3>Voltage</F3>
|
||||||
|
<F4>Current</F4>
|
||||||
|
<F5>Current total</F5>
|
||||||
|
</CURR>
|
||||||
</AC2>
|
</AC2>
|
||||||
<!-- -->
|
<!-- -->
|
||||||
<APM>
|
<APM>
|
||||||
|
|
|
@ -22,9 +22,8 @@
|
||||||
<F1>WP Dist</F1>
|
<F1>WP Dist</F1>
|
||||||
<F2>WP Verify</F2>
|
<F2>WP Verify</F2>
|
||||||
<F3>Target Bear</F3>
|
<F3>Target Bear</F3>
|
||||||
<F4>Nav Bear</F4>
|
<F4>Long Err</F4>
|
||||||
<F5>Long Err</F5>
|
<F5>Lat Err</F5>
|
||||||
<F6>Lat Err</F6>
|
|
||||||
</NTUN>
|
</NTUN>
|
||||||
<CTUN>
|
<CTUN>
|
||||||
<F1>Yaw Sensor</F1>
|
<F1>Yaw Sensor</F1>
|
||||||
|
@ -56,6 +55,13 @@
|
||||||
<F5>Accel Y</F5>
|
<F5>Accel Y</F5>
|
||||||
<F6>Accel Z</F6>
|
<F6>Accel Z</F6>
|
||||||
</RAW>
|
</RAW>
|
||||||
|
<CURR>
|
||||||
|
<F1>Throttle in</F1>
|
||||||
|
<F2>Throttle intergrator</F2>
|
||||||
|
<F3>Voltage</F3>
|
||||||
|
<F4>Current</F4>
|
||||||
|
<F5>Current total</F5>
|
||||||
|
</CURR>
|
||||||
</AC2>
|
</AC2>
|
||||||
<!-- -->
|
<!-- -->
|
||||||
<APM>
|
<APM>
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
File diff suppressed because it is too large
Load Diff
|
@ -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;
|
|
@ -1,12 +1,27 @@
|
||||||
$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/message_definitions/";
|
$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/";
|
||||||
#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/pixhawk-mavlink-c91adfb/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 $!;
|
opendir(DIR,$dir) || die print $!;
|
||||||
|
@files2 = readdir(DIR);
|
||||||
|
closedir(DIR);
|
||||||
|
|
||||||
|
opendir(DIR,$dir2) || die print $!;
|
||||||
@files = readdir(DIR);
|
@files = readdir(DIR);
|
||||||
closedir(DIR);
|
closedir(DIR);
|
||||||
|
|
||||||
|
push(@files,@files2);
|
||||||
|
|
||||||
|
|
||||||
|
push(@files,"../mavlink_types.h");
|
||||||
|
|
||||||
open(OUT,">MAVLinkTypes.cs");
|
open(OUT,">MAVLinkTypes.cs");
|
||||||
|
|
||||||
|
$crcs = 0;
|
||||||
|
|
||||||
print OUT <<EOF;
|
print OUT <<EOF;
|
||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
@ -20,88 +35,72 @@ namespace ArdupilotMega
|
||||||
EOF
|
EOF
|
||||||
|
|
||||||
foreach $file (@files) {
|
foreach $file (@files) {
|
||||||
|
print "$file\n";
|
||||||
|
|
||||||
if (!($file eq "common.xml" || $file eq "ardupilotmega.xml"))
|
if ($done{$file} == 1) {
|
||||||
{
|
|
||||||
next;
|
next;
|
||||||
}
|
}
|
||||||
|
|
||||||
print "$file\n";
|
$done{$file} = 1;
|
||||||
|
|
||||||
open(F,$dir.$file);
|
open(F,$dir.$file) || open(F,$dir2.$file);
|
||||||
|
|
||||||
$start = 0;
|
$start = 0;
|
||||||
|
|
||||||
while ($line = <F>) {
|
while ($line = <F>) {
|
||||||
if ($line =~ /enum name="(MAV_.*)"/) {
|
if ($line =~ /(MAVLINK_MESSAGE_LENGTHS|MAVLINK_MESSAGE_CRCS) (.*)/ && $crcs < 2) {
|
||||||
$start = 1;
|
print OUT "\t\tpublic byte[] $1 = new byte[] $2;\n";
|
||||||
print OUT "\t\tpublic enum $1\n\t\t{ \n";
|
$crcs++;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ($line =~ /<message id="([0-9]+)" name="([^"]+)">/) {
|
if ($line =~ /enum (MAV_.*)/) {
|
||||||
$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;
|
$start = 1;
|
||||||
|
print OUT "\t\tpublic ";
|
||||||
|
}
|
||||||
|
|
||||||
|
if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) {
|
||||||
|
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
|
||||||
|
$no = $2;
|
||||||
|
}
|
||||||
|
if ($line =~ /typedef struct(.*)/) {
|
||||||
|
if ($1 =~ /__mavlink_system|param_union/) {
|
||||||
|
last;
|
||||||
|
}
|
||||||
|
$start = 1;
|
||||||
|
print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n";
|
||||||
#__mavlink_gps_raw_t
|
#__mavlink_gps_raw_t
|
||||||
$structs[$no] = "__mavlink_".$name."_t";
|
$structs[$no] = $1;
|
||||||
} # __mavlink_heartbeat_t
|
}
|
||||||
|
if ($start) {
|
||||||
$line =~ s/MAV_CMD_NAV_//;
|
$line =~ s/MAV_CMD_NAV_//;
|
||||||
|
|
||||||
$line =~ s/MAV_CMD_//;
|
$line =~ s/MAV_CMD_//;
|
||||||
|
|
||||||
if ($line =~ /<entry value="([0-9]+)" name="([^"]+)">/)
|
$line =~ s/typedef/public/;
|
||||||
{
|
$line =~ s/uint8_t/public byte/;
|
||||||
|
$line =~ s/int8_t/public byte/;
|
||||||
|
$line =~ s/^\s+float/public float/;
|
||||||
|
$line =~ s/uint16_t/public ushort/;
|
||||||
|
$line =~ s/uint32_t/public uint/;
|
||||||
|
$line =~ s/uint64_t/public ulong/;
|
||||||
|
$line =~ s/int16_t/public short/;
|
||||||
|
$line =~ s/int32_t/public int/;
|
||||||
|
$line =~ s/int64_t/public long/;
|
||||||
|
$line =~ s/typedef/public/;
|
||||||
|
|
||||||
|
$line =~ s/}.*/};\n/;
|
||||||
|
|
||||||
print OUT "\t\t\t$2 = $1,\n";
|
if ($line =~ /\[(.*)\].*;/) { # array
|
||||||
|
print OUT "\t\t[MarshalAs(
|
||||||
|
UnmanagedType.ByValArray,
|
||||||
|
SizeConst=". $1 .")] \n";
|
||||||
|
$line =~ s/\[.*\]//;
|
||||||
|
$line =~ s/public\s+([^\s]+)/public $1\[\]/o;
|
||||||
}
|
}
|
||||||
|
|
||||||
#<field type="uint8_t" name="type">
|
print OUT "\t\t".$line;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
if ($line =~ /}/) {
|
||||||
print OUT "\t\t\t$type $name; ///< $desc\n";
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) {
|
|
||||||
print OUT "\t\t};\n\n";
|
|
||||||
$start = 0;
|
$start = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -129,4 +128,6 @@ EOF
|
||||||
|
|
||||||
close OUT;
|
close OUT;
|
||||||
|
|
||||||
|
<STDIN>;
|
||||||
|
|
||||||
1;
|
1;
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -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);
|
||||||
|
}
|
|
@ -0,0 +1,4 @@
|
||||||
|
#
|
||||||
|
# Trivial makefile for building APM
|
||||||
|
#
|
||||||
|
include ../../libraries/AP_Common/Arduino.mk
|
|
@ -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
|
|
@ -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))
|
||||||
|
|
Binary file not shown.
|
@ -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}")
|
|
@ -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,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()
|
|
@ -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,7 +2,7 @@
|
||||||
#define APM_BMP085_h
|
#define APM_BMP085_h
|
||||||
|
|
||||||
#define TEMP_FILTER_SIZE 16
|
#define TEMP_FILTER_SIZE 16
|
||||||
#define PRESS_FILTER_SIZE 8
|
#define PRESS_FILTER_SIZE 10
|
||||||
|
|
||||||
class APM_BMP085_Class
|
class APM_BMP085_Class
|
||||||
{
|
{
|
||||||
|
|
|
@ -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,9 +12,11 @@ unsigned long timer;
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
APM_BMP085.Init(); // APM ADC initialization
|
Serial.begin(115200);
|
||||||
Serial.begin(38400);
|
|
||||||
Serial.println("ArduPilot Mega BMP085 library test");
|
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);
|
delay(1000);
|
||||||
timer = millis();
|
timer = millis();
|
||||||
}
|
}
|
||||||
|
|
|
@ -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})
|
|
@ -5,6 +5,19 @@
|
||||||
#define MIN_PULSEWIDTH 900
|
#define MIN_PULSEWIDTH 900
|
||||||
#define MAX_PULSEWIDTH 2100
|
#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>
|
#include <inttypes.h>
|
||||||
|
|
||||||
class APM_RC_Class
|
class APM_RC_Class
|
||||||
|
|
|
@ -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})
|
|
@ -1,6 +1,8 @@
|
||||||
#ifndef AP_ADC_H
|
#ifndef AP_ADC_H
|
||||||
#define AP_ADC_H
|
#define AP_ADC_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
|
AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
|
||||||
Code by James Goppert. DIYDrones.com
|
Code by James Goppert. DIYDrones.com
|
||||||
|
@ -13,7 +15,7 @@
|
||||||
Methods:
|
Methods:
|
||||||
Init() : Initialization of ADC. (interrupts etc)
|
Init() : Initialization of ADC. (interrupts etc)
|
||||||
Ch(ch_num) : Return the ADC channel value
|
Ch(ch_num) : Return the ADC channel value
|
||||||
|
Ch6(channel_numbers, result) : Return 6 ADC channel values
|
||||||
*/
|
*/
|
||||||
|
|
||||||
class AP_ADC
|
class AP_ADC
|
||||||
|
@ -21,8 +23,21 @@ class AP_ADC
|
||||||
public:
|
public:
|
||||||
AP_ADC() {}; // Constructor
|
AP_ADC() {}; // Constructor
|
||||||
virtual void Init() {};
|
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:
|
private:
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -19,9 +19,11 @@
|
||||||
XCK2 = SCK = pin PH2
|
XCK2 = SCK = pin PH2
|
||||||
Chip Select pin is PC4 (33) [PH6 (9)]
|
Chip Select pin is PC4 (33) [PH6 (9)]
|
||||||
We are using the 16 clocks per conversion timming to increase efficiency (fast)
|
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
|
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:
|
Methods:
|
||||||
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
|
||||||
|
@ -44,6 +46,7 @@
|
||||||
extern "C" {
|
extern "C" {
|
||||||
// AVR LibC Includes
|
// AVR LibC Includes
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
#include <stdint.h>
|
||||||
#include <avr/interrupt.h>
|
#include <avr/interrupt.h>
|
||||||
#include "WConstants.h"
|
#include "WConstants.h"
|
||||||
}
|
}
|
||||||
|
@ -53,13 +56,25 @@ extern "C" {
|
||||||
|
|
||||||
// Commands for reading ADC channels on ADS7844
|
// Commands for reading ADC channels on ADS7844
|
||||||
static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
|
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 */
|
/* Put data into buffer, sends the data */
|
||||||
UDR2 = data;
|
UDR2 = data;
|
||||||
/* Wait for data to be received */
|
/* Wait for data to be received */
|
||||||
|
@ -72,32 +87,43 @@ static unsigned char ADC_SPI_transfer(unsigned char data)
|
||||||
ISR (TIMER2_OVF_vect)
|
ISR (TIMER2_OVF_vect)
|
||||||
{
|
{
|
||||||
uint8_t ch;
|
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)
|
bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
|
||||||
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
|
||||||
|
|
||||||
for (ch = 0; ch < 8; ch++) {
|
for (ch = 0; ch < 8; ch++) {
|
||||||
adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
|
uint16_t v;
|
||||||
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
|
||||||
|
|
||||||
// Fill our Moving average filter
|
v = ADC_SPI_transfer(0) << 8; // Read first byte
|
||||||
_filter[ch][_filter_index] = adc_tmp >> 3;
|
v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
|
||||||
|
|
||||||
|
if (v & 0x8007) {
|
||||||
|
// this is a 12-bit ADC, shifted by 3 bits.
|
||||||
|
// if we get other bits set then the value is
|
||||||
|
// bogus and should be ignored
|
||||||
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// increment our filter
|
if (++_count[ch] == 0) {
|
||||||
_filter_index++;
|
// overflow ... shouldn't happen too often
|
||||||
|
// unless we're just not using the
|
||||||
// loop our filter
|
// channel. Notice that we overflow the count
|
||||||
if(_filter_index == ADC_FILTER_SIZE)
|
// to 1 here, not zero, as otherwise the
|
||||||
_filter_index = 0;
|
// reader below could get a division by zero
|
||||||
|
_sum[ch] = 0;
|
||||||
|
_count[ch] = 1;
|
||||||
|
last_ch6_micros = micros();
|
||||||
|
}
|
||||||
|
_sum[ch] += (v >> 3);
|
||||||
|
}
|
||||||
|
|
||||||
bit_set(PORTC, 4); // Disable Chip Select (PIN PC4)
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -123,42 +149,84 @@ void AP_ADC_ADS7844::Init(void)
|
||||||
// Set Baud rate
|
// Set Baud rate
|
||||||
UBRR2 = 2; // SPI clock running at 2.6MHz
|
UBRR2 = 2; // SPI clock running at 2.6MHz
|
||||||
|
|
||||||
|
// get an initial value for each channel. This ensures
|
||||||
|
// _count[] is never zero
|
||||||
|
for (uint8_t i=0; i<8; i++) {
|
||||||
|
uint16_t adc_tmp;
|
||||||
|
adc_tmp = ADC_SPI_transfer(0) << 8;
|
||||||
|
adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]);
|
||||||
|
_count[i] = 1;
|
||||||
|
_sum[i] = adc_tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_ch6_micros = micros();
|
||||||
|
|
||||||
// Enable Timer2 Overflow interrupt to capture ADC data
|
// Enable Timer2 Overflow interrupt to capture ADC data
|
||||||
TIMSK2 = 0; // Disable interrupts
|
TIMSK2 = 0; // Disable interrupts
|
||||||
TCCR2A = 0; // normal counting mode
|
TCCR2A = 0; // normal counting mode
|
||||||
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256
|
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256
|
||||||
TCNT2 = 0;
|
TCNT2 = 0;
|
||||||
TIFR2 = _BV(TOV2); // clear pending interrupts;
|
TIFR2 = _BV(TOV2); // clear pending interrupts;
|
||||||
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
|
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read one channel value
|
// Read one channel value
|
||||||
int AP_ADC_ADS7844::Ch(unsigned char ch_num)
|
uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num)
|
||||||
{
|
{
|
||||||
uint16_t result = 0;
|
uint16_t count;
|
||||||
|
uint32_t sum;
|
||||||
|
|
||||||
//while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator
|
// ensure we have at least one value
|
||||||
|
while (_count[ch_num] == 0) /* noop */ ;
|
||||||
|
|
||||||
// stop interrupts
|
// grab the value with interrupts disabled, and clear the count
|
||||||
cli();
|
cli();
|
||||||
|
count = _count[ch_num];
|
||||||
// sum our filter
|
sum = _sum[ch_num];
|
||||||
for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){
|
_count[ch_num] = 0;
|
||||||
result += _filter[ch_num][i];
|
_sum[ch_num] = 0;
|
||||||
}
|
|
||||||
|
|
||||||
// resume interrupts
|
|
||||||
sei();
|
sei();
|
||||||
|
|
||||||
// average our sampels
|
return sum/count;
|
||||||
result /= ADC_FILTER_SIZE;
|
|
||||||
|
|
||||||
return(result);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read one channel value
|
// Read 6 channel values
|
||||||
int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num)
|
// 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)
|
||||||
{
|
{
|
||||||
return _filter[ch_num][_filter_index]; // close enough
|
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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
#define ADC_DATAIN 50 // MISO
|
#define ADC_DATAIN 50 // MISO
|
||||||
#define ADC_SPICLOCK 52 // SCK
|
#define ADC_SPICLOCK 52 // SCK
|
||||||
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
|
#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 "AP_ADC.h"
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
@ -19,8 +19,12 @@ class AP_ADC_ADS7844 : public AP_ADC
|
||||||
public:
|
public:
|
||||||
AP_ADC_ADS7844(); // Constructor
|
AP_ADC_ADS7844(); // Constructor
|
||||||
void Init();
|
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:
|
private:
|
||||||
};
|
};
|
||||||
|
|
|
@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL()
|
||||||
// set diff press and temp to zero
|
// set diff press and temp to zero
|
||||||
setGyroTemp(0);
|
setGyroTemp(0);
|
||||||
setPressure(0);
|
setPressure(0);
|
||||||
|
|
||||||
|
last_hil_time = millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_ADC_HIL::Init(void)
|
void AP_ADC_HIL::Init(void)
|
||||||
|
@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read one channel value
|
// 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
|
// Set one channel value
|
||||||
|
|
|
@ -32,10 +32,11 @@ class AP_ADC_HIL : public AP_ADC
|
||||||
|
|
||||||
///
|
///
|
||||||
// Read the sensor, part of public AP_ADC interface
|
// 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
|
// Read 6 sensors at once
|
||||||
int Ch_raw(unsigned char ch_num);
|
uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result);
|
||||||
|
|
||||||
///
|
///
|
||||||
// Set the adc raw values given the current rotations rates,
|
// Set the adc raw values given the current rotations rates,
|
||||||
|
@ -49,6 +50,9 @@ class AP_ADC_HIL : public AP_ADC
|
||||||
// The raw adc array
|
// The raw adc array
|
||||||
uint16_t adcValue[8];
|
uint16_t adcValue[8];
|
||||||
|
|
||||||
|
// the time in milliseconds when we last got a HIL update
|
||||||
|
uint32_t last_hil_time;
|
||||||
|
|
||||||
///
|
///
|
||||||
// sensor constants
|
// sensor constants
|
||||||
// constants declared in cpp file
|
// constants declared in cpp file
|
||||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue