From 73be185414043c083486f11184f2e61dc9044b19 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 30 Jul 2011 20:42:54 +0000 Subject: [PATCH] Big update 2.0.38 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 --- ArduCopterMega/APM_Config.h | 14 ++- ArduCopterMega/ArduCopterMega.pde | 182 +++++++++++++--------------- ArduCopterMega/Attitude.pde | 2 + ArduCopterMega/Camera.pde | 9 +- ArduCopterMega/Log.pde | 19 +-- ArduCopterMega/Mavlink_Common.h | 6 +- ArduCopterMega/Parameters.h | 2 +- ArduCopterMega/commands_logic.pde | 4 +- ArduCopterMega/commands_process.pde | 10 +- ArduCopterMega/config.h | 66 +++++----- ArduCopterMega/defines.h | 6 +- ArduCopterMega/events.pde | 6 +- ArduCopterMega/leds.pde | 11 +- ArduCopterMega/motors.pde | 26 +++- ArduCopterMega/navigation.pde | 50 ++++++-- ArduCopterMega/radio.pde | 4 + ArduCopterMega/sensors.pde | 23 ++-- ArduCopterMega/setup.pde | 33 ++++- ArduCopterMega/system.pde | 166 ++++++++++++------------- 19 files changed, 363 insertions(+), 276 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index f9ebcc110c..213945e823 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -4,10 +4,12 @@ //#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: QUAD_FRAME @@ -18,15 +20,15 @@ HELI_FRAME */ -//#define FRAME_ORIENTATION X_FRAME +#define FRAME_ORIENTATION X_FRAME /* PLUS_FRAME X_FRAME - V_FRAME + V_FRAME */ -//#define CHANNEL_6_TUNING CH6_NONE +#define CHANNEL_6_TUNING CH6_NONE /* CH6_NONE CH6_STABILIZE_KP @@ -47,4 +49,4 @@ // experimental!! // Yaw is controled by targeting home. you will not have Yaw override. // flying too close to home may induce spins. -#define SIMPLE_LOOK_AT_HOME 0 +//#define SIMPLE_LOOK_AT_HOME 0 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 1343c96fe1..a29d8e50c0 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -62,6 +62,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // RC Channel Library #include // Range finder library #include // Optical Flow library +#include #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions @@ -226,12 +227,14 @@ static AP_Int8 *flight_modes = &g.flight_mode1; // SONAR selection //////////////////////////////////////////////////////////////////////////////// // +ModeFilter sonar_mode_filter; + #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 // XXX honestly I think these output the same values // 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 //////////////////////////////////////////////////////////////////////////////// @@ -247,7 +250,8 @@ static const char* flight_mode_strings[] = { "AUTO", "GUIDED", "LOITER", - "RTL"}; + "RTL", + "CIRCLE"}; /* Radio values 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 scaleLongDown = 1; // used to reverse longtitude scaling 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 // --------------------- -static const float radius_of_earth = 6378100; // meters -static const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -static long 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 const float radius_of_earth = 6378100; // meters +static const float gravity = 9.81; // meters/ sec^2 +static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +static long 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 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 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 @@ -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_must_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_pitch_x = 1; @@ -343,12 +348,12 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 -static long altitude_error; // meters * 100 we are off in altitude +static long bearing_error; // deg * 100 : 0 to 36000 +static long altitude_error; // meters * 100 we are off in altitude static float crosstrack_error; // meters we are off trackline static long distance_error; // distance to the WP 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; // 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_total; +static bool low_batt = false; // Airspeed Sensors // ---------------- @@ -369,9 +375,9 @@ static float current_total; // -------------------------- static long abs_pressure; static long ground_pressure; -static int ground_temperature; -static int32_t baro_filter[BARO_FILTER_SIZE]; -static byte baro_filter_index; +static int ground_temperature; +static int32_t baro_filter[BARO_FILTER_SIZE]; +static byte baro_filter_index; // 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 // ---------------------------------------------------- -static long nav_roll; // deg * 100 : target roll angle -static long nav_pitch; // deg * 100 : target pitch angle -static long nav_yaw; // deg * 100 : target yaw angle -static long nav_lat; // for error calcs -static long nav_lon; // for error calcs +static long nav_roll; // deg * 100 : target roll angle +static long nav_pitch; // deg * 100 : target pitch angle +static long nav_yaw; // deg * 100 : target yaw angle +static long nav_lat; // for error calcs +static long nav_lon; // for error calcs static int nav_throttle; // 0-1000 for throttle control 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_delta; // how many degrees will we turn static int command_yaw_speed; // how fast to turn -static byte command_yaw_dir; -static byte command_yaw_relative; +static byte command_yaw_dir; +static byte command_yaw_relative; 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 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 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_undo_value; // the value used to undo commands +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_undo_value; // the value used to undo commands static byte repeat_forever; 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_start; -static int condition_rate; +static int condition_rate; // land command // ------------ @@ -463,7 +469,7 @@ static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location static struct Location optflow_offset; // optical flow base position -static boolean new_location; // flag to tell us if location has been updated +static boolean new_location; // flag to tell us if location has been updated // IMU variables @@ -536,7 +542,6 @@ void loop() //if (delta_ms_fast_loop > 6) // Log_Write_Performance(); - // Execute the fast loop // --------------------- fast_loop(); @@ -594,7 +599,7 @@ static void fast_loop() // Read radio // ---------- - read_radio(); // read the radio first + read_radio(); // custom code/exceptions for flight modes // --------------------------------------- @@ -603,10 +608,6 @@ static void fast_loop() // write out the servo PWM values // ------------------------------ set_servos_4(); - - // record throttle output - // ------------------------------ - throttle_integrator += g.rc_3.servo_out; } static void medium_loop() @@ -775,6 +776,10 @@ static void medium_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 // HIL for a copter needs very fast update of the servo values hil.send_message(MSG_RADIO_OUT); @@ -796,9 +801,9 @@ static void fifty_hz_loop() Log_Write_Raw(); #endif - #if CAMERA_STABILIZER == ENABLED + //#if CAMERA_STABILIZER == ENABLED camera_stabilization(); - #endif + //#endif #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT @@ -824,7 +829,6 @@ static void fifty_hz_loop() #endif #if FRAME_CONFIG == TRI_FRAME - // Hack - had to move to 50hz loop to test a theory // servo Yaw g.rc_4.calc_pwm(); 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 // ------------------------------------------------------------------------ - #if BATTERY_EVENT == 1 - read_battery(); - #endif + //#if BATTERY_EVENT == 1 + // read_battery(); + //#endif #if AUTO_RESET_LOITER == 1 if(control_mode == LOITER){ @@ -926,7 +930,7 @@ static void slow_loop() static void super_slow_loop() { loop_step = 9; - if (g.log_bitmask & MASK_LOG_CURRENT) + if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); 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) hil.send_message(MSG_HEARTBEAT); #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) @@ -1024,19 +1010,19 @@ void update_location(void) if( g.optflow_enabled ) { int32_t temp_lat, temp_lng, diff_lat, diff_lng; - + // get optical flow position optflow.read(); optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt); - + // write to log if (g.log_bitmask & MASK_LOG_OPTFLOW){ Log_Write_Optflow(); } - + temp_lat = optflow_offset.lat + optflow.lat; temp_lng = optflow_offset.lng + optflow.lng; - + // if we have good GPS values, don't let optical flow position stray too far if( GPS_enabled && g_gps->fix ) { // ensure current location is within 3m of gps location @@ -1059,11 +1045,11 @@ void update_location(void) //Serial.println("lng dec!"); } } - + // update the current position current_loc.lat = optflow_offset.lat + optflow.lat; current_loc.lng = optflow_offset.lng + optflow.lng; - + /*count++; if( count >= 20 ) { count = 0; @@ -1078,7 +1064,7 @@ void update_location(void) Serial.print(nav_pitch); Serial.println(); }*/ - + // indicate we have a new position for nav functions new_location = true; @@ -1086,7 +1072,7 @@ void update_location(void) // get current position from gps current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lat = g_gps->latitude; // Lat * 10 * *7 - + new_location = g_gps->new_data; } } @@ -1164,8 +1150,8 @@ void update_current_flight_mode(void) case SIMPLE: flight_timer++; - // 25 hz - if(flight_timer > 4){ + + if(flight_timer > 6){ flight_timer = 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.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 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); calc_bearing_error(); - /* Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", 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); break; + case CIRCLE: case GUIDED: case RTL: // mix in user control with Nav control @@ -1279,7 +1263,7 @@ void update_current_flight_mode(void) } // 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; case LOITER: @@ -1334,7 +1318,6 @@ static void update_navigation() break; case GUIDED: - case RTL: if(wp_distance > 20){ // calculates desired Yaw update_nav_yaw(); @@ -1344,7 +1327,7 @@ static void update_navigation() // hack to elmininate crosstrack effect crosstrack_bearing = target_bearing; } - + case RTL: // are we Traversing or Loitering? wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE; @@ -1361,6 +1344,26 @@ static void update_navigation() // calculates the desired Roll and Pitch update_nav_wp(); 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 baro_alt = read_barometer(); - int temp_sonar = sonar.read(); + sonar_alt = sonar.read(); - // spike filter - 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){ + if(baro_alt < 1000){ scale = (sonar_alt - 400) / 200; scale = constrain(scale, 0, 1); - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; - }else{ current_loc.alt = baro_alt + home.alt; } @@ -1458,7 +1445,7 @@ adjust_altitude() if(g.rc_3.control_in <= 200){ 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){ next_WP.alt += 4; // 1 meter per second @@ -1519,7 +1506,7 @@ static void tuning(){ // Simple relay control #elif CHANNEL_6_TUNING == CH6_RELAY - if(g.rc_6.control_in <= 600) relay_on(); + if(g.rc_6.control_in <= 600) relay_on(); if(g.rc_6.control_in >= 400) relay_off(); #endif @@ -1543,7 +1530,7 @@ static void update_nav_wp() update_crosstrack(); // 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 calc_nav_output(); @@ -1581,4 +1568,5 @@ static void auto_throttle() invalid_throttle = false; //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); -} +} + diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index d59cde04b9..82a53ecf86 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -152,7 +152,9 @@ get_nav_yaw_offset(int yaw_input, int reset) _yaw = (long)yaw_input + dcm.yaw_sensor; // we need to wrap our value so we can be 0 to 360 (*100) return wrap_360(_yaw); + }else{ + // no stick input, lets not change nav_yaw return nav_yaw; } } diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index b564d63540..fa49768cba 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -1,6 +1,6 @@ /// -*- 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() { @@ -8,11 +8,15 @@ static void init_camera() g.rc_camera_pitch.radio_min = 1000; g.rc_camera_pitch.radio_trim = 1500; 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.radio_min = 1000; g.rc_camera_roll.radio_trim = 1500; g.rc_camera_roll.radio_max = 2000; + + g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW); + g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW); } static void @@ -50,6 +54,7 @@ camera_stabilization() APM_RC.OutputCh(CH_5, g.rc_camera_pitch.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 diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 4f9b3131a3..46dc70133b 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -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_RAW) Serial.printf_P(PSTR(" RAW")); 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_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")); for(int j = 1; j < 4096; j++) DataFlash.PageErase(j); + DataFlash.StartWrite(1); DataFlash.WriteByte(HEAD_BYTE1); 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"))) { bits = ~0; + bits = bits ^ MASK_LOG_SET_DEFAULTS; } else { #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s TARG(ATTITUDE_FAST); @@ -180,7 +182,7 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(MODE); TARG(RAW); TARG(CMD); - TARG(CURRENT); + TARG(CUR); TARG(MOTORS); TARG(OPTFLOW); #undef TARG @@ -188,7 +190,6 @@ select_logs(uint8_t argc, const Menu::arg *argv) if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { g.log_bitmask.set_and_save(g.log_bitmask | bits); - }else{ 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(); - int start_pages[50]; - int end_pages[50]; + int start_pages[50] = {0,0,0}; + int end_pages[50] = {0,0,0}; if(num_existing_logs > 0){ 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]); }else{ - gcs.send_text_P(SEVERITY_LOW,PSTR(" 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)); // yaw - DataFlash.WriteByte(yaw_debug); + //DataFlash.WriteByte(yaw_debug); DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)yaw_error/100); @@ -599,7 +600,7 @@ static void Log_Write_Control_Tuning() static void Log_Read_Control_Tuning() { 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"), // Control @@ -607,7 +608,7 @@ static void Log_Read_Control_Tuning() DataFlash.ReadInt(), // yaw - (int)DataFlash.ReadByte(), + //(int)DataFlash.ReadByte(), DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt(), diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 976a4bdb99..168936d07b 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -101,7 +101,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui chan, current_loc.lat, 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.b.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, (dcm.yaw_sensor / 100) % 360, 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); break; } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 64b19dd811..d3968412fc 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -316,7 +316,7 @@ public: 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")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 8cdade0c46..fc744ae578 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -319,7 +319,7 @@ static void do_loiter_time() set_next_WP(¤t_loc); loiter_time = millis(); loiter_time_max = next_command.p1 * 1000; // units are (seconds) - Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max); + //Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max); //*/ } @@ -335,7 +335,7 @@ static bool verify_takeoff() 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){ //Serial.println("Y"); diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 5f86519941..7ae4cc9183 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -28,7 +28,7 @@ static void update_commands(void) if (g.waypoint_index < g.waypoint_total) { // only if we have a cmd stored in EEPROM 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 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 // ---------------------------------------- @@ -65,17 +65,17 @@ static void update_commands(void) static void verify_commands(void) { 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; // reset rate controlled nav g.pid_nav_wp.reset_I(); }else{ - Serial.printf("verified must false %d\n" , command_must_index); + //Serial.printf("verified must false %d\n" , command_must_index); } 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_ID = NO_COMMAND; } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index e6c9c3062d..4a3a2f3aa5 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -202,7 +202,7 @@ ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included - #define OPTFLOW_ENABLED + //#define OPTFLOW_ENABLED #endif #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) @@ -299,13 +299,13 @@ // Roll Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.5 +# define STABILIZE_ROLL_P 4.2 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.025 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX .5 // degrees +# define STABILIZE_ROLL_IMAX 15 // degrees #endif #ifndef RATE_ROLL_P @@ -315,7 +315,7 @@ # define RATE_ROLL_I 0.0 #endif #ifndef RATE_ROLL_IMAX -# define RATE_ROLL_IMAX 15 // degrees +# define RATE_ROLL_IMAX 5 // degrees #endif ////////////////////////////////////////////////////////////////////////////// @@ -328,7 +328,7 @@ # define STABILIZE_PITCH_I 0.025 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX .5 // degrees +# define STABILIZE_PITCH_IMAX 15 // degrees #endif #ifndef RATE_PITCH_P @@ -338,7 +338,7 @@ # define RATE_PITCH_I 0.0 #endif #ifndef RATE_PITCH_IMAX -# define RATE_PITCH_IMAX 15 // degrees +# define RATE_PITCH_IMAX 5 // degrees #endif ////////////////////////////////////////////////////////////////////////////// @@ -378,31 +378,31 @@ // Navigation control gains // #ifndef NAV_LOITER_P -# define NAV_LOITER_P 1.3 // +# define NAV_LOITER_P 1.4 // #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.01 // +# define NAV_LOITER_I 0.015 // #endif #ifndef NAV_LOITER_D -# define NAV_LOITER_D 0.4 // +# define NAV_LOITER_D 1.4 // #endif #ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 10 // degrees° +# define NAV_LOITER_IMAX 12 // degrees° #endif #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 #ifndef NAV_WP_I -# define NAV_WP_I 0.5 // this is a fast ramp up +# define NAV_WP_I 0.06 // this #endif #ifndef NAV_WP_D -# define NAV_WP_D .3 // +# define NAV_WP_D .5 // #endif #ifndef NAV_WP_IMAX -# define NAV_WP_IMAX 40 // degrees +# define NAV_WP_IMAX 20 // degrees #endif @@ -414,13 +414,13 @@ // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.3 // trying a lower val +# define THROTTLE_P 0.35 // trying a lower val #endif #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 #ifndef THROTTLE_D -# define THROTTLE_D 0.35 // upped with filter +# define THROTTLE_D 0.4 // upped with filter #endif #ifndef THROTTLE_IMAX # define THROTTLE_IMAX 30 @@ -503,24 +503,18 @@ #ifndef LOG_CMD # define LOG_CMD ENABLED #endif -#ifndef LOG_CURRENT -# define LOG_CURRENT DISABLED +// current +#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 - -// 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 LOG_ATTITUDE_FAST == ENABLED @@ -581,10 +575,6 @@ # define CUT_MOTORS 1 // do we cut the motors with no throttle? #endif -#ifndef CAMERA_STABILIZER -# define CAMERA_STABILIZER 1 // do we cut the motors with no throttle? -#endif - #ifndef BROKEN_SLIDER # define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) #endif diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 95a2ac59cb..db36a29be8 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -119,7 +119,8 @@ #define GUIDED 5 // AUTO control #define LOITER 6 // Hold a single location #define RTL 7 // AUTO control -#define NUM_MODES 8 +#define CIRCLE 8 // AUTO control +#define NUM_MODES 9 // YAW debug // --------- @@ -275,9 +276,10 @@ #define MASK_LOG_MODE (1<<6) #define MASK_LOG_RAW (1<<7) #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_OPTFLOW (1<<11) +#define MASK_LOG_SET_DEFAULTS (1<<15) // Waypoint Modes // ---------------- diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index a542d3f2a8..4c7a25a392 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -49,7 +49,11 @@ static void failsafe_off_event() static void low_battery_event(void) { 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); } diff --git a/ArduCopterMega/leds.pde b/ArduCopterMega/leds.pde index 514e1fdde4..e8a5088533 100644 --- a/ArduCopterMega/leds.pde +++ b/ArduCopterMega/leds.pde @@ -95,22 +95,25 @@ static void clear_leds() static void update_motor_leds(void) { // blink rear - static bool blink; + static bool blink = false; if (blink){ - blink = false; digitalWrite(RE_LED, HIGH); digitalWrite(FR_LED, HIGH); digitalWrite(RI_LED, LOW); digitalWrite(LE_LED, LOW); - }else{ - blink = true; digitalWrite(RE_LED, LOW); digitalWrite(FR_LED, LOW); digitalWrite(RI_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 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 0e0705e959..4eb620d47d 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -17,16 +17,21 @@ static void arm_motors() // full right if (g.rc_4.control_in > 4000) { + // don't allow arming in anything but manual if (control_mode < ALT_HOLD){ if (arming_counter > AUTO_LEVEL_DELAY){ - auto_level_counter = 255; + auto_level_counter = 155; arming_counter = 0; }else if (arming_counter == ARM_DELAY){ motor_armed = true; arming_counter = ARM_DELAY; + // Clear throttle slew + // ------------------- + throttle_slew = 0; + // Remember Orientation // -------------------- init_simple_bearing(); @@ -36,10 +41,15 @@ static void arm_motors() if(home_is_set) init_home(); + if(did_ground_start == false){ + did_ground_start = true; + startup_ground(); + } + // tune down compass // ----------------- - //dcm.kp_yaw(0.02); - //dcm.ki_yaw(0); + dcm.kp_yaw(0.08); + dcm.ki_yaw(0); arming_counter++; } else{ @@ -59,6 +69,16 @@ static void arm_motors() motor_armed = false; arming_counter = DISARM_DELAY; compass.save_offsets(); + + // tune up compass + // ----------------- + dcm.kp_yaw(0.8); + dcm.ki_yaw(0.00004); + + // Clear throttle slew + // ------------------- + throttle_slew = 0; + arming_counter++; }else{ diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 8fffd2a76c..d77055f67f 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -49,7 +49,7 @@ get_nav_throttle(long error) int throttle; // 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.throttle_cruise + constrain(throttle, -80, 80); @@ -138,28 +138,31 @@ static void calc_nav_output() } // called after we get GPS read -static void calc_rate_nav() +static void calc_rate_nav(int speed) { // 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); // 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)); // Reduce speed on RTL - //if(control_mode == RTL){ - int tmp = min(wp_distance, 50) * 100; - waypoint_speed = min(tmp, g.waypoint_speed_max.get()); - waypoint_speed = max(waypoint_speed, 80); - //}else{ - // waypoint_speed = g.waypoint_speed_max.get(); - //} + if(control_mode == RTL){ + int tmp = min(wp_distance, 80) * 50; + waypoint_speed = min(tmp, speed); + waypoint_speed = max(waypoint_speed, 50); + }else{ + 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); // Scale response by kP - nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0); - nav_lat >>= 1; // divide by two + nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0); + 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); @@ -169,6 +172,7 @@ static void calc_rate_nav() static void calc_bearing_error() { + // 83 99 Yaw = -16 bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = wrap_180(bearing_error); } @@ -193,6 +197,28 @@ static void calc_altitude_smoothing_error() 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) { if (error > 36000) error -= 36000; diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 1d49a9a30d..21e6758962 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -78,6 +78,10 @@ static void init_rc_out() OCR4B = 0xFFFF; // PH4, OUT6 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 if(g.esc_calibrate == 1) return; diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index d47dc701bf..5406134391 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -11,10 +11,11 @@ static void init_barometer(void) hil.update(); // look for inbound hil packets for initialization #endif + ground_temperature = barometer.Temp; // We take some readings... - for(int i = 0; i < 200; i++){ - delay(25); + for(int i = 0; i < 20; i++){ + delay(20); #if HIL_MODE == HIL_MODE_SENSORS hil.update(); // look for inbound hil packets @@ -23,12 +24,11 @@ static void init_barometer(void) // Get initial data from absolute pressure sensor ground_pressure = read_baro_filtered(); 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."); } @@ -39,12 +39,13 @@ static long read_baro_filtered(void) // get new data from absolute pressure sensor barometer.Read(); + // add new data into our filter baro_filter[baro_filter_index] = barometer.Press; baro_filter_index++; // loop our filter - if(baro_filter_index == BARO_FILTER_SIZE) + if(baro_filter_index >= BARO_FILTER_SIZE) baro_filter_index = 0; // zero out our accumulator @@ -54,6 +55,7 @@ static long read_baro_filtered(void) pressure += baro_filter[i]; } + // average our sampels return pressure /= BARO_FILTER_SIZE; } @@ -104,8 +106,11 @@ static void read_battery(void) } #if BATTERY_EVENT == 1 - if(battery_voltage < LOW_VOLTAGE) low_battery_event(); - if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); + if(battery_voltage < LOW_VOLTAGE) + low_battery_event(); + + if(g.battery_monitoring == 4 && current_total > g.pack_capacity) + low_battery_event(); #endif } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index f978d30fa2..6cff757029 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -229,7 +229,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t 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); @@ -740,6 +745,32 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) } #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 /***************************************************************************/ diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 1bf8b92aa0..4d268e1129 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // 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 @@ -146,14 +146,13 @@ static void init_ardupilot() } }else{ - // unsigned long before = micros(); // Load all auto-loaded EEPROM variables 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 { @@ -168,10 +167,7 @@ static void init_ardupilot() init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs - - #if CAMERA_STABILIZER == ENABLED - init_camera(); - #endif + init_camera(); #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization @@ -179,7 +175,6 @@ static void init_ardupilot() #endif // Do GPS init - //g_gps = &GPS; g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization @@ -209,19 +204,13 @@ static void init_ardupilot() if(g.compass_enabled) init_compass(); - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.sonar_enabled){ - sonar.init(SONAR_PORT, &adc); - } - #endif - #ifdef OPTFLOW_ENABLED // init the optical flow sensor if(g.optflow_enabled) { init_optflow(); } #endif - + // Logging: // -------- // DataFlash log initialization @@ -260,52 +249,10 @@ static void init_ardupilot() start_new_log(); } - // All of the Gyro calibrations - // ---------------------------- - 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) + //#if(GROUND_START_DELAY > 0) //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); - delay(GROUND_START_DELAY * 1000); - #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(); + // delay(GROUND_START_DELAY * 1000); + //#endif GPS_enabled = false; @@ -325,33 +272,77 @@ static void startup_ground(void) } //*/ - // 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 -#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 + // lengthen the idle timeout for gps Auto_detect + // --------------------------------------------- g_gps->idleTimeout = 20000; - // used to limit the input of error for loiter - loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP(); - //Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max); + // print the GPS status + // -------------------- + 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(); 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) @@ -360,6 +351,14 @@ static void set_mode(byte mode) // don't switch modes if we are already in the correct mode. 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; control_mode = mode; @@ -381,6 +380,7 @@ static void set_mode(byte mode) switch(control_mode) { case ACRO: + g.pid_throttle.reset_I(); break; case SIMPLE: @@ -399,6 +399,7 @@ static void set_mode(byte mode) init_auto(); break; + case CIRCLE: case LOITER: init_throttle_cruise(); do_loiter_at_location(); @@ -502,6 +503,7 @@ init_throttle_cruise() { // are we moving from manual throttle to auto_throttle? 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); } }