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

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

This commit is contained in:
Chris Anderson 2012-01-29 18:18:44 -08:00
parent 6ecac021b5 370d633f51
commit c1c820de72
45 changed files with 5118 additions and 1495 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
ArduCopter/ArduCopter.pde
View File

@ -1,6 +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.2 b4" #define THISFIRMWARE "ArduCopter V2.2 b6"
/* /*
ArduCopter Version 2.2 ArduCopter Version 2.2
Authors: Jason Short Authors: Jason Short
@ -69,7 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads. #include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
#include <AP_DCM.h> // ArduPilot Mega DCM Library #include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <APM_PI.h> // PI library #include <APM_PI.h> // PI library
#include <PID.h> // PID library #include <AC_PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library #include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library #include <AP_OpticalFlow.h> // Optical Flow library
@ -312,11 +312,6 @@ static const char* flight_mode_strings[] = {
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// The GPS based velocity calculated by offsetting the Latitude and Longitude // The GPS based velocity calculated by offsetting the Latitude and Longitude
// updated after GPS read - 5-10hz // updated after GPS read - 5-10hz
static int16_t x_GPS_speed;
static int16_t y_GPS_speed;
// The synthesized velocity calculated by fancy filtering and fusion
// updated at 50hz
static int16_t x_actual_speed; static int16_t x_actual_speed;
static int16_t y_actual_speed; static int16_t y_actual_speed;
@ -325,9 +320,6 @@ static int16_t y_actual_speed;
static int16_t x_rate_error; static int16_t x_rate_error;
static int16_t y_rate_error; static int16_t y_rate_error;
//static int16_t my_max_speed; // used for debugging logs
//static int16_t target_x_rate;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Radio // Radio
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -469,6 +461,8 @@ static uint8_t wp_verify_byte; // used for tracking state of navigating wa
static int16_t waypoint_speed_gov; static int16_t waypoint_speed_gov;
// Used to track how many cm we are from the "next_WP" location // Used to track how many cm we are from the "next_WP" location
static int32_t long_error, lat_error; static int32_t long_error, lat_error;
// Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s
static boolean loiter_override;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -557,19 +551,16 @@ static int32_t altitude_error;
static int16_t climb_rate; static int16_t climb_rate;
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally. // The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
static int16_t sonar_alt; static int16_t sonar_alt;
// The previous altitude as reported by Sonar in cm for calculation of Climb Rate
static int16_t old_sonar_alt;
// The climb_rate as reported by sonar in cm/s // The climb_rate as reported by sonar in cm/s
static int16_t sonar_rate; static int16_t sonar_rate;
// The altitude as reported by Baro in cm – Values can be quite high // The altitude as reported by Baro in cm – Values can be quite high
static int32_t baro_alt; static int32_t baro_alt;
// The previous altitude as reported by Baro in cm for calculation of Climb Rate
static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s // The climb_rate as reported by Baro in cm/s
static int16_t baro_rate; static int16_t baro_rate;
// //
static boolean reset_throttle_flag; static boolean reset_throttle_flag;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// flight modes // flight modes
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -601,6 +592,10 @@ static int16_t manual_boost;
static int16_t angle_boost; static int16_t angle_boost;
// Push copter down for clean landing // Push copter down for clean landing
static int16_t landing_boost; static int16_t landing_boost;
// for controlling the landing throttle curve
//verifies landings
static int16_t ground_detector;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Navigation general // Navigation general
@ -826,7 +821,7 @@ void loop()
uint32_t timer = micros(); uint32_t timer = micros();
// We want this to execute fast // We want this to execute fast
// ---------------------------- // ----------------------------
if ((timer - fast_loopTimer) >= 5000) { if ((timer - fast_loopTimer) >= 4500) {
//PORTK |= B00010000; //PORTK |= B00010000;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer; fast_loopTimer = timer;
@ -852,7 +847,7 @@ void loop()
// update our velocity estimate based on IMU at 50hz // update our velocity estimate based on IMU at 50hz
// ------------------------------------------------- // -------------------------------------------------
estimate_velocity(); //estimate_velocity();
// check for new GPS messages // check for new GPS messages
// -------------------------- // --------------------------
@ -903,6 +898,12 @@ static void fast_loop()
// IMU DCM Algorithm // IMU DCM Algorithm
read_AHRS(); read_AHRS();
if(takeoff_complete == false){
// reset these I terms to prevent awkward tipping on takeoff
reset_rate_I();
reset_stability_I();
}
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
// --------------------------------------- // ---------------------------------------
update_yaw_mode(); update_yaw_mode();
@ -1518,11 +1519,6 @@ void update_throttle_mode(void)
if ((millis() - takeoff_timer) > 5000){ if ((millis() - takeoff_timer) > 5000){
// we must be in the air by now // we must be in the air by now
takeoff_complete = true; takeoff_complete = true;
land_complete = false;
}else{
// reset these I terms to prevent awkward tipping on takeoff
reset_rate_I();
reset_stability_I();
} }
}else{ }else{
// we are on the ground // we are on the ground
@ -1535,11 +1531,6 @@ void update_throttle_mode(void)
// reset_baro(); // reset_baro();
} }
// reset out i terms and takeoff timer
// -----------------------------------
reset_rate_I();
reset_stability_I();
// remember our time since takeoff // remember our time since takeoff
// ------------------------------- // -------------------------------
takeoff_timer = millis(); takeoff_timer = millis();
@ -1596,12 +1587,8 @@ void update_throttle_mode(void)
// how far off are we // how far off are we
altitude_error = get_altitude_error(); altitude_error = get_altitude_error();
// get the AP throttle, if landing boost > 0 we are actually Landing // get the AP throttle
// This allows us to grab just the compensation. nav_throttle = get_nav_throttle(altitude_error);
if(landing_boost > 0)
nav_throttle = get_nav_throttle(-200);
else
nav_throttle = get_nav_throttle(altitude_error);
// clear the new data flag // clear the new data flag
invalid_throttle = false; invalid_throttle = false;
@ -1615,6 +1602,11 @@ void update_throttle_mode(void)
//*/ //*/
} }
// hack to remove the influence of the ground effect
if(current_loc.alt < 200 && landing_boost != 0) {
nav_throttle = min(nav_throttle, 0);
}
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost); throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost);
#else #else
@ -1662,24 +1654,18 @@ static void update_navigation()
case RTL: case RTL:
// We have reached Home // We have reached Home
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if this value > 0, we are set to trigger auto_land after 30 seconds // if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds
set_mode(LOITER); set_mode(LOITER);
auto_land_timer = millis(); auto_land_timer = millis();
break; break;
} }
// We wait until we've reached out new altitude before coming home wp_control = WP_MODE;
// Arg doesn't work, it
//if(alt_change_flag != REACHED_ALT){
// wp_control = NO_NAV_MODE;
//}else{
wp_control = WP_MODE;
// calculates desired Yaw // calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
update_auto_yaw(); update_auto_yaw();
#endif #endif
//}
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
@ -1690,16 +1676,27 @@ static void update_navigation()
case POSITION: case POSITION:
// This feature allows us to reposition the quad when the user lets // This feature allows us to reposition the quad when the user lets
// go of the sticks // go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){
// flag will reset when speed drops below .5 m/s
loiter_override = true;
}
// Allow the user to take control temporarily,
// regain hold when the speed goes down to .5m/s
if(loiter_override){
// this sets the copter to not try and nav while we control it // this sets the copter to not try and nav while we control it
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
// reset LOITER to current position // reset LOITER to current position
next_WP.lat = current_loc.lat; next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng; next_WP.lng = current_loc.lng;
if((g.rc_2.control_in + g.rc_1.control_in) == 0){
loiter_override = false;
wp_control = LOITER_MODE;
}
}else{ }else{
// this is also set by GPS in update_nav
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
} }
@ -1715,7 +1712,10 @@ static void update_navigation()
break; break;
case LAND: case LAND:
verify_land(); if(g.sonar_enabled)
verify_land_sonar();
else
verify_land_baro();
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
@ -1798,6 +1798,10 @@ static void update_trig(void){
// updated at 10hz // updated at 10hz
static void update_altitude() static void update_altitude()
{ {
static int16_t old_sonar_alt = 0;
static int32_t old_baro_alt = 0;
static int16_t old_climb_rate = 0;
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar // we are in the SIM, fake out the baro and Sonar
int fake_relative_alt = g_gps->altitude - gps_base_alt; int fake_relative_alt = g_gps->altitude - gps_base_alt;
@ -1818,7 +1822,7 @@ static void update_altitude()
baro_rate = (temp + baro_rate) >> 1; baro_rate = (temp + baro_rate) >> 1;
old_baro_alt = baro_alt; old_baro_alt = baro_alt;
// sonar_alt is calculated in a faster loop and filtered with a mode filter // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
#endif #endif
@ -1868,9 +1872,15 @@ static void update_altitude()
climb_rate = baro_rate; climb_rate = baro_rate;
} }
// simple smoothing
climb_rate = (climb_rate + old_climb_rate)>>1;
// manage bad data // manage bad data
climb_rate = constrain(climb_rate, -300, 300); climb_rate = constrain(climb_rate, -300, 300);
// save for filtering
old_climb_rate = climb_rate;
// update the target altitude // update the target altitude
next_WP.alt = get_new_altitude(); next_WP.alt = get_new_altitude();
} }
@ -1883,6 +1893,7 @@ adjust_altitude()
manual_boost = g.rc_3.control_in - 180; manual_boost = g.rc_3.control_in - 180;
manual_boost = max(-120, manual_boost); manual_boost = max(-120, manual_boost);
update_throttle_cruise(); update_throttle_cruise();
}else if (g.rc_3.control_in >= 650){ }else if (g.rc_3.control_in >= 650){
// we add 0 to 100 PWM to hover // we add 0 to 100 PWM to hover
manual_boost = g.rc_3.control_in - 650; manual_boost = g.rc_3.control_in - 650;
@ -1898,14 +1909,18 @@ static void tuning(){
switch(g.radio_tuning){ switch(g.radio_tuning){
case CH6_DAMP: case CH6_DAMP:
g.rc_6.set_range(0,80); // 0 to 1 g.rc_6.set_range(0,300); // 0 to 1
g.stablize_d.set(tuning_value); g.stablize_d.set(tuning_value);
//g.rc_6.set_range(0,60); // 0 to 1
//g.pid_rate_roll.kD(tuning_value);
//g.pid_rate_pitch.kD(tuning_value);
break; break;
case CH6_STABILIZE_KP: case CH6_STABILIZE_KP:
g.rc_6.set_range(0,8000); // 0 to 8 g.rc_6.set_range(0,8000); // 0 to 8
g.pi_stabilize_roll.kP(tuning_value); g.pid_rate_roll.kP(tuning_value);
g.pi_stabilize_pitch.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value);
break; break;
case CH6_STABILIZE_KI: case CH6_STABILIZE_KI:
@ -1917,16 +1932,14 @@ static void tuning(){
case CH6_RATE_KP: case CH6_RATE_KP:
g.rc_6.set_range(40,300); // 0 to .3 g.rc_6.set_range(40,300); // 0 to .3
g.pi_rate_roll.kP(tuning_value); g.pid_rate_roll.kP(tuning_value);
g.pi_rate_pitch.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value);
g.pi_acro_roll.kP(tuning_value);
g.pi_acro_pitch.kP(tuning_value);
break; break;
case CH6_RATE_KI: case CH6_RATE_KI:
g.rc_6.set_range(0,300); // 0 to .3 g.rc_6.set_range(0,300); // 0 to .3
g.pi_rate_roll.kI(tuning_value); g.pid_rate_roll.kI(tuning_value);
g.pi_rate_pitch.kI(tuning_value); g.pid_rate_pitch.kI(tuning_value);
break; break;
case CH6_YAW_KP: case CH6_YAW_KP:
@ -1936,12 +1949,12 @@ static void tuning(){
case CH6_YAW_RATE_KP: case CH6_YAW_RATE_KP:
g.rc_6.set_range(0,1000); g.rc_6.set_range(0,1000);
g.pi_rate_yaw.kP(tuning_value); g.pid_rate_yaw.kP(tuning_value);
break; break;
case CH6_THROTTLE_KP: case CH6_THROTTLE_KP:
g.rc_6.set_range(0,1000); // 0 to 1 g.rc_6.set_range(0,1000); // 0 to 1
g.pi_throttle.kP(tuning_value); g.pid_throttle.kP(tuning_value);
break; break;
case CH6_TOP_BOTTOM_RATIO: case CH6_TOP_BOTTOM_RATIO:
@ -1961,15 +1974,15 @@ static void tuning(){
break; break;
case CH6_LOITER_P: case CH6_LOITER_P:
g.rc_6.set_range(0,1000); g.rc_6.set_range(0,2000);
g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value);
break; break;
case CH6_NAV_P: case CH6_NAV_P:
g.rc_6.set_range(0,6000); g.rc_6.set_range(0,6000);
g.pi_nav_lat.kP(tuning_value); g.pid_nav_lat.kP(tuning_value);
g.pi_nav_lon.kP(tuning_value); g.pid_nav_lon.kP(tuning_value);
break; break;
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
@ -1986,20 +1999,20 @@ static void tuning(){
case CH6_OPTFLOW_KP: case CH6_OPTFLOW_KP:
g.rc_6.set_range(0,5000); // 0 to 5 g.rc_6.set_range(0,5000); // 0 to 5
g.pi_optflow_roll.kP(tuning_value); g.pid_optflow_roll.kP(tuning_value);
g.pi_optflow_pitch.kP(tuning_value); g.pid_optflow_pitch.kP(tuning_value);
break; break;
case CH6_OPTFLOW_KI: case CH6_OPTFLOW_KI:
g.rc_6.set_range(0,10000); // 0 to 10 g.rc_6.set_range(0,10000); // 0 to 10
g.pi_optflow_roll.kI(tuning_value); g.pid_optflow_roll.kI(tuning_value);
g.pi_optflow_pitch.kI(tuning_value); g.pid_optflow_pitch.kI(tuning_value);
break; break;
case CH6_OPTFLOW_KD: case CH6_OPTFLOW_KD:
g.rc_6.set_range(0,200); // 0 to 0.2 g.rc_6.set_range(0,200); // 0 to 0.2
g.pi_optflow_roll.kD(tuning_value); g.pid_optflow_roll.kD(tuning_value);
g.pi_optflow_pitch.kD(tuning_value); g.pid_optflow_pitch.kD(tuning_value);
break; break;
} }
} }
@ -2077,8 +2090,9 @@ static void update_nav_wp()
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
// clear out our nav so we can do things like land straight down // clear out our nav so we can do things like land straight down
// or change Loiter position
// We bring in our iterms for wind control, but we don't navigate // We bring copy over our Iterms for wind control, but we don't navigate
nav_lon = g.pi_loiter_lon.get_integrator(); nav_lon = g.pi_loiter_lon.get_integrator();
nav_lat = g.pi_loiter_lat.get_integrator(); nav_lat = g.pi_loiter_lat.get_integrator();

229
ArduCopter/Attitude.pde
View File

@ -3,134 +3,131 @@
static int static int
get_stabilize_roll(int32_t target_angle) get_stabilize_roll(int32_t target_angle)
{ {
int32_t error = 0;
int32_t rate = 0;
static float current_rate = 0;
// angle error // angle error
error = wrap_180(target_angle - dcm.roll_sensor); target_angle = wrap_180(target_angle - dcm.roll_sensor);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -4500, 4500); target_angle = constrain(target_angle, -4500, 4500);
// convert to desired Rate: // convert to desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt); target_angle = g.pi_stabilize_roll.get_pi(target_angle, G_Dt);
// output control: // output control:
rate = constrain(rate, -4500, 4500); return constrain(target_angle, -4500, 4500);
return (int)rate;
#else #else
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); target_angle = constrain(target_angle, -2500, 2500);
// conver to desired Rate: // conver to desired Rate:
rate = g.pi_stabilize_roll.get_p(error); int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt);
// experiment to pipe iterm directly into the output return get_rate_roll(target_rate) + iterm;
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
// rate control
error = rate - (omega.x * DEGX100);
rate = g.pi_rate_roll.get_pi(error, G_Dt);
// D term
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
int16_t d_temp = current_rate * g.stablize_d;
rate -= d_temp;
// output control:
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
#endif #endif
} }
static int static int
get_stabilize_pitch(int32_t target_angle) get_stabilize_pitch(int32_t target_angle)
{ {
int32_t error = 0;
int32_t rate = 0;
static float current_rate = 0;
// angle error // angle error
error = wrap_180(target_angle - dcm.pitch_sensor); target_angle = wrap_180(target_angle - dcm.pitch_sensor);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -4500, 4500); target_angle = constrain(target_angle, -4500, 4500);
// convert to desired Rate: // convert to desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); target_angle = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt);
// output control: // output control:
rate = constrain(rate, -4500, 4500); return constrain(target_angle, -4500, 4500);
return (int)rate;
#else #else
// angle error // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); target_angle = constrain(target_angle, -2500, 2500);
// conver to desired Rate: // conver to desired Rate:
rate = g.pi_stabilize_pitch.get_p(error); int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
// experiment to pipe iterm directly into the output return get_rate_pitch(target_rate) + iterm;
int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt);
// rate control
error = rate - (omega.y * DEGX100);
//error = rate - (omega.y * DEGX100);
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
// D term
current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3;
int16_t d_temp = current_rate * g.stablize_d;
rate -= d_temp;
// output control:
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
#endif #endif
} }
#define YAW_ERROR_MAX 2000
static int static int
get_stabilize_yaw(int32_t target_angle) get_stabilize_yaw(int32_t target_angle)
{ {
int32_t error;
int32_t rate;
// angle error // angle error
error = wrap_180(target_angle - dcm.yaw_sensor); target_angle = wrap_180(target_angle - dcm.yaw_sensor);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX); target_angle = constrain(target_angle, -2000, 2000);
// convert to desired Rate: // conver to desired Rate:
rate = g.pi_stabilize_yaw.get_p(error); int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle);
int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
if( !g.heli_ext_gyro_enabled ) { if(!g.heli_ext_gyro_enabled){
error = rate - (omega.z * DEGX100); return get_rate_yaw(target_rate) + iterm;
rate = g.pi_rate_yaw.get_pi(error, G_Dt); }else{
return constrain((target_rate + iterm), -4500, 4500);
} }
// output control:
rate = constrain(rate, -4500, 4500);
#else #else
error = rate - (omega.z * DEGX100); return get_rate_yaw(target_rate) + iterm;
rate = g.pi_rate_yaw.get_pi(error, G_Dt); #endif
}
static int
get_rate_roll(int32_t target_rate)
{
static int32_t last_rate = 0;
int32_t current_rate = (omega.x * DEGX100);
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
last_rate = current_rate;
// output control: // output control:
int16_t yaw_input = 1400 + abs(g.rc_4.control_in); return constrain(target_rate, -2500, 2500);
// smoother Yaw control: }
rate = constrain(rate, -yaw_input, yaw_input);
#endif
return (int)rate + iterm; static int
get_rate_pitch(int32_t target_rate)
{
static int32_t last_rate = 0;
int32_t current_rate = (omega.y * DEGX100);
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
last_rate = current_rate;
// output control:
return constrain(target_rate, -2500, 2500);
}
static int
get_rate_yaw(int32_t target_rate)
{
// rate control
target_rate = target_rate - (omega.z * DEGX100);
target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt);
// output control:
int16_t yaw_limit = 1400 + abs(g.rc_4.control_in);
// smoother Yaw control:
return constrain(target_rate, -yaw_limit, yaw_limit);
} }
#define ALT_ERROR_MAX 400 #define ALT_ERROR_MAX 400
@ -138,10 +135,8 @@ static int16_t
get_nav_throttle(int32_t z_error) get_nav_throttle(int32_t z_error)
{ {
static int16_t old_output = 0; static int16_t old_output = 0;
//static int16_t rate_d = 0; int16_t rate_error = 0;
int16_t output = 0;
int16_t rate_error;
int16_t output;
// limit error to prevent I term run up // limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
@ -155,17 +150,10 @@ get_nav_throttle(int32_t z_error)
// calculate rate error // calculate rate error
rate_error = rate_error - climb_rate; rate_error = rate_error - climb_rate;
// limit the rate - iterm is not used // limit the rate
output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180); output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180);
// a positive climb rate means we're going up
//rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain
// slight adjustment to alt hold output
//output -= constrain(rate_d, -25, 25);
// light filter of output // light filter of output
//output = (old_output * 3 + output) / 4;
output = (old_output + output) / 2; output = (old_output + output) / 2;
// save our output // save our output
@ -175,33 +163,6 @@ get_nav_throttle(int32_t z_error)
return output + iterm; return output + iterm;
} }
static int
get_rate_roll(int32_t target_rate)
{
int32_t error = (target_rate * 3.5) - (omega.x * DEGX100);
error = constrain(error, -20000, 20000);
return g.pi_acro_roll.get_pi(error, G_Dt);
}
static int
get_rate_pitch(int32_t target_rate)
{
int32_t error = (target_rate * 3.5) - (omega.y * DEGX100);
error = constrain(error, -20000, 20000);
return g.pi_acro_pitch.get_pi(error, G_Dt);
}
static int
get_rate_yaw(int32_t target_rate)
{
int32_t error = (target_rate * 4.5) - (omega.z * DEGX100);
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
// output control:
return (int)constrain(target_rate, -2500, 2500);
}
// Keeps old data out of our calculation / logs // Keeps old data out of our calculation / logs
static void reset_nav_params(void) static void reset_nav_params(void)
{ {
@ -247,17 +208,15 @@ static void reset_I_all(void)
static void reset_rate_I() static void reset_rate_I()
{ {
g.pi_rate_roll.reset_I(); g.pid_rate_roll.reset_I();
g.pi_rate_pitch.reset_I(); g.pid_rate_pitch.reset_I();
g.pi_acro_roll.reset_I(); g.pid_rate_yaw.reset_I();
g.pi_acro_pitch.reset_I();
g.pi_rate_yaw.reset_I();
} }
static void reset_optflow_I(void) static void reset_optflow_I(void)
{ {
g.pi_optflow_roll.reset_I(); g.pid_optflow_roll.reset_I();
g.pi_optflow_pitch.reset_I(); g.pid_optflow_pitch.reset_I();
of_roll = 0; of_roll = 0;
of_pitch = 0; of_pitch = 0;
} }
@ -272,15 +231,15 @@ static void reset_wind_I(void)
static void reset_nav_I(void) static void reset_nav_I(void)
{ {
// Rate control for WP navigation // Rate control for WP navigation
g.pi_nav_lat.reset_I(); g.pid_nav_lat.reset_I();
g.pi_nav_lon.reset_I(); g.pid_nav_lon.reset_I();
} }
static void reset_throttle_I(void) static void reset_throttle_I(void)
{ {
// For Altitude Hold // For Altitude Hold
g.pi_alt_hold.reset_I(); g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I(); g.pid_throttle.reset_I();
} }
static void reset_stability_I(void) static void reset_stability_I(void)
@ -325,7 +284,7 @@ static int get_angle_boost(int value)
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0); temp = 1.0 - constrain(temp, .5, 1.0);
int16_t output = temp * value; int16_t output = temp * value;
return constrain(output, 0, 100); return constrain(output, 0, 200);
// return (int)(temp * value); // return (int)(temp * value);
} }
@ -482,9 +441,9 @@ get_of_roll(int32_t control_roll)
// only stop roll if caller isn't modifying roll // only stop roll if caller isn't modifying roll
if( control_roll == 0 && current_loc.alt < 1500) { if( control_roll == 0 && current_loc.alt < 1500) {
new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
}else{ }else{
g.pi_optflow_roll.reset_I(); g.pid_optflow_roll.reset_I();
tot_x_cm = 0; tot_x_cm = 0;
} }
// limit amount of change and maximum angle // limit amount of change and maximum angle
@ -516,10 +475,10 @@ get_of_pitch(int32_t control_pitch)
// only stop roll if caller isn't modifying pitch // only stop roll if caller isn't modifying pitch
if( control_pitch == 0 && current_loc.alt < 1500 ) { if( control_pitch == 0 && current_loc.alt < 1500 ) {
new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
}else{ }else{
tot_y_cm = 0; tot_y_cm = 0;
g.pi_optflow_pitch.reset_I(); g.pid_optflow_pitch.reset_I();
} }
// limit amount of change // limit amount of change

5
ArduCopter/Camera.pde
View File

@ -4,8 +4,9 @@
static void init_camera() static void init_camera()
{ {
APM_RC.enable_out(CH_CAM_PITCH); APM_RC.enable_out(CH_CAM_PITCH);
APM_RC.enable_out(CH_CAM_ROLL); APM_RC.enable_out(CH_CAM_ROLL);
// ch 6 high(right) is down. // ch 6 high(right) is down.
g.rc_camera_pitch.set_angle(4500); g.rc_camera_pitch.set_angle(4500);
g.rc_camera_roll.set_angle(4500); g.rc_camera_roll.set_angle(4500);

19
ArduCopter/GCS_Mavlink.pde
View File

@ -824,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_ACTION_EMCY_LAND: case MAV_ACTION_EMCY_LAND:
//set_mode(LAND); set_mode(LAND);
break; break;
case MAV_ACTION_HALT: case MAV_ACTION_HALT:
@ -1529,6 +1529,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
imu.set_gyro(gyros); imu.set_gyro(gyros);
*/ */
//imu.set_accel(accels); //imu.set_accel(accels);
// rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM
Vector3f gyros;
gyros.x = (float)packet.rollspeed / 1000.0;
gyros.y = (float)packet.pitchspeed / 1000.0;
gyros.z = (float)packet.yawspeed / 1000.0;
imu.set_gyro(gyros);
// m/s/s
Vector3f accels;
accels.x = (float)packet.roll * gravity / 1000.0;
accels.y = (float)packet.pitch * gravity / 1000.0;
accels.z = (float)packet.yaw * gravity / 1000.0;
imu.set_accel(accels);
break; break;
} }
#endif #endif

2
ArduCopter/Log.pde
View File

@ -590,7 +590,7 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt(climb_rate); // 10 DataFlash.WriteInt(climb_rate); // 10
DataFlash.WriteInt(g.rc_3.servo_out); // 11 DataFlash.WriteInt(g.rc_3.servo_out); // 11
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12 DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12
DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13 DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }

85
ArduCopter/Parameters.h
View File

@ -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 = 114; static const uint16_t k_format_version = 115;
// 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)
@ -26,7 +26,6 @@ public:
// //
static const uint16_t k_software_type = 10; // 0 for APM trunk static const uint16_t k_software_type = 10; // 0 for APM trunk
//
// Parameter identities. // Parameter identities.
// //
// The enumeration defined here is used to ensure that every parameter // The enumeration defined here is used to ensure that every parameter
@ -168,23 +167,20 @@ public:
// 235: PI/D Controllers // 235: PI/D Controllers
// //
k_param_stabilize_d = 234, k_param_stabilize_d = 234,
k_param_pi_rate_roll = 235, k_param_pid_rate_roll = 235,
k_param_pi_rate_pitch, k_param_pid_rate_pitch,
k_param_pi_rate_yaw, k_param_pid_rate_yaw,
k_param_pi_stabilize_roll, k_param_pi_stabilize_roll,
k_param_pi_stabilize_pitch, k_param_pi_stabilize_pitch,
k_param_pi_stabilize_yaw, k_param_pi_stabilize_yaw,
k_param_pi_loiter_lat, k_param_pi_loiter_lat,
k_param_pi_loiter_lon, k_param_pi_loiter_lon,
k_param_pi_nav_lat, k_param_pid_nav_lat,
k_param_pi_nav_lon, k_param_pid_nav_lon,
k_param_pi_alt_hold, k_param_pi_alt_hold,
k_param_pi_throttle, k_param_pid_throttle,
k_param_pi_acro_roll, k_param_pid_optflow_roll,
k_param_pi_acro_pitch, k_param_pid_optflow_pitch, // 250
k_param_pi_optflow_roll,
k_param_pi_optflow_pitch, // 250
k_param_loiter_d,
// 254,255: reserved // 254,255: reserved
}; };
@ -286,31 +282,24 @@ public:
AP_Float camera_pitch_gain; AP_Float camera_pitch_gain;
AP_Float camera_roll_gain; AP_Float camera_roll_gain;
AP_Float stablize_d; AP_Float stablize_d;
AP_Float loiter_d;
// PI/D controllers // PI/D controllers
APM_PI pi_rate_roll; AC_PID pid_rate_roll;
APM_PI pi_rate_pitch; AC_PID pid_rate_pitch;
APM_PI pi_rate_yaw; AC_PID pid_rate_yaw;
AC_PID pid_nav_lat;
AC_PID pid_nav_lon;
APM_PI pi_stabilize_roll; AC_PID pid_throttle;
APM_PI pi_stabilize_pitch; AC_PID pid_optflow_roll;
APM_PI pi_stabilize_yaw; AC_PID pid_optflow_pitch;
APM_PI pi_loiter_lat; APM_PI pi_loiter_lat;
APM_PI pi_loiter_lon; APM_PI pi_loiter_lon;
APM_PI pi_stabilize_roll;
APM_PI pi_nav_lat; APM_PI pi_stabilize_pitch;
APM_PI pi_nav_lon; APM_PI pi_stabilize_yaw;
APM_PI pi_alt_hold; APM_PI pi_alt_hold;
APM_PI pi_throttle;
APM_PI pi_acro_roll;
APM_PI pi_acro_pitch;
PID pi_optflow_roll;
PID pi_optflow_pitch;
uint8_t junk; uint8_t junk;
@ -369,7 +358,7 @@ public:
radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),
ch7_option (CH7_SAVE_WP, k_param_ch7_option, PSTR("CH7_OPT")), ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")),
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")), heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
@ -409,34 +398,32 @@ public:
//------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------
camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")), camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")),
camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")),
stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")), stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")),
loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")),
// PID controller group key name initial P initial I initial D initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100),
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100),
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100),
// PI controller group key name initial P initial I initial imax // PI controller group key name initial P initial I initial imax
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100),
pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100),
pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100),
pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100), pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100), pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100), pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX),
pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX),
pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX),
pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100),
pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100),
pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100),
junk(0) // XXX just so that we can add things without worrying about the trailing comma junk(0) // XXX just so that we can add things without worrying about the trailing comma
{ {

110
ArduCopter/commands_logic.pde
View File

@ -16,6 +16,7 @@ static void process_nav_command()
break; break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
yaw_mode = YAW_HOLD;
do_land(); do_land();
break; break;
@ -121,8 +122,6 @@ static void process_now_command()
static bool verify_must() static bool verify_must()
{ {
//Serial.printf("vmust: %d\n", command_nav_ID);
switch(command_nav_queue.id) { switch(command_nav_queue.id) {
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
@ -130,7 +129,11 @@ static bool verify_must()
break; break;
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
return verify_land(); if(g.sonar_enabled == true){
return verify_land_sonar();
}else{
return verify_land_baro();
}
break; break;
case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_WAYPOINT:
@ -265,13 +268,21 @@ static void do_land()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
// just to make sure
land_complete = false; land_complete = false;
// landing boost lowers the main throttle to mimmick
// the effect of a user's hand
landing_boost = 0;
// A counter that goes up if our climb rate stalls out.
ground_detector = 0;
// hold at our current location // hold at our current location
set_next_WP(&current_loc); set_next_WP(&current_loc);
// Set a new target altitude // Set a new target altitude very low, incase we are landing on a hill!
set_new_altitude(-200); set_new_altitude(-1000);
} }
static void do_loiter_unlimited() static void do_loiter_unlimited()
@ -341,46 +352,71 @@ static bool verify_takeoff()
} }
// called at 10hz // called at 10hz
static bool verify_land() static bool verify_land_sonar()
{ {
static int16_t velocity_land = -1; static float icount = 1;
static float icount = 0;
// landing detector if(current_loc.alt > 300){
if (current_loc.alt > 300){ wp_control = LOITER_MODE;
velocity_land = 2000;
icount = 1; icount = 1;
ground_detector = 0;
}else{ }else{
// a LP filter used to tell if we have landed // begin to pull down on the throttle
// will drive to 0 if we are on the ground - maybe, the baro is noisy landing_boost++;
velocity_land = ((velocity_land * 7) + climb_rate ) / 8;
} }
if ((current_loc.alt - home.alt) < 200){ if(current_loc.alt < 200 ){
// don't bank to hold position wp_control = NO_NAV_MODE;
wp_control = NO_NAV_MODE;
// try and come down faster
//landing_boost++;
//landing_boost = min(landing_boost, 15);
float tmp = (1.75 * icount * icount) - (7.2 * icount);
landing_boost = tmp;
landing_boost = constrain(landing_boost, 1, 200);
icount += 0.4;
//Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount);
}else{
landing_boost = 0;
wp_control = LOITER_MODE;
} }
//if(/*(current_loc.alt < 100) && */ (velocity_land < 20)){ if(current_loc.alt < 150 ){
if((landing_boost > 150) || (velocity_land < 20)){ //rapid throttle reduction
icount = 1; int16_t lb = (1.75 * icount * icount) - (7.2 * icount);
land_complete = true; icount++;
landing_boost = 0; lb = constrain(lb, 0, 180);
return true; landing_boost += lb;
//Serial.printf("%0.0f, %d, %d, %d\n", icount, current_loc.alt, landing_boost, lb);
if(current_loc.alt < 40 || abs(climb_rate) < 20) {
if(ground_detector++ > 20) {
land_complete = true;
ground_detector = 0;
icount = 1;
// init disarm motors
init_disarm_motors();
return true;
}
}
}
return false;
}
static bool verify_land_baro()
{
if(current_loc.alt > 300){
wp_control = LOITER_MODE;
ground_detector = 0;
}else{
// begin to pull down on the throttle
landing_boost++;
landing_boost = min(landing_boost, 40);
}
if(current_loc.alt < 200 ){
wp_control = NO_NAV_MODE;
}
if(current_loc.alt < 150 ){
if(abs(climb_rate) < 20) {
landing_boost++;
if(ground_detector++ > 30) {
land_complete = true;
ground_detector = 0;
// init disarm motors
init_disarm_motors();
return true;
}
}
} }
return false; return false;
} }

9
ArduCopter/commands_process.pde
View File

@ -62,14 +62,7 @@ static void update_commands()
g.command_index = command_nav_index = 255; g.command_index = command_nav_index = 255;
// if we are on the ground, enter stabilize, else Land // if we are on the ground, enter stabilize, else Land
if (land_complete == true){ if (land_complete == true){
// So what state does this leave us in? // we will disarm the motors after landing.
// We are still in the same mode as what landed us,
// so maybe we try to continue to descend just in case we are still in the air
// This will also drive down the Iterm to -300
// We can't disarm the motors easily. We could very well be wrong
//
//init_disarm_motors();
} else { } else {
set_mode(LAND); set_mode(LAND);
} }

119
ArduCopter/config.h
View File

@ -525,114 +525,98 @@
#ifdef MOTORS_JD880 #ifdef MOTORS_JD880
# define STABILIZE_ROLL_P 3.6 # define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0
# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 3.6 # define STABILIZE_PITCH_P 3.6
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0
# define STABILIZE_PITCH_IMAX 40.0 // degrees # define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif #endif
#ifdef MOTORS_JD850 #ifdef MOTORS_JD850
# define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_P 4.0
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0
# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_P 4.0
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0
# define STABILIZE_PITCH_IMAX 40.0 // degrees # define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif #endif
#ifndef STABILIZE_D #ifndef STABILIZE_D
# define STABILIZE_D .03 # define STABILIZE_D .12
#endif #endif
// Jasons default values that are good for smaller payload motors. // Jasons default values that are good for smaller payload motors.
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.6 # define STABILIZE_ROLL_P 4.5
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.1 # define STABILIZE_ROLL_I 0.0
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 40 // degrees # define STABILIZE_ROLL_IMAX 40 // degrees
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.6 # define STABILIZE_PITCH_P 4.5
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.1 # define STABILIZE_PITCH_I 0.0
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 40 // degrees # define STABILIZE_PITCH_IMAX 40 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// #ifndef STABILIZE_YAW_P
// Acro Rate Control # define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
//
#ifndef ACRO_ROLL_P
# define ACRO_ROLL_P 0.155
#endif #endif
#ifndef ACRO_ROLL_I #ifndef STABILIZE_YAW_I
# define ACRO_ROLL_I 0.0 # define STABILIZE_YAW_I 0.01
#endif #endif
#ifndef ACRO_ROLL_IMAX #ifndef STABILIZE_YAW_IMAX
# define ACRO_ROLL_IMAX 15 // degrees # define STABILIZE_YAW_IMAX 8 // degrees * 100
#endif #endif
#ifndef ACRO_PITCH_P
# define ACRO_PITCH_P 0.155
#endif
#ifndef ACRO_PITCH_I
# define ACRO_PITCH_I 0 //0.18
#endif
#ifndef ACRO_PITCH_IMAX
# define ACRO_PITCH_IMAX 15 // degrees
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Stabilize Rate Control // Stabilize Rate Control
// //
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.155 # define RATE_ROLL_P 0.14
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0 # define RATE_ROLL_I 0.0
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0
#endif #endif
#ifndef RATE_ROLL_IMAX #ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 15 // degrees # define RATE_ROLL_IMAX 15 // degrees
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.155 # define RATE_PITCH_P 0.14
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0 //0.18 # define RATE_PITCH_I 0 // 0.18
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0 // 0.002
#endif #endif
#ifndef RATE_PITCH_IMAX #ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 15 // degrees # define RATE_PITCH_IMAX 15 // degrees
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// YAW Control
//
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef STABILIZE_YAW_IMAX
# define STABILIZE_YAW_IMAX 8 // degrees * 100
#endif
#ifndef RATE_YAW_P #ifndef RATE_YAW_P
# define RATE_YAW_P .13 // used to control response in turning # define RATE_YAW_P .13
#endif #endif
#ifndef RATE_YAW_I #ifndef RATE_YAW_I
# define RATE_YAW_I 0.0 # define RATE_YAW_I 0.0
#endif
#ifndef RATE_YAW_D
# define RATE_YAW_D .002
#endif #endif
#ifndef RATE_YAW_IMAX #ifndef RATE_YAW_IMAX
# define RATE_YAW_IMAX 50 # define RATE_YAW_IMAX 50 // degrees
#endif #endif
@ -640,37 +624,37 @@
// Loiter control gains // Loiter control gains
// //
#ifndef LOITER_P #ifndef LOITER_P
# define LOITER_P 1.5 // was .25 in previous # define LOITER_P .4 // was .25 in previous
#endif #endif
#ifndef LOITER_I #ifndef LOITER_I
# define LOITER_I 0.09 // Wind control # define LOITER_I 0.2 // Wind control
#endif #endif
#ifndef LOITER_IMAX #ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees° # define LOITER_IMAX 30 // degrees
#endif
#ifndef LOITER_D
# define LOITER_D 2.2 // rate control
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// WP Navigation control gains // WP Navigation control gains
// //
#ifndef NAV_P #ifndef NAV_P
# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter # define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0 // # define NAV_I 0 //
#endif #endif
#ifndef NAV_D
# define NAV_D 0.015 //
#endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 30 // degrees # define NAV_IMAX 30 // degrees
#endif #endif
#ifndef WAYPOINT_SPEED_MAX #ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph # define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph
#endif #endif
#ifndef WAYPOINT_SPEED_MIN #ifndef WAYPOINT_SPEED_MIN
# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph # define WAYPOINT_SPEED_MIN 100 // 1m/s
#endif #endif
@ -678,28 +662,31 @@
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_CRUISE #ifndef THROTTLE_CRUISE
# define THROTTLE_CRUISE 350 // # define THROTTLE_CRUISE 350 //
#endif #endif
#ifndef THR_HOLD_P #ifndef ALT_HOLD_P
# define THR_HOLD_P 0.4 // # define ALT_HOLD_P 0.4 //
#endif #endif
#ifndef THR_HOLD_I #ifndef ALT_HOLD_I
# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup # define ALT_HOLD_I 0.02
#endif #endif
#ifndef THR_HOLD_IMAX #ifndef ALT_HOLD_IMAX
# define THR_HOLD_IMAX 300 # define ALT_HOLD_IMAX 300
#endif #endif
// RATE control // RATE control
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.5 // # define THROTTLE_P 0.35 //
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.0 // # define THROTTLE_I 0.0 //
#endif
#ifndef THROTTLE_D
# define THROTTLE_D 0.02 //
#endif #endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 300 # define THROTTLE_IMAX 300
#endif #endif

2
ArduCopter/control_modes.pde
View File

@ -114,6 +114,7 @@ static void read_trim_switch()
// reset the mission // reset the mission
CH7_wp_index = 0; CH7_wp_index = 0;
g.command_total.set_and_save(1); g.command_total.set_and_save(1);
set_mode(RTL);
return; return;
} }
@ -154,7 +155,6 @@ static void read_trim_switch()
// 1 = takeoff // 1 = takeoff
// 2 = WP 2 // 2 = WP 2
// 3 = command total // 3 = command total
} }
} }
} }

93
ArduCopter/heli.pde
View File

@ -7,6 +7,7 @@
static bool heli_swash_initialised = false; static bool heli_swash_initialised = false;
static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000) static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000)
static float heli_coll_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
// heli_servo_averaging: // heli_servo_averaging:
// 0 or 1 = no averaging, 250hz // 0 or 1 = no averaging, 250hz
@ -20,27 +21,12 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1
static void heli_reset_swash() static void heli_reset_swash()
{ {
// free up servo ranges // free up servo ranges
if( g.heli_servo_1.get_reverse() ) { g.heli_servo_1.radio_min = 1000;
g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); g.heli_servo_1.radio_max = 2000;
g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); g.heli_servo_2.radio_min = 1000;
}else{ g.heli_servo_2.radio_max = 2000;
g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); g.heli_servo_3.radio_min = 1000;
g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); g.heli_servo_3.radio_max = 2000;
}
if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500);
}
// pitch factors // pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
@ -52,6 +38,9 @@ static void heli_reset_swash()
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// set throttle scaling
heli_coll_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0;
// we must be in set-up mode so mark swash as uninitialised // we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false; heli_swash_initialised = false;
} }
@ -60,7 +49,6 @@ static void heli_reset_swash()
static void heli_init_swash() static void heli_init_swash()
{ {
int i; int i;
float coll_range_comp = 1; // factor to negate collective range's effect on roll & pitch range
// swash servo initialisation // swash servo initialisation
g.heli_servo_1.set_range(0,1000); g.heli_servo_1.set_range(0,1000);
@ -75,45 +63,29 @@ static void heli_init_swash()
} }
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max);
// calculate compensation for collective range on roll & pitch range
if( g.heli_coll_max - g.heli_coll_min > 100 )
coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min);
// calculate throttle mid point // calculate throttle mid point
heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min)); heli_throttle_mid = ((float)(g.heli_coll_mid-g.heli_coll_min))/((float)(g.heli_coll_max-g.heli_coll_min))*1000.0;
// determine scalar throttle input
heli_coll_scalar = ((float)(g.heli_coll_max-g.heli_coll_min))/1000.0;
// pitch factors // pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp; heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp; heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp; heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle));
// roll factors // roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp; heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)) * coll_range_comp; heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp; heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// servo min/max values // servo min/max values
if( g.heli_servo_1.get_reverse() ) { g.heli_servo_1.radio_min = 1000;
g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); g.heli_servo_1.radio_max = 2000;
g.heli_servo_1.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); g.heli_servo_2.radio_min = 1000;
}else{ g.heli_servo_2.radio_max = 2000;
g.heli_servo_1.radio_min = g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); g.heli_servo_3.radio_min = 1000;
g.heli_servo_1.radio_max = g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); g.heli_servo_3.radio_max = 2000;
}
if( g.heli_servo_2.get_reverse() ) {
g.heli_servo_2.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_2.radio_trim-1500);
}else{
g.heli_servo_2.radio_min = g.heli_coll_min + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_2.radio_max = g.heli_coll_max + (g.heli_servo_2.radio_trim-1500);
}
if( g.heli_servo_3.get_reverse() ) {
g.heli_servo_3.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_3.radio_trim-1500);
}else{
g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500);
}
// reset the servo averaging // reset the servo averaging
for( i=0; i<=3; i++ ) for( i=0; i<=3; i++ )
@ -148,13 +120,15 @@ static void heli_move_servos_to_mid()
// //
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
{ {
int yaw_offset = 0; int yaw_offset = 0;
int coll_out_scaled;
if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to freeup the swash // check if we need to freeup the swash
if( heli_swash_initialised ) { if( heli_swash_initialised ) {
heli_reset_swash(); heli_reset_swash();
} }
coll_out_scaled = coll_out * heli_coll_scalar + g.rc_3.radio_min - 1000;
}else{ // regular flight mode }else{ // regular flight mode
// check if we need to reinitialise the swash // check if we need to reinitialise the swash
@ -166,19 +140,20 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, 0, 1000); coll_out = constrain(coll_out, 0, 1000);
coll_out_scaled = coll_out * heli_coll_scalar + g.heli_coll_min - 1000;
// rudder feed forward based on collective // rudder feed forward based on collective
#if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator
if( !g.heli_ext_gyro_enabled ) { if( !g.heli_ext_gyro_enabled ) {
yaw_offset = g.heli_coll_yaw_effect * (coll_out - g.heli_coll_mid); yaw_offset = g.heli_coll_yaw_effect * (coll_out_scaled - g.heli_coll_mid);
} }
#endif #endif
} }
// swashplate servos // swashplate servos
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_1.radio_trim-1500);
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_2.radio_trim-1500);
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_3.radio_trim-1500);
g.heli_servo_4.servo_out = yaw_out + yaw_offset; g.heli_servo_4.servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out // use servo_out to calculate pwm_out and radio_out

6
ArduCopter/leds.pde
View File

@ -19,7 +19,11 @@ static void update_GPS_light(void)
switch (g_gps->status()){ switch (g_gps->status()){
case(2): case(2):
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. if(home_is_set) { // JLN update
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
} else {
digitalWrite(C_LED_PIN, LED_OFF);
}
break; break;
case(1): case(1):

2
ArduCopter/motors.pde
View File

@ -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 -*-
// 10 = 1 second // 10 = 1 second
#define ARM_DELAY 30 #define ARM_DELAY 20
#define DISARM_DELAY 20 #define DISARM_DELAY 20
#define LEVEL_DELAY 100 #define LEVEL_DELAY 100

256
ArduCopter/navigation.pde
View File

@ -7,13 +7,11 @@ static byte navigate()
{ {
// waypoint distance from plane in meters // waypoint distance from plane in meters
// --------------------------------------- // ---------------------------------------
wp_distance = get_distance(&current_loc, &next_WP); wp_distance = get_distance(&current_loc, &next_WP);
home_distance = get_distance(&current_loc, &home); home_distance = get_distance(&current_loc, &home);
if (wp_distance < 0){ if (wp_distance < 0){
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0")); // something went very wrong
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return 0; return 0;
} }
@ -61,9 +59,8 @@ static void calc_XY_velocity(){
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp;
// slight averaging filter // slight averaging filter
x_GPS_speed = (x_GPS_speed + x_estimate) >> 1; x_actual_speed = (x_actual_speed + x_estimate) >> 1;
y_GPS_speed = (y_GPS_speed + y_estimate) >> 1; y_actual_speed = (y_actual_speed + y_estimate) >> 1;
//*/
/* /*
// Ryan Beall's forward estimator: // Ryan Beall's forward estimator:
@ -99,67 +96,7 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
} }
/*
//static void calc_loiter3(int x_error, int y_error)
{
static int32_t gps_lat_I = 0;
static int32_t gps_lon_I = 0;
// If close to goal <1m reset the I term
if (abs(x_error) < 50)
gps_lon_I = 0;
if (abs(y_error) < 50)
gps_lat_I = 0;
gps_lon_I += x_error;
gps_lat_I += y_error;
gps_lon_I = constrain(gps_lon_I,-3000,3000);
gps_lat_I = constrain(gps_lat_I,-3000,3000);
int16_t lon_P = 1.2 * (float)x_error;
int16_t lon_I = 0.1 * (float)gps_lon_I; //.1
int16_t lon_D = 3 * x_GPS_speed ; // this controls the small bumps
int16_t lat_P = 1.2 * (float)y_error;
int16_t lat_I = 0.1 * (float)gps_lat_I;
int16_t lat_D = 3 * y_GPS_speed ;
//limit of terms
lon_I = constrain(lon_I,-3000,3000);
lat_I = constrain(lat_I,-3000,3000);
lon_D = constrain(lon_D,-500,500); //this controls the long distance dampimg
lat_D = constrain(lat_D,-500,500); //this controls the long distance dampimg
nav_lon = lon_P + lon_I - lon_D;
nav_lat = lat_P + lat_I - lat_D;
Serial.printf("%d, %d, %d, %d, %d, %d\n",
lon_P, lat_P,
lon_I, lat_I,
lon_D, lat_D);
}
*/
#define NAV_ERR_MAX 800 #define NAV_ERR_MAX 800
static void calc_loiter1(int x_error, int y_error)
{
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps
int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav);
int16_t lat_D = g.loiter_d * y_actual_speed ;
//limit of terms
lon_D = constrain(lon_D,-500,500);
lat_D = constrain(lat_D,-500,500);
nav_lon = constrain(lon_PI - lon_D, -2500, 2500);
nav_lat = constrain(lat_PI - lat_D, -2500, 2500);
}
static void calc_loiter(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
// East/West // East/West
@ -167,22 +104,20 @@ static void calc_loiter(int x_error, int y_error)
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed; x_rate_error = x_target_speed - x_actual_speed;
nav_lon_p = x_rate_error * g.loiter_d;
nav_lon_p = constrain(nav_lon_p, -1200, 1200);
nav_lon = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -2500, 2500);
// North/South // North/South
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed; y_rate_error = y_target_speed - y_actual_speed;
nav_lat_p = y_rate_error * g.loiter_d;
nav_lat_p = constrain(nav_lat_p, -1200, 1200); calc_nav_lon(x_rate_error);
nav_lat = nav_lat_p + y_iterm; calc_nav_lat(y_rate_error);
nav_lat = constrain(nav_lat, -2500, 2500);
nav_lat = nav_lat + y_iterm;
nav_lon = nav_lon + x_iterm;
/* /*
int8_t ttt = 1.0/dTnav; int8_t ttt = 1.0/dTnav;
@ -204,7 +139,7 @@ static void calc_loiter(int x_error, int y_error)
//*/ //*/
/* /*
int16_t t1 = g.pi_nav_lon.get_integrator(); // X int16_t t1 = g.pid_nav_lon.get_integrator(); // X
Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n", Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n",
wp_distance, //1 wp_distance, //1
dTnav, //2 dTnav, //2
@ -219,33 +154,91 @@ static void calc_loiter(int x_error, int y_error)
//*/ //*/
} }
static void calc_nav_rate(int max_speed)
{
// push us towards the original track
update_crosstrack();
// nav_bearing includes crosstrack
float temp = (9000l - nav_bearing) * RADX100;
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
x_rate_error = constrain(x_rate_error, -1000, 1000);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
calc_nav_lon(x_rate_error);
calc_nav_lat(y_rate_error);
nav_lon = nav_lon + x_iterm;
nav_lat = nav_lat + y_iterm;
/*
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
max_speed,
x_actual_speed,
y_actual_speed,
x_rate_error,
y_rate_error,
nav_lon,
nav_lat,
x_iterm,
y_iterm,
crosstrack_error);
//*/
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
max_speed,
x_actual_speed,
y_actual_speed,
x_rate_error,
y_rate_error,
nav_lon,
nav_lat);*/
}
static void calc_nav_lon(int rate)
{
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
nav_lon = get_corrected_angle(rate, nav_lon);
nav_lon = constrain(nav_lon, -3000, 3000);
}
static void calc_nav_lat(int rate)
{
nav_lat = g.pid_nav_lon.get_pid(rate, dTnav);
nav_lat = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000);
}
static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
{
int16_t tt = desired_rate;
// scale down the desired rate and square it
desired_rate = desired_rate / 20;
desired_rate = desired_rate * desired_rate;
int16_t tmp = 0;
if (tt > 0){
tmp = rate_out + (rate_out - desired_rate);
tmp = max(tmp, rate_out);
}else if (tt < 0){
tmp = rate_out + (rate_out + desired_rate);
tmp = min(tmp, rate_out);
}
//Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp);
return tmp;
}
//wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2 //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
#define ERR_GAIN .01
// called at 50hz
static void estimate_velocity()
{
// we need to extimate velocity when below GPS threshold of 1.5m/s
//if(g_gps->ground_speed < 120){
// some smoothing to prevent bumpy rides
x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16;
y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16;
// integration of nav_p angle
//x_actual_speed += (nav_lon_p >>2);
//y_actual_speed += (nav_lat_p >>2);
// this is just what worked best in SIM
//x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4;
//y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4;
//}else{
// less smoothing needed since the GPS already filters
// x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
// y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
//}
}
// this calculation rotates our World frame of reference to the copter's frame of reference // this calculation rotates our World frame of reference to the copter's frame of reference
// We use the DCM's matrix to precalculate these trig values at 50hz // We use the DCM's matrix to precalculate these trig values at 50hz
@ -291,55 +284,6 @@ static int16_t calc_desired_speed(int16_t max_speed)
return max_speed; return max_speed;
} }
static void calc_nav_rate(int max_speed)
{
// push us towards the original track
update_crosstrack();
// nav_bearing includes crosstrack
float temp = (9000l - nav_bearing) * RADX100;
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
x_rate_error = constrain(x_rate_error, -1000, 1000);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
nav_lon = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -3000, 3000);
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
nav_lat = nav_lat_p + y_iterm;
nav_lat = constrain(nav_lat, -3000, 3000);
/*
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
max_speed,
x_actual_speed,
y_actual_speed,
x_rate_error,
y_rate_error,
nav_lon,
nav_lat,
x_iterm,
y_iterm,
crosstrack_error);
//*/
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
max_speed,
x_actual_speed,
y_actual_speed,
x_rate_error,
y_rate_error,
nav_lon,
nav_lat);*/
}
static void update_crosstrack(void) static void update_crosstrack(void)
{ {
@ -433,7 +377,7 @@ static int32_t get_new_altitude()
} }
int32_t diff = abs(next_WP.alt - target_altitude); int32_t diff = abs(next_WP.alt - target_altitude);
int8_t _scale = 3; int8_t _scale = 4;
if (next_WP.alt < target_altitude){ if (next_WP.alt < target_altitude){
// we are below the target alt // we are below the target alt
@ -444,12 +388,12 @@ static int32_t get_new_altitude()
} }
}else { }else {
// we are above the target, going down // we are above the target, going down
if(diff < 600){ if(diff < 400){
_scale = 4;
}
if(diff < 300){
_scale = 5; _scale = 5;
} }
if(diff < 100){
_scale = 6;
}
} }
int32_t change = (millis() - alt_change_timer) >> _scale; int32_t change = (millis() - alt_change_timer) >> _scale;

4
ArduCopter/sensors.pde
View File

@ -52,8 +52,8 @@ static void init_barometer(void)
static void reset_baro(void) static void reset_baro(void)
{ {
ground_pressure = barometer.get_pressure(); ground_pressure = barometer.get_pressure();
ground_temperature = barometer.get_temperature(); ground_temperature = barometer.get_temperature();
} }
static int32_t read_barometer(void) static int32_t read_barometer(void)

2
ArduCopter/setup.pde
View File

@ -1096,7 +1096,7 @@ static void print_enabled(boolean b)
static void static void
init_esc() init_esc()
{ {
motors_output_enable(); motors_output_enable();
while(1){ while(1){
read_radio(); read_radio();
delay(100); delay(100);

4
ArduCopter/system.pde
View File

@ -354,8 +354,8 @@ static void init_ardupilot()
Log_Write_Startup(); Log_Write_Startup();
Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(10, g.pi_stabilize_roll.kP());
Log_Write_Data(11, g.pi_stabilize_pitch.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP());
Log_Write_Data(12, g.pi_rate_roll.kP()); Log_Write_Data(12, g.pid_rate_roll.kP());
Log_Write_Data(13, g.pi_rate_pitch.kP()); Log_Write_Data(13, g.pid_rate_pitch.kP());
#endif #endif
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");

284
ArduCopter/test.pde
View File

@ -94,7 +94,7 @@ test_mode(uint8_t argc, const Menu::arg *argv)
{ {
//Serial.printf_P(PSTR("Test Mode\n\n")); //Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run(); test_menu.run();
return 0; return 0;
} }
static int8_t static int8_t
@ -469,7 +469,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_imu(uint8_t argc, const Menu::arg *argv) test_imu(uint8_t argc, const Menu::arg *argv)
{ {
#if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
print_test_disabled(); print_test_disabled();
return (0); return (0);
#else #else
@ -515,7 +515,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
//dcm.kp_yaw(0.02); //dcm.kp_yaw(0.02);
//dcm.ki_yaw(0); //dcm.ki_yaw(0);
imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler);
report_imu(); report_imu();
imu.init_gyro(delay, flash_leds); imu.init_gyro(delay, flash_leds);
@ -555,7 +555,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
compass.calculate(dcm.get_dcm_matrix()); compass.calculate(dcm.get_dcm_matrix());
} }
} }
fast_loopTimer = millis(); fast_loopTimer = millis();
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -731,59 +731,68 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_battery(uint8_t argc, const Menu::arg *argv) test_battery(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
//delta_ms_medium_loop = 100; print_test_disabled();
return (0);
#else
print_hit_enter();
while(1){
delay(100);
read_radio();
read_battery();
if (g.battery_monitoring == 3){
Serial.printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
while(1){ if(Serial.available() > 0){
delay(100); return (0);
read_radio(); }
read_battery();
if (g.battery_monitoring == 3){
Serial.printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} }
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); return (0);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); #endif
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
if(Serial.available() > 0){
return (0);
}
}
return (0);
} }
static int8_t test_relay(uint8_t argc, const Menu::arg *argv) static int8_t test_relay(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
delay(1000); print_test_disabled();
return (0);
#else
while(1){ print_hit_enter();
Serial.printf_P(PSTR("Relay on\n")); delay(1000);
relay.on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.printf_P(PSTR("Relay off\n")); while(1){
relay.off(); Serial.printf_P(PSTR("Relay on\n"));
delay(3000); relay.on();
if(Serial.available() > 0){ delay(3000);
return (0); if(Serial.available() > 0){
return (0);
}
Serial.printf_P(PSTR("Relay off\n"));
relay.off();
delay(3000);
if(Serial.available() > 0){
return (0);
}
} }
} #endif
} }
static int8_t static int8_t
test_wp(uint8_t argc, const Menu::arg *argv) test_wp(uint8_t argc, const Menu::arg *argv)
{ {
@ -810,20 +819,20 @@ test_wp(uint8_t argc, const Menu::arg *argv)
/* /*
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
while(1){ while(1){
if (Serial3.available()){ if (Serial3.available()){
digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read()); Serial1.write(Serial3.read());
digitalWrite(B_LED_PIN, LED_OFF); digitalWrite(B_LED_PIN, LED_OFF);
} }
if (Serial1.available()){ if (Serial1.available()){
digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read()); Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
} }
} }
*/ */
//} //}
@ -836,7 +845,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){ while(1){
if (Serial3.available()) if (Serial3.available())
Serial3.write(Serial3.read()); Serial3.write(Serial3.read());
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -850,30 +859,30 @@ test_wp(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_baro(uint8_t argc, const Menu::arg *argv) test_baro(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
init_barometer(); print_test_disabled();
return (0);
#else
print_hit_enter();
init_barometer();
while(1){ while(1){
delay(100); delay(100);
int32_t alt = read_barometer(); // calls barometer.read() int32_t alt = read_barometer(); // calls barometer.read()
#if defined( __AVR_ATmega1280__ ) int32_t pres = barometer.get_pressure();
Serial.printf_P(PSTR("alt: %ldcm\n"),alt); int16_t temp = barometer.get_temperature();
int32_t raw_pres = barometer.get_raw_pressure();
#else int32_t raw_temp = barometer.get_raw_temp();
int32_t pres = barometer.get_pressure(); Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC,"
int16_t temp = barometer.get_temperature(); " raw pres: %ld, raw temp: %ld\n"),
int32_t raw_pres = barometer.get_raw_pressure(); alt, pres ,temp, raw_pres, raw_temp);
int32_t raw_temp = barometer.get_raw_temp(); if(Serial.available() > 0){
Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," return (0);
" raw pres: %ld, raw temp: %ld\n"), }
alt, pres ,temp, raw_pres, raw_temp);
#endif
if(Serial.available() > 0){
return (0);
} }
} return 0;
return 0; #endif
} }
#endif #endif
@ -881,35 +890,38 @@ test_baro(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if(g.compass_enabled) { #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); print_test_disabled();
print_hit_enter();
while(1){
delay(100);
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
} else {
Serial.println_P(PSTR("not healthy"));
}
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0); return (0);
} #else
return (0); if(g.compass_enabled) {
print_hit_enter();
while(1){
delay(100);
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
} else {
Serial.println_P(PSTR("not healthy"));
}
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
return (0);
#endif
} }
/* /*
@ -1023,25 +1035,31 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
/* /*
test the dataflash is working test the dataflash is working
*/ */
static int8_t static int8_t
test_logging(uint8_t argc, const Menu::arg *argv) test_logging(uint8_t argc, const Menu::arg *argv)
{ {
Serial.println_P(PSTR("Testing dataflash logging")); #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included
if (!DataFlash.CardInserted()) { print_test_disabled();
Serial.println_P(PSTR("ERR: No dataflash inserted")); return (0);
return 0; #else
} Serial.println_P(PSTR("Testing dataflash logging"));
DataFlash.ReadManufacturerID(); if (!DataFlash.CardInserted()) {
Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), Serial.println_P(PSTR("ERR: No dataflash inserted"));
(unsigned)DataFlash.df_manufacturer, return 0;
(unsigned)DataFlash.df_device); }
Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), DataFlash.ReadManufacturerID();
(unsigned)DataFlash.df_NumPages+1, Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"),
(unsigned)DataFlash.df_PageSize); (unsigned)DataFlash.df_manufacturer,
DataFlash.StartRead(DataFlash.df_NumPages+1); (unsigned)DataFlash.df_device);
Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"),
(unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); (unsigned)DataFlash.df_NumPages+1,
return 0; (unsigned)DataFlash.df_PageSize);
DataFlash.StartRead(DataFlash.df_NumPages+1);
Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"),
(unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
return 0;
#endif
} }
@ -1061,11 +1079,11 @@ static int8_t
//} //}
// clear home // clear home
{Location t = {0, 0, 0, 0, 0, 0}; {Location t = {0, 0, 0, 0, 0, 0};
set_cmd_with_index(t,0);} set_cmd_with_index(t,0);}
// CMD opt pitch alt/cm // CMD opt pitch alt/cm
{Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0};
set_cmd_with_index(t,1);} set_cmd_with_index(t,1);}
if (!strcmp_P(argv[1].str, PSTR("wp"))) { if (!strcmp_P(argv[1].str, PSTR("wp"))) {
@ -1074,25 +1092,25 @@ static int8_t
{Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0};
set_cmd_with_index(t,2);} set_cmd_with_index(t,2);}
// CMD opt // CMD opt
{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0};
set_cmd_with_index(t,3);} set_cmd_with_index(t,3);}
// CMD opt // CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_cmd_with_index(t,4);} set_cmd_with_index(t,4);}
} else { } else {
//2250 = 25 meteres //2250 = 25 meteres
// CMD opt p1 //alt //NS //WE // CMD opt p1 //alt //NS //WE
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19
set_cmd_with_index(t,2);} set_cmd_with_index(t,2);}
// CMD opt dir angle/deg deg/s relative // CMD opt dir angle/deg deg/s relative
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
set_cmd_with_index(t,3);} set_cmd_with_index(t,3);}
// CMD opt // CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_cmd_with_index(t,4);} set_cmd_with_index(t,4);}
} }

9
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -253,6 +253,12 @@
<Compile Include="Controls\XorPlus.Designer.cs"> <Compile Include="Controls\XorPlus.Designer.cs">
<DependentUpon>XorPlus.cs</DependentUpon> <DependentUpon>XorPlus.cs</DependentUpon>
</Compile> </Compile>
<Compile Include="SerialInput.cs">
<SubType>Form</SubType>
</Compile>
<Compile Include="SerialInput.Designer.cs">
<DependentUpon>SerialInput.cs</DependentUpon>
</Compile>
<Compile Include="GCSViews\Firmware.Designer.cs"> <Compile Include="GCSViews\Firmware.Designer.cs">
<DependentUpon>Firmware.cs</DependentUpon> <DependentUpon>Firmware.cs</DependentUpon>
</Compile> </Compile>
@ -432,6 +438,9 @@
<EmbeddedResource Include="Controls\XorPlus.resx"> <EmbeddedResource Include="Controls\XorPlus.resx">
<DependentUpon>XorPlus.cs</DependentUpon> <DependentUpon>XorPlus.cs</DependentUpon>
</EmbeddedResource> </EmbeddedResource>
<EmbeddedResource Include="SerialInput.resx">
<DependentUpon>SerialInput.cs</DependentUpon>
</EmbeddedResource>
<EmbeddedResource Include="GCSViews\Configuration.fr.resx"> <EmbeddedResource Include="GCSViews\Configuration.fr.resx">
<DependentUpon>Configuration.cs</DependentUpon> <DependentUpon>Configuration.cs</DependentUpon>
</EmbeddedResource> </EmbeddedResource>

35
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -198,11 +198,43 @@ namespace ArdupilotMega
this.Lng = pll.Lng; this.Lng = pll.Lng;
} }
public PointLatLngAlt(PointLatLngAlt plla)
{
this.Lat = plla.Lat;
this.Lng = plla.Lng;
this.Alt = plla.Alt;
this.color = plla.color;
this.Tag = plla.Tag;
}
public PointLatLng Point() public PointLatLng Point()
{ {
return new PointLatLng(Lat, Lng); return new PointLatLng(Lat, Lng);
} }
public override bool Equals(Object pllaobj)
{
PointLatLngAlt plla = (PointLatLngAlt)pllaobj;
if (plla == null)
return false;
if (this.Lat == plla.Lat &&
this.Lng == plla.Lng &&
this.Alt == plla.Alt &&
this.color == plla.color &&
this.Tag == plla.Tag)
{
return true;
}
return false;
}
public override int GetHashCode()
{
return (int)((Lat + Lng + Alt) * 100);
}
/// <summary> /// <summary>
/// Calc Distance in M /// Calc Distance in M
/// </summary> /// </summary>
@ -263,7 +295,8 @@ namespace ArdupilotMega
RTL = 6, // AUTO control RTL = 6, // AUTO control
CIRCLE = 7, CIRCLE = 7,
POSITION = 8, POSITION = 8,
LAND = 9 // AUTO control LAND = 9, // AUTO control
OF_LOITER = 10
} }
public static void linearRegression() public static void linearRegression()

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

@ -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 dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle(); System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = 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();
@ -285,6 +285,8 @@
this.BUT_load = new ArdupilotMega.MyButton(); this.BUT_load = new ArdupilotMega.MyButton();
this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components);
this.BUT_compare = new ArdupilotMega.MyButton(); this.BUT_compare = new ArdupilotMega.MyButton();
this.myLabel2 = new ArdupilotMega.MyLabel();
this.TUNE = new System.Windows.Forms.ComboBox();
((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit();
this.ConfigTabs.SuspendLayout(); this.ConfigTabs.SuspendLayout();
this.TabAP.SuspendLayout(); this.TabAP.SuspendLayout();
@ -405,14 +407,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");
dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon; dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon;
dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White; dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White;
dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight; dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText; dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True; dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1; this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3;
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,
@ -421,14 +423,14 @@
this.mavScale, this.mavScale,
this.RawValue}); this.RawValue});
this.Params.Name = "Params"; this.Params.Name = "Params";
dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft;
dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption; dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption;
dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText; dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText;
dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight; dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight;
dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText; dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText;
dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True; dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True;
this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4;
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);
// //
@ -1089,6 +1091,8 @@
// //
// TabAC // TabAC
// //
this.TabAC.Controls.Add(this.myLabel2);
this.TabAC.Controls.Add(this.TUNE);
this.TabAC.Controls.Add(this.myLabel1); this.TabAC.Controls.Add(this.myLabel1);
this.TabAC.Controls.Add(this.CH7_OPT); this.TabAC.Controls.Add(this.CH7_OPT);
this.TabAC.Controls.Add(this.groupBox17); this.TabAC.Controls.Add(this.groupBox17);
@ -2092,6 +2096,40 @@
this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.UseVisualStyleBackColor = true;
this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click);
// //
// myLabel2
//
resources.ApplyResources(this.myLabel2, "myLabel2");
this.myLabel2.Name = "myLabel2";
this.myLabel2.resize = false;
//
// TUNE
//
this.TUNE.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
this.TUNE.FormattingEnabled = true;
this.TUNE.Items.AddRange(new object[] {
resources.GetString("TUNE.Items"),
resources.GetString("TUNE.Items1"),
resources.GetString("TUNE.Items2"),
resources.GetString("TUNE.Items3"),
resources.GetString("TUNE.Items4"),
resources.GetString("TUNE.Items5"),
resources.GetString("TUNE.Items6"),
resources.GetString("TUNE.Items7"),
resources.GetString("TUNE.Items8"),
resources.GetString("TUNE.Items9"),
resources.GetString("TUNE.Items10"),
resources.GetString("TUNE.Items11"),
resources.GetString("TUNE.Items12"),
resources.GetString("TUNE.Items13"),
resources.GetString("TUNE.Items14"),
resources.GetString("TUNE.Items15"),
resources.GetString("TUNE.Items16"),
resources.GetString("TUNE.Items17"),
resources.GetString("TUNE.Items18"),
resources.GetString("TUNE.Items19")});
resources.ApplyResources(this.TUNE, "TUNE");
this.TUNE.Name = "TUNE";
//
// Configuration // Configuration
// //
resources.ApplyResources(this, "$this"); resources.ApplyResources(this, "$this");
@ -2479,5 +2517,7 @@
private System.Windows.Forms.Label label48; private System.Windows.Forms.Label label48;
private MyLabel myLabel1; private MyLabel myLabel1;
private System.Windows.Forms.ComboBox CH7_OPT; private System.Windows.Forms.ComboBox CH7_OPT;
private MyLabel myLabel2;
private System.Windows.Forms.ComboBox TUNE;
} }
} }

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

File diff suppressed because it is too large Load Diff

2
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx
View File

@ -134,7 +134,7 @@
<value>Point Camera Here</value> <value>Point Camera Here</value>
</data> </data>
<data name="contextMenuStrip1.Size" type="System.Drawing.Size, System.Drawing"> <data name="contextMenuStrip1.Size" type="System.Drawing.Size, System.Drawing">
<value>175, 48</value> <value>175, 70</value>
</data> </data>
<data name="&gt;&gt;contextMenuStrip1.Name" xml:space="preserve"> <data name="&gt;&gt;contextMenuStrip1.Name" xml:space="preserve">
<value>contextMenuStrip1</value> <value>contextMenuStrip1</value>

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

@ -290,11 +290,13 @@ namespace ArdupilotMega.GCSViews
} }
float ans; float ans;
if (float.TryParse(TXT_homealt.Text, out result) && float.TryParse(cell.Value.ToString(), out ans)) if (float.TryParse(cell.Value.ToString(), out ans))
{ {
ans = (int)ans; ans = (int)ans;
if (alt != 0) if (alt != 0) // use passed in value;
cell.Value = alt.ToString(); cell.Value = alt.ToString();
if (ans == 0)
cell.Value = 50;
// online verify height // online verify height
if (isonline && CHK_geheight.Checked) if (isonline && CHK_geheight.Checked)
{ {
@ -314,7 +316,7 @@ namespace ArdupilotMega.GCSViews
{ {
cell.Value = (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text)).ToString(); cell.Value = (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text)).ToString();
} // is relative and check height } // is relative and check height
else if (float.TryParse(TXT_homealt.Text, out result) && isonline && CHK_geheight.Checked) else if (isonline && CHK_geheight.Checked)
{ {
alt = (int)getGEAlt(lat, lng); alt = (int)getGEAlt(lat, lng);
@ -328,10 +330,6 @@ namespace ArdupilotMega.GCSViews
cell.Style.BackColor = Color.LightGreen; cell.Style.BackColor = Color.LightGreen;
} }
} }
else
{
cell.Value = 100 ;
}
} }
cell.DataGridView.EndEdit(); cell.DataGridView.EndEdit();

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

@ -348,7 +348,6 @@ namespace ArdupilotMega.GCSViews
ArdupilotMega.MainV2.config["REV_pitch"] = CHKREV_pitch.Checked.ToString(); ArdupilotMega.MainV2.config["REV_pitch"] = CHKREV_pitch.Checked.ToString();
ArdupilotMega.MainV2.config["REV_rudder"] = CHKREV_rudder.Checked.ToString(); ArdupilotMega.MainV2.config["REV_rudder"] = CHKREV_rudder.Checked.ToString();
ArdupilotMega.MainV2.config["GPSrate"] = GPSrate.Text; ArdupilotMega.MainV2.config["GPSrate"] = GPSrate.Text;
ArdupilotMega.MainV2.config["Xplanes"] = RAD_softXplanes.Checked.ToString();
ArdupilotMega.MainV2.config["MAVrollgain"] = TXT_rollgain.Text; ArdupilotMega.MainV2.config["MAVrollgain"] = TXT_rollgain.Text;
ArdupilotMega.MainV2.config["MAVpitchgain"] = TXT_pitchgain.Text; ArdupilotMega.MainV2.config["MAVpitchgain"] = TXT_pitchgain.Text;
@ -389,9 +388,6 @@ namespace ArdupilotMega.GCSViews
case "GPSrate": case "GPSrate":
GPSrate.Text = ArdupilotMega.MainV2.config[key].ToString(); GPSrate.Text = ArdupilotMega.MainV2.config[key].ToString();
break; break;
case "Xplanes":
RAD_softXplanes.Checked = bool.Parse(ArdupilotMega.MainV2.config[key].ToString());
break;
case "MAVrollgain": case "MAVrollgain":
TXT_rollgain.Text = ArdupilotMega.MainV2.config[key].ToString(); TXT_rollgain.Text = ArdupilotMega.MainV2.config[key].ToString();
break; break;

2
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -2006,7 +2006,7 @@ namespace ArdupilotMega
} }
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] != 254 && 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' || lastbad[1] == 'G') // out of sync
{ {
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r') if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r')
{ {

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

@ -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.1.29")] [assembly: AssemblyFileVersion("1.1.30")]
[assembly: NeutralResourcesLanguageAttribute("")] [assembly: NeutralResourcesLanguageAttribute("")]

4
Tools/ArdupilotMegaPlanner/Script.cs
View File

@ -10,8 +10,8 @@ namespace ArdupilotMega
{ {
DateTime timeout = DateTime.Now; DateTime timeout = DateTime.Now;
List<string> items = new List<string>(); List<string> items = new List<string>();
Microsoft.Scripting.Hosting.ScriptEngine engine; static Microsoft.Scripting.Hosting.ScriptEngine engine;
Microsoft.Scripting.Hosting.ScriptScope scope; static Microsoft.Scripting.Hosting.ScriptScope scope;
// keeps history // keeps history
MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t(); MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t();

135
Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs generated Normal file
View File

@ -0,0 +1,135 @@
namespace ArdupilotMega
{
partial class SerialInput
{
/// <summary>
/// Required designer variable.
/// </summary>
private System.ComponentModel.IContainer components = null;
/// <summary>
/// Clean up any resources being used.
/// </summary>
/// <param name="disposing">true if managed resources should be disposed; otherwise, false.</param>
protected override void Dispose(bool disposing)
{
if (disposing && (components != null))
{
components.Dispose();
}
base.Dispose(disposing);
}
#region Windows Form Designer generated code
/// <summary>
/// Required method for Designer support - do not modify
/// the contents of this method with the code editor.
/// </summary>
private void InitializeComponent()
{
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(SerialInput));
this.CMB_serialport = new System.Windows.Forms.ComboBox();
this.BUT_connect = new ArdupilotMega.MyButton();
this.CMB_baudrate = new System.Windows.Forms.ComboBox();
this.label1 = new System.Windows.Forms.Label();
this.LBL_location = new System.Windows.Forms.Label();
this.textBox1 = new System.Windows.Forms.TextBox();
this.SuspendLayout();
//
// CMB_serialport
//
this.CMB_serialport.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
this.CMB_serialport.FormattingEnabled = true;
this.CMB_serialport.Location = new System.Drawing.Point(13, 13);
this.CMB_serialport.Name = "CMB_serialport";
this.CMB_serialport.Size = new System.Drawing.Size(121, 21);
this.CMB_serialport.TabIndex = 0;
//
// BUT_connect
//
this.BUT_connect.Location = new System.Drawing.Point(279, 12);
this.BUT_connect.Name = "BUT_connect";
this.BUT_connect.Size = new System.Drawing.Size(75, 23);
this.BUT_connect.TabIndex = 1;
this.BUT_connect.Text = "Connect";
this.BUT_connect.UseVisualStyleBackColor = true;
this.BUT_connect.Click += new System.EventHandler(this.BUT_connect_Click);
//
// CMB_baudrate
//
this.CMB_baudrate.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
this.CMB_baudrate.FormattingEnabled = true;
this.CMB_baudrate.Items.AddRange(new object[] {
"4800",
"9600",
"14400",
"19200",
"28800",
"38400",
"57600",
"115200"});
this.CMB_baudrate.Location = new System.Drawing.Point(140, 12);
this.CMB_baudrate.Name = "CMB_baudrate";
this.CMB_baudrate.Size = new System.Drawing.Size(121, 21);
this.CMB_baudrate.TabIndex = 2;
//
// label1
//
this.label1.AutoSize = true;
this.label1.Location = new System.Drawing.Point(90, 47);
this.label1.Name = "label1";
this.label1.Size = new System.Drawing.Size(187, 13);
this.label1.TabIndex = 3;
this.label1.Text = "Pick the Nmea gps port and baud rate\r\n";
//
// LBL_location
//
this.LBL_location.AutoSize = true;
this.LBL_location.Font = new System.Drawing.Font("Microsoft Sans Serif", 14.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0)));
this.LBL_location.Location = new System.Drawing.Point(9, 78);
this.LBL_location.Name = "LBL_location";
this.LBL_location.Size = new System.Drawing.Size(50, 24);
this.LBL_location.TabIndex = 4;
this.LBL_location.Text = "0,0,0";
//
// textBox1
//
this.textBox1.Enabled = false;
this.textBox1.Location = new System.Drawing.Point(19, 126);
this.textBox1.Multiline = true;
this.textBox1.Name = "textBox1";
this.textBox1.Size = new System.Drawing.Size(335, 133);
this.textBox1.TabIndex = 5;
this.textBox1.Text = resources.GetString("textBox1.Text");
//
// SerialInput
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.ClientSize = new System.Drawing.Size(369, 300);
this.Controls.Add(this.textBox1);
this.Controls.Add(this.LBL_location);
this.Controls.Add(this.label1);
this.Controls.Add(this.CMB_baudrate);
this.Controls.Add(this.BUT_connect);
this.Controls.Add(this.CMB_serialport);
this.Icon = ((System.Drawing.Icon)(resources.GetObject("$this.Icon")));
this.Name = "SerialInput";
this.Text = "Follow Me";
this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.SerialOutput_FormClosing);
this.ResumeLayout(false);
this.PerformLayout();
}
#endregion
private System.Windows.Forms.ComboBox CMB_serialport;
private MyButton BUT_connect;
private System.Windows.Forms.ComboBox CMB_baudrate;
private System.Windows.Forms.Label label1;
private System.Windows.Forms.Label LBL_location;
private System.Windows.Forms.TextBox textBox1;
}
}

224
Tools/ArdupilotMegaPlanner/SerialInput.cs Normal file
View File

@ -0,0 +1,224 @@
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
namespace ArdupilotMega
{
public partial class SerialInput : Form
{
System.Threading.Thread t12;
static bool threadrun = false;
static internal SerialPort comPort = new SerialPort();
static internal PointLatLngAlt lastgotolocation = new PointLatLngAlt(0, 0, 0, "Goto last");
static internal PointLatLngAlt gotolocation = new PointLatLngAlt(0, 0, 0, "Goto");
static internal int intalt = 100;
public SerialInput()
{
InitializeComponent();
CMB_serialport.DataSource = SerialPort.GetPortNames();
if (threadrun)
{
BUT_connect.Text = "Stop";
}
}
private void BUT_connect_Click(object sender, EventArgs e)
{
if (comPort.IsOpen)
{
threadrun = false;
comPort.Close();
BUT_connect.Text = "Connect";
}
else
{
try
{
comPort.PortName = CMB_serialport.Text;
}
catch { MessageBox.Show("Invalid PortName"); return; }
try {
comPort.BaudRate = int.Parse(CMB_baudrate.Text);
} catch {MessageBox.Show("Invalid BaudRate"); return;}
try {
comPort.Open();
} catch {MessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;}
string alt = "100";
if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
alt = (10 * MainV2.cs.multiplierdist).ToString("0");
}
else
{
alt = (100 * MainV2.cs.multiplierdist).ToString("0");
}
if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Alt", ref alt))
return;
intalt = (int)(100 * MainV2.cs.multiplierdist);
if (!int.TryParse(alt, out intalt))
{
MessageBox.Show("Bad Alt");
return;
}
t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
{
IsBackground = true,
Name = "Nmea Input"
};
t12.Start();
BUT_connect.Text = "Stop";
}
}
void mainloop()
{
threadrun = true;
int counter = 0;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.cs.lng < 0 ? "W" : "E", MainV2.cs.gpsstatus, MainV2.cs.satcount, MainV2.cs.gpshdop, MainV2.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA")) //
{
int c1 = line.IndexOf(',',0)+ 1;
int c2 = line.IndexOf(',', c1) + 1;
int c3 = line.IndexOf(',', c2) + 1;
int c4 = line.IndexOf(',', c3 ) + 1;
int c5 = line.IndexOf(',', c4 ) + 1;
int c6 = line.IndexOf(',', c5 ) + 1;
int c7 = line.IndexOf(',', c6 ) + 1;
int c8 = line.IndexOf(',', c7 ) + 1;
int c9 = line.IndexOf(',', c8 ) + 1;
int c10 = line.IndexOf(',', c9 ) + 1;
int c11 = line.IndexOf(',', c10 ) + 1;
int c12 = line.IndexOf(',', c11) + 1;
gotolocation.Lat = double.Parse(line.Substring(c2, c3 - c2 - 1)) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (line.Substring(c3, c4 - c3 - 1) == "S")
gotolocation.Lat *= -1;
gotolocation.Lng = double.Parse(line.Substring(c4, c5 - c4 - 1)) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (line.Substring(c5, c6 - c5 - 1) == "W")
gotolocation.Lng *= -1;
gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +
}
if (counter % 10 == 0 && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (float)(gotolocation.Lat);
gotohere.lng = (float)(gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch { }
if (MainV2.comPort.BaseStream.IsOpen && MainV2.givecomport == false)
{
try
{
MainV2.givecomport = true;
MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2);
MainV2.givecomport = false;
}
catch { MainV2.givecomport = false; }
}
}
System.Threading.Thread.Sleep(200);
counter++;
}
catch { System.Threading.Thread.Sleep(2000); }
}
}
private void updateLocationLabel(Locationwp plla)
{
this.BeginInvoke((MethodInvoker)delegate
{
LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt;
}
);
}
private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e)
{
}
// Calculates the checksum for a sentence
string GetChecksum(string sentence)
{
// Loop through all chars to get a checksum
int Checksum = 0;
foreach (char Character in sentence.ToCharArray())
{
switch (Character)
{
case '$':
// Ignore the dollar sign
break;
case '*':
// Stop processing before the asterisk
continue;
default:
// Is this the first value for the checksum?
if (Checksum == 0)
{
// Yes. Set the checksum to the value
Checksum = Convert.ToByte(Character);
}
else
{
// No. XOR the checksum with this character's value
Checksum = Checksum ^ Convert.ToByte(Character);
}
break;
}
}
// Return the checksum formatted as a two-character hexadecimal
return Checksum.ToString("X2");
}
}
}

208
Tools/ArdupilotMegaPlanner/SerialInput.resx Normal file
View File

@ -0,0 +1,208 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<data name="textBox1.Text" xml:space="preserve">
<value>What this does.
1. gets the current gps coords from a nmea gps.
2. sends a guided mode WP to the AP every 2 seconds.
How to use it
1. connect to ap.
2. take off, test guided mode is working.
3. open this and pick your comport, and baud rate for your nmea gps.
4. it should now be following you.</value>
</data>
<assembly alias="System.Drawing" name="System.Drawing, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a" />
<data name="$this.Icon" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>
AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAABILAAASCwAAAAAAAAAA
AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADOxkjAtnoOAKpJ4vyiK
c+8nh3D/J4Zv/yeHcP8oi3PvKpJ4vy6fg4AzsZIwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADjGo2AyspPfLZ+D/yiQ
d/8hlXj/G6F9/xeqg/8XqYL/GKqD/xuhfv8ilnn/KZB3/y2fhP8yspPfN8ajYAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADvRrDA1vpzfL6uN/yel
hP8XvJD/DMyY/wfQl/8FzJP/A8qS/wPJkf8EypL/BsyU/wnRmP8PzZn/Gb2R/yemhP8tqoz/Mb2a3zbQ
qkAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAA4y6ZgMbWV/yin
iP8WwZP/Btqf/wDPlf8AyI7/A8aP/yfNnv9T2LP/UNax/03XsP8506b/G8ya/wHKkf8F0Zf/CNuf/xLB
kv8fpYT/J7KQ/y7IomAAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAANcajny+w
kf8hqoj/CNSd/wDRlf8Axor/Hcyd/3Lhwf+p7Nj/o+vV/57m0/+X5dD/k+TN/4/jzf+K5Mz/fuHH/0PW
rf8HzJT/ANCT/wDRlv8OpX//HayI/yrFn58AAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAADDC
nmAtro7/H62J/wPWmv8Ay47/AMaO/3XhxP+e6tT/mObP/5Pjy/+Q4sr/jODJ/4ffx/+C3MT/f9vC/3nb
wf9y2r7/adq7/2DauP8ZzZv/Fdae/8T/9/9WxKj/HKuI/y7IomAAAAAAAAAAAAAAAAD///8AAAAAAAAA
AAAiuZMwKKyM/x6ohf8C1Zr/AMmL/wHGjv+49OL///////////9+3ML/f9zD/4Dcwv9+28L/e9rA/3bZ
vv9w1rr/Z9S4/17Rs/9Qz63/Qcyn/3LewP////////////n///8MpH7/JbKP/zXQqUAAAAAAAAAAAP//
/wAAAAAAAAAAABymhN8dnn//BNGa/wDKjP8AxY3/sfHf/////////////////2nXt/9w1rv/c9e8/3TX
vP9x17z/a9W5/2TTtf9Y0K//SMyp/zXFoP9i07X/////////////////f/LR/wDQlf8epYT/Mb2a3wAA
AAAAAAAA////AAAAAAADlnJgFZR1/wq4iv8AzpH/AMCD/4rmzf//////////////////////WdGv/2PU
tf9p1rf/atS4/2nUtv9i0rT/Vc+u/0fKpv8zxZz/Ws+w//////////////////////8GyJL/ANCS/xLB
kv8tq4z/OMajYAAAAAD///8AAAAAAACHZt8NkW//ANKV/wDChP9i27r//////////////////////9Dx
6P9MzKn/Vc+v/17Rsv9g0rP/XNCx/1XNrv9Fyaf/McSd/1fPr///////////////////////QM2m/ynK
oP8JzJX/C9yh/ymmhf80spPfAAAAAP///wAAcUwwAHtc/wCrfP8AyIv/AMKK////////////////////
/////////////5Dgyv9Gyqb/TMyq/07Nq/9MzKn/Qcmj/y/Fnf9Wzq3//////////////////////57k
0v8av5T/Lceg/yzOo/8M05v/Hr6T/zCghf80spIw////AABoRYAAclT/AL2H/wDBhf9R1rL/////////
////////4vfw//////////////////H8+P9KzKn/Ocah/zTFnv8qwpj/Us2t////////////////////
////////DLqM/yDBlv8wxp//OM6m/xPPm/8Xz53/LZF5/y+fg4////8AAGNAvwB7Wf8Aw4j/ALyC/4bj
yP+g5tL/g93E/2HSsv9Pzqz/Us6s//////////////////////9Yzq//Gr2S/0jLp///////////////
/////////////yrDm/8SvI//JMGY/zDHn/81zKT/Is2e/xTUnf8nl3v/LJJ5v////wAAXz3vAIlg/wDA
hf8AuoD/quzZ/5Hjyv9628D/ada2/1jRsP9Jy6f/a9a4//////////////////////+Y4s7/////////
//////////////////+c4tD/AbaH/xW8kf8jwZj/LcWd/y/Jn/8kzJ3/E9Ca/yGjgf8ri3Tv////AABd
PP8Ak2b/AL6D/w/Ekv+m6tf/j+HJ/3vawP9p1rf/W9Gx/0rNqf85yJ//Nsaf////////////////////
/////////////////////////////wCwe/8AtoT/ELqP/xu+k/8jwZj/KMeb/yHKm/8QzZf/HqyG/ymI
cf////8AAF07/wCSZP8AvYL/GMWU/6Dn1P+K38f/ddi+/27Wuf+E3MX/leHN/6fm1f+l5tX/neLQ////
////////////////////////////////////////j9/J/27Vuv9Tzq7/JsKY/xa/kv8aw5T/FcaW/wvL
lf8aqoT/J4dw/////wAAXTv/AJFk/wC9gP8GwY3/mObQ/5rkz/+26dv/y/Hl/8Dt3/+06tz/pebV/5bg
zP+g5NL//////////////v///f7+//7+/v//////7fn2////////////tOnb/6Ll0v+v6Nj/jeDI/zXK
o/8IxJD/BMqS/xaqgv8lh2//////AABeO+8AgVf/AL1//wDBif/R9uv/1PPq/8Tv5P+36t3/rujY/6Lk
0v+U4cv/jt7J//j8+///////+/38//f8+//2+/r/+Pz7//3+/v/m9/P/9Pv6//D6+P9/28L/jd7J/5jj
z/+h5dL/qOvX/4Hmyf8f1J//E596/yOJcO////8AAGA8vwB3U/8p06P/hufM/8Ty5f/D7+T/s+vb/6bm
1P+c4c//j9/K/4vcyP/t+fb///7///j8/P/0+/r/8vr5//P7+f/1+/r/+/39///////i9fL/ZNO1/3HW
vP992sH/htzG/4vhyv+S5dD/mO7W/6X74v80noT/Io90v////wAAZkCAAHla/33ny/945cb/nunV/7Xr
3v+l5tT/luDN/4ndxv992cL/1vLq//v9/P/1+/n/8vv4//L69//z+/j/9Pv5/7Xo2//x+vn/////////
//+y59n/aNS3/3LWvP932r//fNzD/4Ljyf+J7ND/l/bd/yORdf8knH6A////AABuRzAAdlT/Xc6x/23o
xv9s4MH/qurZ/5jiz/+I3cb/edjA/8ju5f/3/Pv/8vv4//H6+P/y+/j/6/f0/7np3v/7/fz//v7+/6fk
1f+56tz///////////9h0bT/aNW4/23Wu/9v3L//dOLG/37w0f9m1rn/Hpt8/ymujTD///8AAAAAAACD
X98po4X/Z+7K/1vgvP+A4sf/jOHK/3rZwv+r59f/9Pv6/+/69//v+vf/8vr4/9fy6/9n0rf/VM6t/6Di
0v/N7+f/adO4/1PMrf9t1Lr/i9zI/1/Rs/9h0rX/ZNe4/2bbvf9s5sb/ePfV/z2ylf8lrozfAAAAAP//
/wAAAAAAAJNsYAWQbf9U1rP/Vee//0rYsf993sb/pebV//P7+v/s+Pb/6/f1/+749v+s5tj/Vc2u/1jP
r/9ZzrD/btW5/1bOr/9Wza//Vs6v/1fOr/9Z0LD/WdCy/1vTtP9d1rX/Xt+8/2btyP9k4L//IaaF/y7D
nmAAAAAA////AAAAAAAAAAAAD6J9zyCjgv9S68L/P9+0/2Pevv/5////7/v6/+v59//j9/L/gtvF/1PN
r/9Wz7D/Wc+x/1nQsf9Zz7H/WM6w/1fPsP9UzrD/VM+w/1TPrv9U0a//U9Oy/1Tatv9Z5sD/Y/LL/zSx
lP8qupbPAAAAAAAAAAD///8AAAAAAAAAAAAYto4wGaeE/y23lP8+5rn/6/////j////w//3/ve/i/2bV
uP9Tzq7/Vc+v/1jPsP9Z0LL/WM+w/1fOsf9Wz7D/Us2w/1HOrf9Qzq3/T9Cu/0zSr/9M2LP/TeC5/1bt
xP9HxaX/KLKQ/zTPqDAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAkvpdgG6iF/y++m//e/////P///3rl
yf9G0K3/VdKy/1bPsf9Wz7H/Vs6w/1bPsP9Sza//Ucyu/0/Nrf9NzKz/S82s/0fOrP9G0a7/QdWv/0Le
tP9I6L7/Q8Ok/yitjP8yyKJgAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAmwJlgG6iF/yK3
kP8k3q7/H9el/x7Pn/8tzKT/Q9Cs/1HQsP9Q0K7/TM6u/0nMrf9Hzaz/RMyp/0LNqf8+zqn/ONGo/zTV
qf833rD/O+S4/zvCof8orIv/MMSfYAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAA
AAAkvpdgG6iE/xukgv8gy53/HNql/xzRn/8czJz/HcmZ/yXJnP8qyp7/Lcqg/yzLn/8nypz/JMqc/yTO
n/8l1KT/KN2r/y3Tpv8nq4n/JaqJ/yzAm2AAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAato8wFKN/zxCScv8RnHn/DbqM/wjIlP8GyZT/BsaS/wbFkf8GxZH/B8WR/wfH
k/8IypX/DMmV/xG3jP8WoX3/Fph2/xqkgs8ft5EwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA////AAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAJVvYACGZM8Aelr/AHlZ/wCFX/8AiWL/AJlr/wCb
bP8AlGf/AI5k/wB/W/8AeFj/AHtb/wCHZd8ClXBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAABwSzAAaESAAGI/vwBf
Pd8AXTz/AF08/wBdPP8AXz3fAGJAvwBoRIAAcUswAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP//
/wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP//
/wD///8A////AP///wD///8A/+AD//+AAP/+AAA//AAAH/gAAA/wAAAH4AAAA+AAAAPAAAABwAAAAYAA
AACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAwAAAAcAAAAHgAAAD4AAAA/AA
AAf4AAAP/AAAH/4AAD//gAD//+AD//////8=
</value>
</data>
</root>

13
Tools/ArdupilotMegaPlanner/temp.Designer.cs generated
View File

@ -45,6 +45,7 @@
this.BUT_clearcustommaps = new ArdupilotMega.MyButton(); this.BUT_clearcustommaps = new ArdupilotMega.MyButton();
this.BUT_lang_edit = new ArdupilotMega.MyButton(); this.BUT_lang_edit = new ArdupilotMega.MyButton();
this.BUT_georefimage = new ArdupilotMega.MyButton(); this.BUT_georefimage = new ArdupilotMega.MyButton();
this.BUT_follow_me = new ArdupilotMega.MyButton();
this.SuspendLayout(); this.SuspendLayout();
// //
// button1 // button1
@ -212,11 +213,22 @@
this.BUT_georefimage.Text = "Geo ref images"; this.BUT_georefimage.Text = "Geo ref images";
this.BUT_georefimage.Click += new System.EventHandler(this.BUT_georefimage_Click); this.BUT_georefimage.Click += new System.EventHandler(this.BUT_georefimage_Click);
// //
// BUT_follow_me
//
this.BUT_follow_me.Location = new System.Drawing.Point(525, 164);
this.BUT_follow_me.Name = "BUT_follow_me";
this.BUT_follow_me.Size = new System.Drawing.Size(75, 23);
this.BUT_follow_me.TabIndex = 17;
this.BUT_follow_me.Text = "Follow Me";
this.BUT_follow_me.UseVisualStyleBackColor = true;
this.BUT_follow_me.Click += new System.EventHandler(this.BUT_follow_me_Click);
//
// temp // temp
// //
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.ClientSize = new System.Drawing.Size(731, 281); this.ClientSize = new System.Drawing.Size(731, 281);
this.Controls.Add(this.BUT_follow_me);
this.Controls.Add(this.BUT_georefimage); this.Controls.Add(this.BUT_georefimage);
this.Controls.Add(this.BUT_lang_edit); this.Controls.Add(this.BUT_lang_edit);
this.Controls.Add(this.BUT_clearcustommaps); this.Controls.Add(this.BUT_clearcustommaps);
@ -261,6 +273,7 @@
private MyButton BUT_clearcustommaps; private MyButton BUT_clearcustommaps;
private MyButton BUT_lang_edit; private MyButton BUT_lang_edit;
private MyButton BUT_georefimage; private MyButton BUT_georefimage;
private MyButton BUT_follow_me;
//private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1; //private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1;
} }

7
Tools/ArdupilotMegaPlanner/temp.cs
View File

@ -871,5 +871,12 @@ namespace ArdupilotMega
{ {
new georefimage().Show(); new georefimage().Show();
} }
private void BUT_follow_me_Click(object sender, EventArgs e)
{
SerialInput si = new SerialInput();
MainV2.fixtheme((Form)si);
si.Show();
}
} }
} }

8
Tools/autotest/arducopter.py
View File

@ -519,10 +519,10 @@ def fly_ArduCopter(viewerip=None):
print("land failed") print("land failed")
failed = True failed = True
print("# disarm motors") #print("# disarm motors")
if not disarm_motors(mavproxy, mav): #if not disarm_motors(mavproxy, mav):
print("disarm_motors failed") # print("disarm_motors failed")
failed = True # failed = True
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT, e:
failed = True failed = True

2
Tools/autotest/mission2.txt
View File

@ -23,7 +23,7 @@ QGC WPL 110
# MAV_CMD_NAV_LOITER_TURNS # MAV_CMD_NAV_LOITER_TURNS
# Turns lat lon alt continue # Turns lat lon alt continue
6 0 3 18 2 0 0 0 0 0 20 1 #6 0 3 18 2 0 0 0 0 0 20 1
# MAV_CMD_DO_SET_ROI, MAV_ROI_WPNEXT = 1 # MAV_CMD_DO_SET_ROI, MAV_ROI_WPNEXT = 1
# MAV_ROI WP index ROI index lat lon alt continue # MAV_ROI WP index ROI index lat lon alt continue

118
libraries/AC_PID/AC_PID.cpp Executable file
View File

@ -0,0 +1,118 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AC_PID.cpp
/// @brief Generic PID algorithm
#include <math.h>
#include "AC_PID.h"
int32_t AC_PID::get_p(int32_t error)
{
return (float)error * _kp;
}
int32_t AC_PID::get_i(int32_t error, float dt)
{
if((_ki != 0) && (dt != 0)){
_integrator += ((float)error * _ki) * dt;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
}
return _integrator;
}
int32_t AC_PID::get_d(int32_t input, float dt)
{
if ((_kd != 0) && (dt != 0)) {
_derivative = (input - _last_input) / dt;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
_derivative = _last_derivative +
(dt / ( _filter + dt)) * (_derivative - _last_derivative);
// update state
_last_input = input;
_last_derivative = _derivative;
// add in derivative component
return _kd * _derivative;
}
}
int32_t AC_PID::get_pi(int32_t error, float dt)
{
return get_p(error) + get_i(error, dt);
}
int32_t AC_PID::get_pid(int32_t error, float dt)
{
return get_p(error) + get_i(error, dt) + get_d(error, dt);
}
/*
int32_t AC_PID::get_pid(int32_t error, float dt)
{
// Compute proportional component
_output = error * _kp;
// Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) {
_derivative = (error - _last_input) / dt;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
_derivative = _last_derivative +
(dt / ( _filter + dt)) * (_derivative - _last_derivative);
// update state
_last_input = error;
_last_derivative = _derivative;
// add in derivative component
_output += _kd * _derivative;
}
// Compute integral component if time has elapsed
if ((fabs(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * dt;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
_output += _integrator;
}
return _output;
}
*/
void
AC_PID::reset_I()
{
_integrator = 0;
_last_input = 0;
_last_derivative = 0;
}
void
AC_PID::load_gains()
{
_group.load();
}
void
AC_PID::save_gains()
{
_group.save();
}

152
libraries/AC_PID/AC_PID.h Executable file
View File

@ -0,0 +1,152 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AC_PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef AC_PID_h
#define AC_PID_h
#include <AP_Common.h>
#include <math.h> // for fabs()
/// @class AC_PID
/// @brief Object managing one PID control
class AC_PID {
public:
/// Constructor for PID that saves its settings to EEPROM
///
/// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
/// @param key Storage key assigned to this PID. Should be unique.
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
AC_PID(AP_Var::Key key,
const prog_char_t *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.0) :
_group(key, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
// no need for explicit load, assuming that the main code uses AP_Var::load_all.
}
/// Constructor for PID that does not save its settings.
///
/// @param name Name by which the PID is known, or NULL for an anonymous PID.
/// The name is prefixed to the P, I, D, IMAX variable names when
/// they are reported.
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
AC_PID(const prog_char_t *name,
const float &initial_p = 0.0,
const float &initial_i = 0.0,
const float &initial_d = 0.0,
const int16_t &initial_imax = 0.0) :
_group(AP_Var::k_key_none, name),
// group, index, initial value, name
_kp (&_group, 0, initial_p, PSTR("P")),
_ki (&_group, 1, initial_i, PSTR("I")),
_kd (&_group, 2, initial_d, PSTR("D")),
_imax(&_group, 3, initial_imax, PSTR("IMAX"))
{
}
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
int32_t get_pid(int32_t error, float dt);
int32_t get_pi(int32_t error, float dt);
int32_t get_p(int32_t error);
int32_t get_i(int32_t error, float dt);
int32_t get_d(int32_t error, float dt);
/// Reset the PID integrator
///
void reset_I();
/// Load gain properties
///
void load_gains();
/// Save gain properties
///
void save_gains();
/// @name parameter accessors
//@{
/// Overload the function call operator to permit relatively easy initialisation
void operator() (const float p,
const float i,
const float d,
const int16_t imaxval) {
_kp = p; _ki = i; _kd = d; _imax = imaxval;
}
float kP() const { return _kp.get(); }
float kI() const { return _ki.get(); }
float kD() const { return _kd.get(); }
int16_t imax() const { return _imax.get(); }
void kP(const float v) { _kp.set(v); }
void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); }
void imax(const int16_t v) { _imax.set(abs(v)); }
float get_integrator() const { return _integrator; }
private:
AP_Var_group _group;
AP_Float16 _kp;
AP_Float16 _ki;
AP_Float16 _kd;
AP_Int16 _imax;
float _integrator; ///< integrator value
int32_t _last_input; ///< last input for derivative
float _last_derivative; ///< last derivative for low-pass filter
float _output;
float _derivative;
/// Low pass filter cut frequency for derivative calculation.
///
static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )";
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
};
#endif

9
libraries/AC_PID/keywords.txt Executable file
View File

@ -0,0 +1,9 @@
PID KEYWORD1
get_pid KEYWORD2
reset_I KEYWORD2
kP KEYWORD2
kD KEYWORD2
kI KEYWORD2
imax KEYWORD2
load_gains KEYWORD2
save_gains KEYWORD2

2
libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde
View File

@ -12,7 +12,7 @@
FastSerialPort0(Serial); // FTDI/console FastSerialPort0(Serial); // FTDI/console
Arduino_Mega_ISR_Registry isr_registry; Arduino_Mega_ISR_Registry isr_registry;
AP_PeriodicProcess adc_scheduler; AP_TimerProcess adc_scheduler;
unsigned long timer; unsigned long timer;

2
libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
View File

@ -1,8 +1,8 @@
#include <stdint.h> #include <stdint.h>
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <FastSerial.h>
#include <I2C.h> #include <I2C.h>
#include <SPI.h> #include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h> #include <Arduino_Mega_ISR_Registry.h>

17
libraries/AP_PID/examples/AP_pid/AP_pid.pde
View File

@ -3,21 +3,26 @@
2010 Code by Jason Short. DIYDrones.com 2010 Code by Jason Short. DIYDrones.com
*/ */
#include <Arduino_Mega_ISR_Registry.h>
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <APM_RC.h> // ArduPilot RC Library #include <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega RC Library #include <AP_PID.h> // ArduPilot Mega RC Library
long radio_in; long radio_in;
long radio_trim; long radio_trim;
PID pid(); Arduino_Mega_ISR_Registry isr_registry;
AP_PID pid;
APM_RC_APM1 APM_RC;
void setup() void setup()
{ {
Serial.begin(38400); Serial.begin(115200);
Serial.println("ArduPilot Mega PID library test"); Serial.println("ArduPilot Mega AP_PID library test");
APM_RC.Init(); // APM Radio initialization
isr_registry.init();
APM_RC.Init(&isr_registry); // APM Radio initialization
delay(1000); delay(1000);
//rc.trim(); //rc.trim();
@ -27,12 +32,10 @@ void setup()
pid.kI(0); pid.kI(0);
pid.kD(0.5); pid.kD(0.5);
pid.imax(50); pid.imax(50);
pid.save_gains();
pid.kP(0); pid.kP(0);
pid.kI(0); pid.kI(0);
pid.kD(0); pid.kD(0);
pid.imax(0); pid.imax(0);
pid.load_gains();
Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
} }

3
libraries/AP_RC_Channel/AP_RC_Channel.cpp
View File

@ -88,11 +88,14 @@ AP_RC_Channel::set_pwm(int pwm)
control_in = pwm_to_angle(); control_in = pwm_to_angle();
control_in = (abs(control_in) < dead_zone) ? 0 : control_in; control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
/*
// coming soon ??
if(expo) { if(expo) {
long temp = control_in; long temp = control_in;
temp = (temp * temp) / (long)_high; temp = (temp * temp) / (long)_high;
control_in = (int)((control_in >= 0) ? temp : -temp); control_in = (int)((control_in >= 0) ? temp : -temp);
} }
*/
} }
} }

1
libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde
View File

@ -29,7 +29,6 @@ AP_RC rc;
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
//Serial.begin(38400);
Serial.println("ArduPilot RC Channel test"); Serial.println("ArduPilot RC Channel test");
rc.init(); // APM Radio initialization rc.init(); // APM Radio initialization

7
libraries/FastSerial/examples/FastSerial/FastSerial.pde
View File

@ -17,8 +17,9 @@
#undef PROGMEM #undef PROGMEM
#define PROGMEM __attribute__(( section(".progmem.data") )) #define PROGMEM __attribute__(( section(".progmem.data") ))
#undef PSTR # undef PSTR
#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) # define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \
(prog_char_t *)&__c[0];}))
// //
// Create a FastSerial driver that looks just like the stock Arduino // Create a FastSerial driver that looks just like the stock Arduino
@ -38,7 +39,7 @@ void setup(void)
// //
// Set the speed for our replacement serial port. // Set the speed for our replacement serial port.
// //
Serial.begin(38400); Serial.begin(115200);
// //
// Test printing things // Test printing things

6
libraries/GPS_IMU/GPS_IMU.cpp
View File

@ -4,7 +4,11 @@
#include "GPS_IMU.h" #include "GPS_IMU.h"
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include "WProgram.h" #if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////