Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Big update 2.0.38

Browse Source 
moved ground start to first arming
added ground start flag
moved throttle_integrator to 50hz loop
CAMERA_STABILIZER deprecated - now always on
renamed current logging bit mask to match APM
added MA filter to PID - D term
Adjusted PIDs based on continued testing and new PID filter
added MASK_LOG_SET_DEFAULTS to match APM
moved some stuff out of ground start into system start where it belonged
Added slower Yaw gains for DCM when the copter is in the air
changed camera output to be none scaled PWM
fixed bug where ground_temperature was unfiltered
shortened Baro startup time
fixed issue with Nav_WP integrator not being reset
RTL no longer yaws towards home
Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-30 20:42:54 +00:00
parent e1b677a25d
commit 73be185414
19 changed files with 363 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopterMega/APM_Config.h
View File

@ -4,10 +4,12 @@
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//#define HIL_MODE HIL_MODE_ATTITUDE
#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)
//#define FRAME_CONFIG QUAD_FRAME //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)
#define FRAME_CONFIG QUAD_FRAME
/* /*
options: options:
QUAD_FRAME QUAD_FRAME
@ -18,15 +20,15 @@
HELI_FRAME HELI_FRAME
*/ */
//#define FRAME_ORIENTATION X_FRAME #define FRAME_ORIENTATION X_FRAME
/* /*
PLUS_FRAME PLUS_FRAME
X_FRAME X_FRAME
V_FRAME V_FRAME
*/ */
//#define CHANNEL_6_TUNING CH6_NONE #define CHANNEL_6_TUNING CH6_NONE
/* /*
CH6_NONE CH6_NONE
CH6_STABILIZE_KP CH6_STABILIZE_KP
@ -47,4 +49,4 @@
// experimental!! // experimental!!
// Yaw is controled by targeting home. you will not have Yaw override. // Yaw is controled by targeting home. you will not have Yaw override.
// flying too close to home may induce spins. // flying too close to home may induce spins.
#define SIMPLE_LOOK_AT_HOME 0 //#define SIMPLE_LOOK_AT_HOME 0

160
ArduCopterMega/ArduCopterMega.pde
View File

@ -62,6 +62,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
#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
#include <ModeFilter.h>
#define MAVLINK_COMM_NUM_BUFFERS 2 #define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions #include <GCS_MAVLink.h> // MAVLink GCS definitions
@ -226,12 +227,14 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
// SONAR selection // SONAR selection
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// //
ModeFilter sonar_mode_filter;
#if SONAR_TYPE == MAX_SONAR_XL #if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc); AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#elif SONAR_TYPE == MAX_SONAR_LV #elif SONAR_TYPE == MAX_SONAR_LV
// XXX honestly I think these output the same values // XXX honestly I think these output the same values
// If someone knows, can they confirm it? // If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc); AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -247,7 +250,8 @@ static const char* flight_mode_strings[] = {
"AUTO", "AUTO",
"GUIDED", "GUIDED",
"LOITER", "LOITER",
"RTL"}; "RTL",
"CIRCLE"};
/* Radio values /* Radio values
Channel assignments Channel assignments
@ -308,19 +312,20 @@ static const float t7 = 10000000.0; // used to scale GPS values for EEPROM st
static float scaleLongUp = 1; // used to reverse longtitude scaling static float scaleLongUp = 1; // used to reverse longtitude scaling
static float scaleLongDown = 1; // used to reverse longtitude scaling static float scaleLongDown = 1; // used to reverse longtitude scaling
static byte ground_start_count = 10; // have we achieved first lock and set Home? static byte ground_start_count = 10; // have we achieved first lock and set Home?
static bool did_ground_start = false; // have we ground started after first arming
// Location & Navigation // Location & Navigation
// --------------------- // ---------------------
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
static int last_ground_speed; // used to dampen navigation static int last_ground_speed; // used to dampen navigation
static int waypoint_speed; static int waypoint_speed;
static byte wp_control; // used to control - navgation or loiter static byte wp_control; // used to control - navgation or loiter
@ -328,7 +333,7 @@ static byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location static byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID static byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID static byte command_may_ID; // current command ID
static byte wp_verify_byte; // used for tracking state of navigating waypoints static byte wp_verify_byte; // used for tracking state of navigating waypoints
static float cos_roll_x = 1; static float cos_roll_x = 1;
static float cos_pitch_x = 1; static float cos_pitch_x = 1;
@ -343,12 +348,12 @@ static int airspeed; // m/s * 100
// Location Errors // Location Errors
// --------------- // ---------------
static long bearing_error; // deg * 100 : 0 to 36000 static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude static long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline static float crosstrack_error; // meters we are off trackline
static long distance_error; // distance to the WP static long distance_error; // distance to the WP
static long yaw_error; // how off are we pointed static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging static long long_error, lat_error; // temp for debugging
static int loiter_error_max; static int loiter_error_max;
// Battery Sensors // Battery Sensors
@ -361,6 +366,7 @@ static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cel
static float current_amps; static float current_amps;
static float current_total; static float current_total;
static bool low_batt = false;
// Airspeed Sensors // Airspeed Sensors
// ---------------- // ----------------
@ -369,9 +375,9 @@ static float current_total;
// -------------------------- // --------------------------
static long abs_pressure; static long abs_pressure;
static long ground_pressure; static long ground_pressure;
static int ground_temperature; static int ground_temperature;
static int32_t baro_filter[BARO_FILTER_SIZE]; static int32_t baro_filter[BARO_FILTER_SIZE];
static byte baro_filter_index; static byte baro_filter_index;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
@ -400,11 +406,11 @@ static unsigned long loiter_time_max; // millis : how long to stay in LOITER
// these are the values for navigation control functions // these are the values for navigation control functions
// ---------------------------------------------------- // ----------------------------------------------------
static long nav_roll; // deg * 100 : target roll angle static long nav_roll; // deg * 100 : target roll angle
static long nav_pitch; // deg * 100 : target pitch angle static long nav_pitch; // deg * 100 : target pitch angle
static long nav_yaw; // deg * 100 : target yaw angle static long nav_yaw; // deg * 100 : target yaw angle
static long nav_lat; // for error calcs static long nav_lat; // for error calcs
static long nav_lon; // for error calcs static long nav_lon; // for error calcs
static int nav_throttle; // 0-1000 for throttle control static int nav_throttle; // 0-1000 for throttle control
static long throttle_integrator; // used to control when we calculate nav_throttle static long throttle_integrator; // used to control when we calculate nav_throttle
@ -417,8 +423,8 @@ static unsigned int command_yaw_time; // how long we are turning
static long command_yaw_end; // what angle are we trying to be static long command_yaw_end; // what angle are we trying to be
static long command_yaw_delta; // how many degrees will we turn static long command_yaw_delta; // how many degrees will we turn
static int command_yaw_speed; // how fast to turn static int command_yaw_speed; // how fast to turn
static byte command_yaw_dir; static byte command_yaw_dir;
static byte command_yaw_relative; static byte command_yaw_relative;
static int auto_level_counter; static int auto_level_counter;
@ -433,9 +439,9 @@ static byte next_wp_index; // Current active command index
static byte event_id; // what to do - see defines static byte event_id; // what to do - see defines
static unsigned long event_timer; // when the event was asked for in ms static unsigned long event_timer; // when the event was asked for in ms
static unsigned int event_delay; // how long to delay the next firing of event in millis static unsigned int event_delay; // how long to delay the next firing of event in millis
static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int event_value; // per command value, such as PWM for servos static int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to undo commands static int event_undo_value; // the value used to undo commands
static byte repeat_forever; static byte repeat_forever;
static byte undo_event; // counter for timing the undo static byte undo_event; // counter for timing the undo
@ -443,7 +449,7 @@ static byte undo_event; // counter for timing the undo
// -------------- // --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.) static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start; static long condition_start;
static int condition_rate; static int condition_rate;
// land command // land command
// ------------ // ------------
@ -536,7 +542,6 @@ void loop()
//if (delta_ms_fast_loop > 6) //if (delta_ms_fast_loop > 6)
// Log_Write_Performance(); // Log_Write_Performance();
// Execute the fast loop // Execute the fast loop
// --------------------- // ---------------------
fast_loop(); fast_loop();
@ -594,7 +599,7 @@ static void fast_loop()
// Read radio // Read radio
// ---------- // ----------
read_radio(); // read the radio first read_radio();
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
// --------------------------------------- // ---------------------------------------
@ -603,10 +608,6 @@ static void fast_loop()
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
set_servos_4(); set_servos_4();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
} }
static void medium_loop() static void medium_loop()
@ -775,6 +776,10 @@ static void medium_loop()
// --------------------------- // ---------------------------
static void fifty_hz_loop() static void fifty_hz_loop()
{ {
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values // HIL for a copter needs very fast update of the servo values
hil.send_message(MSG_RADIO_OUT); hil.send_message(MSG_RADIO_OUT);
@ -796,9 +801,9 @@ static void fifty_hz_loop()
Log_Write_Raw(); Log_Write_Raw();
#endif #endif
#if CAMERA_STABILIZER == ENABLED //#if CAMERA_STABILIZER == ENABLED
camera_stabilization(); camera_stabilization();
#endif //#endif
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT
@ -824,7 +829,6 @@ static void fifty_hz_loop()
#endif #endif
#if FRAME_CONFIG == TRI_FRAME #if FRAME_CONFIG == TRI_FRAME
// Hack - had to move to 50hz loop to test a theory
// servo Yaw // servo Yaw
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
APM_RC.OutputCh(CH_7, g.rc_4.radio_out); APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
@ -862,9 +866,9 @@ static void slow_loop()
// Read main battery voltage if hooked up - does not read the 5v from radio // Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------ // ------------------------------------------------------------------------
#if BATTERY_EVENT == 1 //#if BATTERY_EVENT == 1
read_battery(); // read_battery();
#endif //#endif
#if AUTO_RESET_LOITER == 1 #if AUTO_RESET_LOITER == 1
if(control_mode == LOITER){ if(control_mode == LOITER){
@ -926,7 +930,7 @@ static void slow_loop()
static void super_slow_loop() static void super_slow_loop()
{ {
loop_step = 9; loop_step = 9;
if (g.log_bitmask & MASK_LOG_CURRENT) if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current(); Log_Write_Current();
gcs.send_message(MSG_HEARTBEAT); gcs.send_message(MSG_HEARTBEAT);
@ -934,24 +938,6 @@ static void super_slow_loop()
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT); hil.send_message(MSG_HEARTBEAT);
#endif #endif
//Serial.printf("r:%d p:%d\n",dcm.roll_sensor, dcm.pitch_sensor);
//if(gcs_simple.read()){
// Serial.print("!");
/*
Location temp;
temp.id = gcs_simple.id;
temp.p1 = gcs_simple.p1;
temp.alt = gcs_simple.altitude;
temp.lat = gcs_simple.latitude;
temp.lng = gcs_simple.longitude;
set_command_with_index(temp, gcs_simple.index);
gcs_simple.ack();
*/
//}
} }
static void update_GPS(void) static void update_GPS(void)
@ -1164,8 +1150,8 @@ void update_current_flight_mode(void)
case SIMPLE: case SIMPLE:
flight_timer++; flight_timer++;
// 25 hz
if(flight_timer > 4){ if(flight_timer > 6){
flight_timer = 0; flight_timer = 0;
// make sure this is always 0 // make sure this is always 0
@ -1174,8 +1160,6 @@ void update_current_flight_mode(void)
next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres
next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres
//next_WP.lng = g.rc_1.control_in; // X: 4500 * .7 = 2250 = 25 meteres
//next_WP.lat = -g.rc_2.control_in; // Y: 4500 * .7 = 2250 = 25 meteres
// calc a new bearing // calc a new bearing
nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing;
@ -1183,7 +1167,6 @@ void update_current_flight_mode(void)
wp_distance = get_distance(&simple_WP, &next_WP); wp_distance = get_distance(&simple_WP, &next_WP);
calc_bearing_error(); calc_bearing_error();
/* /*
Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
next_WP.lat, next_WP.lat,
@ -1261,6 +1244,7 @@ void update_current_flight_mode(void)
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
break; break;
case CIRCLE:
case GUIDED: case GUIDED:
case RTL: case RTL:
// mix in user control with Nav control // mix in user control with Nav control
@ -1279,7 +1263,7 @@ void update_current_flight_mode(void)
} }
// Yaw control // Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.5); g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.25);
break; break;
case LOITER: case LOITER:
@ -1334,7 +1318,6 @@ static void update_navigation()
break; break;
case GUIDED: case GUIDED:
case RTL:
if(wp_distance > 20){ if(wp_distance > 20){
// calculates desired Yaw // calculates desired Yaw
update_nav_yaw(); update_nav_yaw();
@ -1344,7 +1327,7 @@ static void update_navigation()
// hack to elmininate crosstrack effect // hack to elmininate crosstrack effect
crosstrack_bearing = target_bearing; crosstrack_bearing = target_bearing;
} }
case RTL:
// are we Traversing or Loitering? // are we Traversing or Loitering?
wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE; wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE;
@ -1361,6 +1344,26 @@ static void update_navigation()
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
break; break;
case CIRCLE:
yaw_tracking = MAV_ROI_WPNEXT;
// calculates desired Yaw
update_nav_yaw();
// hack to elmininate crosstrack effect
crosstrack_bearing = target_bearing;
// get a new nav_bearing
update_loiter();
// calc a rate dampened pitch to the target
calc_rate_nav(200);
// rotate that pitch to the copter frame of reference
calc_nav_output();
break;
} }
} }
@ -1414,28 +1417,12 @@ static void update_alt()
// read barometer // read barometer
baro_alt = read_barometer(); baro_alt = read_barometer();
int temp_sonar = sonar.read(); sonar_alt = sonar.read();
// spike filter if(baro_alt < 1000){
if((temp_sonar - sonar_alt) < 50){
sonar_alt = temp_sonar;
}
sonar_alt = temp_sonar;
/*
doesn't really seem to be a need for this using EZ0:
float temp = cos_pitch_x * cos_roll_x;
temp = max(temp, 0.707);
sonar_alt = (float)sonar_alt * temp;
*/
if(baro_alt < 1500){
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
}else{ }else{
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
} }
@ -1458,7 +1445,7 @@ adjust_altitude()
if(g.rc_3.control_in <= 200){ if(g.rc_3.control_in <= 200){
next_WP.alt -= 3; // 1 meter per second next_WP.alt -= 3; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 600)); // don't go more than 4 meters below current location next_WP.alt = max(next_WP.alt, (current_loc.alt - 900)); // don't go more than 4 meters below current location
}else if (g.rc_3.control_in > 700){ }else if (g.rc_3.control_in > 700){
next_WP.alt += 4; // 1 meter per second next_WP.alt += 4; // 1 meter per second
@ -1543,7 +1530,7 @@ static void update_nav_wp()
update_crosstrack(); update_crosstrack();
// calc a rate dampened pitch to the target // calc a rate dampened pitch to the target
calc_rate_nav(); calc_rate_nav(g.waypoint_speed_max.get());
// rotate that pitch to the copter frame of reference // rotate that pitch to the copter frame of reference
calc_nav_output(); calc_nav_output();
@ -1582,3 +1569,4 @@ static void auto_throttle()
//Serial.printf("wp:%d, \te:%d \tt%d \t%d\n", (int)next_WP.alt, (int)altitude_error, nav_throttle, g.rc_3.servo_out); //Serial.printf("wp:%d, \te:%d \tt%d \t%d\n", (int)next_WP.alt, (int)altitude_error, nav_throttle, g.rc_3.servo_out);
} }

2
ArduCopterMega/Attitude.pde
View File

@ -152,7 +152,9 @@ get_nav_yaw_offset(int yaw_input, int reset)
_yaw = (long)yaw_input + dcm.yaw_sensor; _yaw = (long)yaw_input + dcm.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100) // we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw); return wrap_360(_yaw);
}else{ }else{
// no stick input, lets not change nav_yaw
return nav_yaw; return nav_yaw;
} }
} }

9
ArduCopterMega/Camera.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 -*-
#if CAMERA_STABILIZER == ENABLED //#if CAMERA_STABILIZER == ENABLED
static void init_camera() static void init_camera()
{ {
@ -8,11 +8,15 @@ static void init_camera()
g.rc_camera_pitch.radio_min = 1000; g.rc_camera_pitch.radio_min = 1000;
g.rc_camera_pitch.radio_trim = 1500; g.rc_camera_pitch.radio_trim = 1500;
g.rc_camera_pitch.radio_max = 2000; g.rc_camera_pitch.radio_max = 2000;
//g.rc_camera_pitch.set_reverse(1);
g.rc_camera_roll.set_angle(4500); g.rc_camera_roll.set_angle(4500);
g.rc_camera_roll.radio_min = 1000; g.rc_camera_roll.radio_min = 1000;
g.rc_camera_roll.radio_trim = 1500; g.rc_camera_roll.radio_trim = 1500;
g.rc_camera_roll.radio_max = 2000; g.rc_camera_roll.radio_max = 2000;
g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
} }
static void static void
@ -50,6 +54,7 @@ camera_stabilization()
APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out);
APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out); APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out);
//Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out);
} }
#endif //#endif

19
ArduCopterMega/Log.pde
View File

@ -69,7 +69,7 @@ print_log_menu(void)
if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN")); if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN"));
if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW")); if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD")); if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
if (g.log_bitmask & MASK_LOG_CURRENT) Serial.printf_P(PSTR(" CURRENT")); if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT"));
if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS")); if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS"));
if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW"));
} }
@ -138,6 +138,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("\nErasing log...\n")); Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4096; j++) for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j); DataFlash.PageErase(j);
DataFlash.StartWrite(1); DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -169,6 +170,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
// //
if (!strcasecmp_P(argv[1].str, PSTR("all"))) { if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~0; bits = ~0;
bits = bits ^ MASK_LOG_SET_DEFAULTS;
} else { } else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST); TARG(ATTITUDE_FAST);
@ -180,7 +182,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(MODE); TARG(MODE);
TARG(RAW); TARG(RAW);
TARG(CMD); TARG(CMD);
TARG(CURRENT); TARG(CUR);
TARG(MOTORS); TARG(MOTORS);
TARG(OPTFLOW); TARG(OPTFLOW);
#undef TARG #undef TARG
@ -188,7 +190,6 @@ select_logs(uint8_t argc, const Menu::arg *argv)
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
g.log_bitmask.set_and_save(g.log_bitmask | bits); g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{ }else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits); g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
} }
@ -251,8 +252,8 @@ static void start_new_log()
{ {
byte num_existing_logs = get_num_logs(); byte num_existing_logs = get_num_logs();
int start_pages[50]; int start_pages[50] = {0,0,0};
int end_pages[50]; int end_pages[50] = {0,0,0};
if(num_existing_logs > 0){ if(num_existing_logs > 0){
for(int i = 0; i < num_existing_logs; i++) { for(int i = 0; i < num_existing_logs; i++) {
@ -286,7 +287,7 @@ static void start_new_log()
DataFlash.StartWrite(start_pages[num_existing_logs - 1]); DataFlash.StartWrite(start_pages[num_existing_logs - 1]);
}else{ }else{
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full")); gcs.send_text_P(SEVERITY_LOW,PSTR("Logs full"));
} }
} }
@ -576,7 +577,7 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw // yaw
DataFlash.WriteByte(yaw_debug); //DataFlash.WriteByte(yaw_debug);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error/100); DataFlash.WriteInt((int)yaw_error/100);
@ -599,7 +600,7 @@ static void Log_Write_Control_Tuning()
static void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()
{ {
Serial.printf_P(PSTR( "CTUN, %d, %d, " Serial.printf_P(PSTR( "CTUN, %d, %d, "
"%d, %d, %d, %d, %1.4f, " "%d, %d, %d, %1.4f, "
"%d, %d, %d, %d, %d, %d\n"), "%d, %d, %d, %d, %d, %d\n"),
// Control // Control
@ -607,7 +608,7 @@ static void Log_Read_Control_Tuning()
DataFlash.ReadInt(), DataFlash.ReadInt(),
// yaw // yaw
(int)DataFlash.ReadByte(), //(int)DataFlash.ReadByte(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),

6
ArduCopterMega/Mavlink_Common.h
View File

@ -101,7 +101,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
chan, chan,
current_loc.lat, current_loc.lat,
current_loc.lng, current_loc.lng,
current_loc.alt * 10, /*current_loc.alt * 10,*/ // changed to absolute altitude
g_gps->altitude,
g_gps->ground_speed * rot.a.x, g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x, g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x); g_gps->ground_speed * rot.c.x);
@ -215,7 +216,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
(float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360, (dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10, g.rc_3.servo_out/10,
current_loc.alt / 100.0, /*current_loc.alt / 100.0,*/ // changed to absolute altitude
g_gps->altitude/100.0,
climb_rate); climb_rate);
break; break;
} }

2
ArduCopterMega/Parameters.h
View File

@ -316,7 +316,7 @@ public:
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),

4
ArduCopterMega/commands_logic.pde
View File

@ -319,7 +319,7 @@ static void do_loiter_time()
set_next_WP(&current_loc); set_next_WP(&current_loc);
loiter_time = millis(); loiter_time = millis();
loiter_time_max = next_command.p1 * 1000; // units are (seconds) loiter_time_max = next_command.p1 * 1000; // units are (seconds)
Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max); //Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
//*/ //*/
} }
@ -335,7 +335,7 @@ static bool verify_takeoff()
return false; return false;
} }
Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt); //Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
if (current_loc.alt > next_WP.alt){ if (current_loc.alt > next_WP.alt){
//Serial.println("Y"); //Serial.println("Y");

10
ArduCopterMega/commands_process.pde
View File

@ -28,7 +28,7 @@ static void update_commands(void)
if (g.waypoint_index < g.waypoint_total) { if (g.waypoint_index < g.waypoint_total) {
// only if we have a cmd stored in EEPROM // only if we have a cmd stored in EEPROM
next_command = get_command_with_index(g.waypoint_index + 1); next_command = get_command_with_index(g.waypoint_index + 1);
Serial.printf("queue CMD %d\n", next_command.id); //Serial.printf("queue CMD %d\n", next_command.id);
} }
} }
@ -45,7 +45,7 @@ static void update_commands(void)
// check to see if we need to act on our command queue // check to see if we need to act on our command queue
if (process_next_command()){ if (process_next_command()){
Serial.printf("did PNC: %d\n", next_command.id); //Serial.printf("did PNC: %d\n", next_command.id);
// We acted on the queue - let's debug that // We acted on the queue - let's debug that
// ---------------------------------------- // ----------------------------------------
@ -65,17 +65,17 @@ static void update_commands(void)
static void verify_commands(void) static void verify_commands(void)
{ {
if(verify_must()){ if(verify_must()){
Serial.printf("verified must cmd %d\n" , command_must_index); //Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
// reset rate controlled nav // reset rate controlled nav
g.pid_nav_wp.reset_I(); g.pid_nav_wp.reset_I();
}else{ }else{
Serial.printf("verified must false %d\n" , command_must_index); //Serial.printf("verified must false %d\n" , command_must_index);
} }
if(verify_may()){ if(verify_may()){
Serial.printf("verified may cmd %d\n" , command_may_index); //Serial.printf("verified may cmd %d\n" , command_may_index);
command_may_index = NO_COMMAND; command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND; command_may_ID = NO_COMMAND;
} }

66
ArduCopterMega/config.h
View File

@ -202,7 +202,7 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW // OPTICAL_FLOW
#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included
#define OPTFLOW_ENABLED //#define OPTFLOW_ENABLED
#endif #endif
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
@ -299,13 +299,13 @@
// Roll Control // Roll Control
// //
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5 # define STABILIZE_ROLL_P 4.2
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025 # define STABILIZE_ROLL_I 0.025
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX .5 // degrees # define STABILIZE_ROLL_IMAX 15 // degrees
#endif #endif
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
@ -315,7 +315,7 @@
# define RATE_ROLL_I 0.0 # define RATE_ROLL_I 0.0
#endif #endif
#ifndef RATE_ROLL_IMAX #ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 15 // degrees # define RATE_ROLL_IMAX 5 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -328,7 +328,7 @@
# define STABILIZE_PITCH_I 0.025 # define STABILIZE_PITCH_I 0.025
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX .5 // degrees # define STABILIZE_PITCH_IMAX 15 // degrees
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
@ -338,7 +338,7 @@
# define RATE_PITCH_I 0.0 # define RATE_PITCH_I 0.0
#endif #endif
#ifndef RATE_PITCH_IMAX #ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 15 // degrees # define RATE_PITCH_IMAX 5 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -378,31 +378,31 @@
// Navigation control gains // Navigation control gains
// //
#ifndef NAV_LOITER_P #ifndef NAV_LOITER_P
# define NAV_LOITER_P 1.3 // # define NAV_LOITER_P 1.4 //
#endif #endif
#ifndef NAV_LOITER_I #ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.01 // # define NAV_LOITER_I 0.015 //
#endif #endif
#ifndef NAV_LOITER_D #ifndef NAV_LOITER_D
# define NAV_LOITER_D 0.4 // # define NAV_LOITER_D 1.4 //
#endif #endif
#ifndef NAV_LOITER_IMAX #ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 10 // degrees° # define NAV_LOITER_IMAX 12 // degrees°
#endif #endif
#ifndef NAV_WP_P #ifndef NAV_WP_P
# define NAV_WP_P 3.0 // for 4.5 ms error = 13.5 pitch # define NAV_WP_P 2.2 // for 4.5 ms error = 13.5 pitch
#endif #endif
#ifndef NAV_WP_I #ifndef NAV_WP_I
# define NAV_WP_I 0.5 // this is a fast ramp up # define NAV_WP_I 0.06 // this
#endif #endif
#ifndef NAV_WP_D #ifndef NAV_WP_D
# define NAV_WP_D .3 // # define NAV_WP_D .5 //
#endif #endif
#ifndef NAV_WP_IMAX #ifndef NAV_WP_IMAX
# define NAV_WP_IMAX 40 // degrees # define NAV_WP_IMAX 20 // degrees
#endif #endif
@ -414,13 +414,13 @@
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.3 // trying a lower val # define THROTTLE_P 0.35 // trying a lower val
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.04 //with 4m error, 12.5s windup # define THROTTLE_I 0.01 //with 4m error, 12.5s windup
#endif #endif
#ifndef THROTTLE_D #ifndef THROTTLE_D
# define THROTTLE_D 0.35 // upped with filter # define THROTTLE_D 0.4 // upped with filter
#endif #endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 30 # define THROTTLE_IMAX 30
@ -503,24 +503,18 @@
#ifndef LOG_CMD #ifndef LOG_CMD
# define LOG_CMD ENABLED # define LOG_CMD ENABLED
#endif #endif
#ifndef LOG_CURRENT // current
# define LOG_CURRENT DISABLED #ifndef LOG_CUR
# define LOG_CUR DISABLED
#endif
// quad motor PWMs
#ifndef LOG_MOTORS
# define LOG_MOTORS DISABLED
#endif
// guess!
#ifndef LOG_OPTFLOW
# define LOG_OPTFLOW DISABLED
#endif #endif
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CURRENT)
// if we are using fast, Disable Medium // if we are using fast, Disable Medium
//#if LOG_ATTITUDE_FAST == ENABLED //#if LOG_ATTITUDE_FAST == ENABLED
@ -581,10 +575,6 @@
# define CUT_MOTORS 1 // do we cut the motors with no throttle? # define CUT_MOTORS 1 // do we cut the motors with no throttle?
#endif #endif
#ifndef CAMERA_STABILIZER
# define CAMERA_STABILIZER 1 // do we cut the motors with no throttle?
#endif
#ifndef BROKEN_SLIDER #ifndef BROKEN_SLIDER
# define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) # define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)
#endif #endif

6
ArduCopterMega/defines.h
View File

@ -119,7 +119,8 @@
#define GUIDED 5 // AUTO control #define GUIDED 5 // AUTO control
#define LOITER 6 // Hold a single location #define LOITER 6 // Hold a single location
#define RTL 7 // AUTO control #define RTL 7 // AUTO control
#define NUM_MODES 8 #define CIRCLE 8 // AUTO control
#define NUM_MODES 9
// YAW debug // YAW debug
// --------- // ---------
@ -275,9 +276,10 @@
#define MASK_LOG_MODE (1<<6) #define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7) #define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8) #define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9) #define MASK_LOG_CUR (1<<9)
#define MASK_LOG_MOTORS (1<<10) #define MASK_LOG_MOTORS (1<<10)
#define MASK_LOG_OPTFLOW (1<<11) #define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_SET_DEFAULTS (1<<15)
// Waypoint Modes // Waypoint Modes
// ---------------- // ----------------

6
ArduCopterMega/events.pde
View File

@ -49,7 +49,11 @@ static void failsafe_off_event()
static void low_battery_event(void) static void low_battery_event(void)
{ {
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL); low_batt = true;
// if we are in Auto mode, come home
if(control_mode >= AUTO)
set_mode(RTL);
} }

11
ArduCopterMega/leds.pde
View File

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

26
ArduCopterMega/motors.pde
View File

@ -17,16 +17,21 @@ static void arm_motors()
// full right // full right
if (g.rc_4.control_in > 4000) { if (g.rc_4.control_in > 4000) {
// don't allow arming in anything but manual
if (control_mode < ALT_HOLD){ if (control_mode < ALT_HOLD){
if (arming_counter > AUTO_LEVEL_DELAY){ if (arming_counter > AUTO_LEVEL_DELAY){
auto_level_counter = 255; auto_level_counter = 155;
arming_counter = 0; arming_counter = 0;
}else if (arming_counter == ARM_DELAY){ }else if (arming_counter == ARM_DELAY){
motor_armed = true; motor_armed = true;
arming_counter = ARM_DELAY; arming_counter = ARM_DELAY;
// Clear throttle slew
// -------------------
throttle_slew = 0;
// Remember Orientation // Remember Orientation
// -------------------- // --------------------
init_simple_bearing(); init_simple_bearing();
@ -36,10 +41,15 @@ static void arm_motors()
if(home_is_set) if(home_is_set)
init_home(); init_home();
if(did_ground_start == false){
did_ground_start = true;
startup_ground();
}
// tune down compass // tune down compass
// ----------------- // -----------------
//dcm.kp_yaw(0.02); dcm.kp_yaw(0.08);
//dcm.ki_yaw(0); dcm.ki_yaw(0);
arming_counter++; arming_counter++;
} else{ } else{
@ -59,6 +69,16 @@ static void arm_motors()
motor_armed = false; motor_armed = false;
arming_counter = DISARM_DELAY; arming_counter = DISARM_DELAY;
compass.save_offsets(); compass.save_offsets();
// tune up compass
// -----------------
dcm.kp_yaw(0.8);
dcm.ki_yaw(0.00004);
// Clear throttle slew
// -------------------
throttle_slew = 0;
arming_counter++; arming_counter++;
}else{ }else{

50
ArduCopterMega/navigation.pde
View File

@ -49,7 +49,7 @@ get_nav_throttle(long error)
int throttle; int throttle;
// limit error to 4 meters to prevent I term run up // limit error to 4 meters to prevent I term run up
error = constrain(error, -400,400); error = constrain(error, -800,800);
throttle = g.pid_throttle.get_pid(error, delta_ms_medium_loop, 1.0); throttle = g.pid_throttle.get_pid(error, delta_ms_medium_loop, 1.0);
throttle = g.throttle_cruise + constrain(throttle, -80, 80); throttle = g.throttle_cruise + constrain(throttle, -80, 80);
@ -138,28 +138,31 @@ static void calc_nav_output()
} }
// called after we get GPS read // called after we get GPS read
static void calc_rate_nav() static void calc_rate_nav(int speed)
{ {
// which direction are we moving? // which direction are we moving?
long target_error = target_bearing - g_gps->ground_course; long target_error = nav_bearing - g_gps->ground_course;
target_error = wrap_180(target_error); target_error = wrap_180(target_error);
// calc the cos of the error to tell how fast we are moving towards the target in cm // calc the cos of the error to tell how fast we are moving towards the target in cm
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100)); int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
// Reduce speed on RTL // Reduce speed on RTL
//if(control_mode == RTL){ if(control_mode == RTL){
int tmp = min(wp_distance, 50) * 100; int tmp = min(wp_distance, 80) * 50;
waypoint_speed = min(tmp, g.waypoint_speed_max.get()); waypoint_speed = min(tmp, speed);
waypoint_speed = max(waypoint_speed, 80); waypoint_speed = max(waypoint_speed, 50);
//}else{ }else{
// waypoint_speed = g.waypoint_speed_max.get(); int tmp = min(wp_distance, 200) * 90;
//} waypoint_speed = min(tmp, speed);
waypoint_speed = max(waypoint_speed, 50);
//waypoint_speed = g.waypoint_speed_max.get();
}
int error = constrain(waypoint_speed - groundspeed, -1000, 1000); int error = constrain(waypoint_speed - groundspeed, -1000, 1000);
// Scale response by kP // Scale response by kP
nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0); nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0);
nav_lat >>= 1; // divide by two nav_lat >>= 1; // divide by two for smooting
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
@ -169,6 +172,7 @@ static void calc_rate_nav()
static void calc_bearing_error() static void calc_bearing_error()
{ {
// 83 99 Yaw = -16
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }
@ -193,6 +197,28 @@ static void calc_altitude_smoothing_error()
altitude_error = target_altitude - current_loc.alt; altitude_error = target_altitude - current_loc.alt;
} }
static void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
nav_bearing = wrap_360(nav_bearing);
}
static long wrap_360(long error) static long wrap_360(long error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;

4
ArduCopterMega/radio.pde
View File

@ -78,6 +78,10 @@ static void init_rc_out()
OCR4B = 0xFFFF; // PH4, OUT6 OCR4B = 0xFFFF; // PH4, OUT6
OCR4C = 0xFFFF; // PH5, OUT5 OCR4C = 0xFFFF; // PH5, OUT5
// this is the camera pitch5 and roll6
APM_RC.OutputCh(CH_5, 1500);
APM_RC.OutputCh(CH_6, 1500);
// don't fuss if we are calibrating // don't fuss if we are calibrating
if(g.esc_calibrate == 1) if(g.esc_calibrate == 1)
return; return;

23
ArduCopterMega/sensors.pde
View File

@ -11,10 +11,11 @@ static void init_barometer(void)
hil.update(); // look for inbound hil packets for initialization hil.update(); // look for inbound hil packets for initialization
#endif #endif
ground_temperature = barometer.Temp;
// We take some readings... // We take some readings...
for(int i = 0; i < 200; i++){ for(int i = 0; i < 20; i++){
delay(25); delay(20);
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets hil.update(); // look for inbound hil packets
@ -23,12 +24,11 @@ static void init_barometer(void)
// Get initial data from absolute pressure sensor // Get initial data from absolute pressure sensor
ground_pressure = read_baro_filtered(); ground_pressure = read_baro_filtered();
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
} }
abs_pressure = ground_pressure;
ground_temperature = barometer.Temp;
//Serial.printf("abs_pressure %ld\n", ground_temperature); abs_pressure = ground_pressure;
//Serial.printf("init %ld\n", abs_pressure);
//SendDebugln("barometer calibration complete."); //SendDebugln("barometer calibration complete.");
} }
@ -39,12 +39,13 @@ static long read_baro_filtered(void)
// get new data from absolute pressure sensor // get new data from absolute pressure sensor
barometer.Read(); barometer.Read();
// add new data into our filter // add new data into our filter
baro_filter[baro_filter_index] = barometer.Press; baro_filter[baro_filter_index] = barometer.Press;
baro_filter_index++; baro_filter_index++;
// loop our filter // loop our filter
if(baro_filter_index == BARO_FILTER_SIZE) if(baro_filter_index >= BARO_FILTER_SIZE)
baro_filter_index = 0; baro_filter_index = 0;
// zero out our accumulator // zero out our accumulator
@ -54,6 +55,7 @@ static long read_baro_filtered(void)
pressure += baro_filter[i]; pressure += baro_filter[i];
} }
// average our sampels // average our sampels
return pressure /= BARO_FILTER_SIZE; return pressure /= BARO_FILTER_SIZE;
} }
@ -104,8 +106,11 @@ static void read_battery(void)
} }
#if BATTERY_EVENT == 1 #if BATTERY_EVENT == 1
if(battery_voltage < LOW_VOLTAGE) low_battery_event(); if(battery_voltage < LOW_VOLTAGE)
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)
low_battery_event();
#endif #endif
} }

33
ArduCopterMega/setup.pde
View File

@ -229,7 +229,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_esc(uint8_t argc, const Menu::arg *argv) setup_esc(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("\nESC Calibration:\n-1 Unplug USB and battery\n-2 Move CLI/FLY switch to FLY mode\n-3 Move throttle to max, connect battery\n-4 After two long beeps, throttle to 0, then test\n\n Press Enter to cancel.\n")); Serial.printf_P(PSTR("\nESC Calibration:\n"
"-1 Unplug USB and battery\n"
"-2 Move CLI/FLY switch to FLY mode\n"
"-3 Move throttle to max, connect battery\n"
"-4 After two long beeps, throttle to 0, then test\n\n"
" Press Enter to cancel.\n"));
g.esc_calibrate.set_and_save(1); g.esc_calibrate.set_and_save(1);
@ -740,6 +745,32 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
} }
#endif #endif
void
default_log_bitmask()
{
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
g.log_bitmask =
LOGBIT(ATTITUDE_FAST) |
LOGBIT(ATTITUDE_MED) |
LOGBIT(GPS) |
LOGBIT(PM) |
LOGBIT(CTUN) |
LOGBIT(NTUN) |
LOGBIT(MODE) |
LOGBIT(RAW) |
LOGBIT(CMD) |
LOGBIT(CUR) |
LOGBIT(MOTORS) |
LOGBIT(OPTFLOW);
#undef LOGBIT
g.log_bitmask.save();
}
/***************************************************************************/ /***************************************************************************/
// CLI reports // CLI reports
/***************************************************************************/ /***************************************************************************/

164
ArduCopterMega/system.pde
View File

@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "AC 2.0.37 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.38 Beta", main_menu_commands);
#endif // CLI_ENABLED #endif // CLI_ENABLED
@ -146,14 +146,13 @@ static void init_ardupilot()
} }
}else{ }else{
// unsigned long before = micros();
// Load all auto-loaded EEPROM variables // Load all auto-loaded EEPROM variables
AP_Var::load_all(); AP_Var::load_all();
// Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
// Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
} }
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
#ifdef RADIO_OVERRIDE_DEFAULTS #ifdef RADIO_OVERRIDE_DEFAULTS
{ {
@ -168,10 +167,7 @@ static void init_ardupilot()
init_rc_in(); // sets up rc channels from radio init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs init_rc_out(); // sets up the timer libs
init_camera();
#if CAMERA_STABILIZER == ENABLED
init_camera();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization adc.Init(); // APM ADC library initialization
@ -179,7 +175,6 @@ static void init_ardupilot()
#endif #endif
// Do GPS init // Do GPS init
//g_gps = &GPS;
g_gps = &g_gps_driver; g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization g_gps->init(); // GPS Initialization
@ -209,12 +204,6 @@ static void init_ardupilot()
if(g.compass_enabled) if(g.compass_enabled)
init_compass(); init_compass();
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){
sonar.init(SONAR_PORT, &adc);
}
#endif
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
// init the optical flow sensor // init the optical flow sensor
if(g.optflow_enabled) { if(g.optflow_enabled) {
@ -260,52 +249,10 @@ static void init_ardupilot()
start_new_log(); start_new_log();
} }
// All of the Gyro calibrations //#if(GROUND_START_DELAY > 0)
// ----------------------------
startup_ground();
// set the correct flight mode
// ---------------------------
reset_control_switch();
delay(100);
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
#if(GROUND_START_DELAY > 0)
//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
delay(GROUND_START_DELAY * 1000); // delay(GROUND_START_DELAY * 1000);
#endif //#endif
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
report_imu();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
// initialize commands
// -------------------
init_commands();
GPS_enabled = false; GPS_enabled = false;
@ -325,33 +272,77 @@ static void startup_ground(void)
} }
//*/ //*/
// setup DCM for copters: // lengthen the idle timeout for gps Auto_detect
#if HIL_MODE != HIL_MODE_ATTITUDE // ---------------------------------------------
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate
#endif
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
clear_leds();
// print the GPS status
report_gps();
// lenthen the idle timeout for gps Auto_detect
g_gps->idleTimeout = 20000; g_gps->idleTimeout = 20000;
// used to limit the input of error for loiter // print the GPS status
loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP(); // --------------------
//Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max); report_gps();
// used to limit the input of error for loiter
// -------------------------------------------
loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP();
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
// initialize commands
// -------------------
init_commands();
// Output waypoints for confirmation
// XXX do we need this?
// --------------------------------
//for(int i = 1; i < g.waypoint_total + 1; i++) {
// gcs.send_message(MSG_COMMAND_LIST, i);
//}
// set the correct flight mode
// ---------------------------
reset_control_switch();
//delay(100);
//Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max);
Log_Write_Startup(); Log_Write_Startup();
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
//gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); }
// output control mode to the ground station //********************************************************************************
gcs.send_message(MSG_HEARTBEAT); //This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
report_imu();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// setup DCM for copters:
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// reset the leds
// ---------------------------
clear_leds();
} }
static void set_mode(byte mode) static void set_mode(byte mode)
@ -360,6 +351,14 @@ static void set_mode(byte mode)
// don't switch modes if we are already in the correct mode. // don't switch modes if we are already in the correct mode.
return; return;
} }
// XXX
Serial.printf_P(PSTR("\nRAM: %lu\n"), freeRAM());
// reset the Nav_WP I term
g.pid_nav_wp.reset_I();
old_control_mode = control_mode; old_control_mode = control_mode;
control_mode = mode; control_mode = mode;
@ -381,6 +380,7 @@ static void set_mode(byte mode)
switch(control_mode) switch(control_mode)
{ {
case ACRO: case ACRO:
g.pid_throttle.reset_I();
break; break;
case SIMPLE: case SIMPLE:
@ -399,6 +399,7 @@ static void set_mode(byte mode)
init_auto(); init_auto();
break; break;
case CIRCLE:
case LOITER: case LOITER:
init_throttle_cruise(); init_throttle_cruise();
do_loiter_at_location(); do_loiter_at_location();
@ -502,6 +503,7 @@ init_throttle_cruise()
{ {
// are we moving from manual throttle to auto_throttle? // are we moving from manual throttle to auto_throttle?
if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){ if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){
g.pid_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in); g.throttle_cruise.set_and_save(g.rc_3.control_in);
} }
} }