mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
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
This commit is contained in:
jasonshort
parent
e1b677a25d
commit
73be185414
@ -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,7 +20,7 @@
|
||||
HELI_FRAME
|
||||
*/
|
||||
|
||||
//#define FRAME_ORIENTATION X_FRAME
|
||||
#define FRAME_ORIENTATION X_FRAME
|
||||
/*
|
||||
PLUS_FRAME
|
||||
X_FRAME
|
||||
@ -26,7 +28,7 @@
|
||||
*/
|
||||
|
||||
|
||||
//#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
|
||||
|
||||
@ -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 <AP_RangeFinder.h> // Range finder library
|
||||
#include <AP_OpticalFlow.h> // Optical Flow library
|
||||
#include <ModeFilter.h>
|
||||
|
||||
#define MAVLINK_COMM_NUM_BUFFERS 2
|
||||
#include <GCS_MAVLink.h> // 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,6 +312,7 @@ 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
|
||||
// ---------------------
|
||||
@ -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
|
||||
// ----------------
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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();
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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("<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));
|
||||
|
||||
// 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(),
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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")),
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
// ----------------
|
||||
|
||||
@ -49,6 +49,10 @@ static void failsafe_off_event()
|
||||
static void low_battery_event(void)
|
||||
{
|
||||
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
|
||||
low_batt = true;
|
||||
|
||||
// if we are in Auto mode, come home
|
||||
if(control_mode >= AUTO)
|
||||
set_mode(RTL);
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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{
|
||||
|
||||
@ -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{
|
||||
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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
/***************************************************************************/
|
||||
|
||||
@ -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
|
||||
|
||||
#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,12 +204,6 @@ 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) {
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user