Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Removed Simple Mode

Browse Source 
Added Simple option to mode switch positions
removed unused OptFlow code
added reset lat and lon error
removed unused init_auto
Fixed log formatting
Added simple mode bitmask
removed pitchmax
added CLI setup for enabling Simple mode to any switch position
This commit is contained in:
Jason Short 2011-09-14 13:58:18 -07:00
parent bca934b538
commit ae81a758de
12 changed files with 240 additions and 327 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/APM_Config.h
View File

@ -46,10 +46,17 @@
CH6_TRAVERSE_SPEED CH6_TRAVERSE_SPEED
*/ */
# define CH7_OPTION DO_SET_HOVER
/*
DO_SET_HOVER
DO_FLIP
SIMPLE_MODE_CONTROL
*/
// See the config.h and defines.h files for how to set this up! // See the config.h and defines.h files for how to set this up!
// //
// lets use SIMPLE mode for Roll and Pitch during Alt Hold // lets use SIMPLE mode for Roll and Pitch during Alt Hold
#define ALT_HOLD_RP ROLL_PITCH_SIMPLE //#define ALT_HOLD_RP ROLL_PITCH_SIMPLE
// lets use Manual throttle during Loiter // lets use Manual throttle during Loiter
//#define LOITER_THR THROTTLE_MANUAL //#define LOITER_THR THROTTLE_MANUAL

237
ArduCopter/ArduCopter.pde
View File

@ -237,7 +237,6 @@ static const char *comma = ",";
static const char* flight_mode_strings[] = { static const char* flight_mode_strings[] = {
"STABILIZE", "STABILIZE",
"ACRO", "ACRO",
"SIMPLE",
"ALT_HOLD", "ALT_HOLD",
"AUTO", "AUTO",
"GUIDED", "GUIDED",
@ -275,6 +274,7 @@ static byte control_mode = STABILIZE;
static byte old_control_mode = STABILIZE; static byte old_control_mode = STABILIZE;
static byte oldSwitchPosition; // for remembering the control mode switch static byte oldSwitchPosition; // for remembering the control mode switch
static int motor_out[8]; static int motor_out[8];
static bool do_simple = false;
// Heli // Heli
// ---- // ----
@ -338,7 +338,7 @@ static float cos_pitch_x = 1;
static float cos_yaw_x = 1; static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y; static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static long initial_simple_bearing; // used for Simple mode static long initial_simple_bearing; // used for Simple mode
static float boost; // used to give a little extra to maintain altitude
// Acro // Acro
#if CH7_OPTION == DO_FLIP #if CH7_OPTION == DO_FLIP
@ -493,7 +493,7 @@ static int gps_fix_count;
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count; //static int mainLoop_count;
static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
@ -504,7 +504,6 @@ static uint8_t delta_ms_fiftyhz;
static byte slow_loopCounter; static byte slow_loopCounter;
static int superslow_loopCounter; static int superslow_loopCounter;
static byte alt_timer; // for limiting the execution of flight mode thingys
static byte simple_timer; // for limiting the execution of flight mode thingys static byte simple_timer; // for limiting the execution of flight mode thingys
@ -533,12 +532,7 @@ void loop()
//PORTK |= B00010000; //PORTK |= B00010000;
delta_ms_fast_loop = millis() - fast_loopTimer; delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis(); fast_loopTimer = millis();
//load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
mainLoop_count++;
//if (delta_ms_fast_loop > 6)
// Log_Write_Performance();
// Execute the fast loop // Execute the fast loop
// --------------------- // ---------------------
@ -568,7 +562,7 @@ void loop()
} }
if (millis() - perf_mon_timer > 20000) { if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) { //if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT); gcs.send_message(MSG_PERF_REPORT);
@ -576,7 +570,7 @@ void loop()
Log_Write_Performance(); Log_Write_Performance();
resetPerfData(); resetPerfData();
} //}
} }
//PORTK &= B10111111; //PORTK &= B10111111;
} }
@ -610,7 +604,6 @@ static void fast_loop()
// --------------------------------------- // ---------------------------------------
update_yaw_mode(); update_yaw_mode();
update_roll_pitch_mode(); update_roll_pitch_mode();
update_throttle_mode();
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
@ -630,6 +623,12 @@ static void medium_loop()
medium_loopCounter++; medium_loopCounter++;
/*
if(g.optflow_enabled){
optflow.update()
}
*/
if(GPS_enabled){ if(GPS_enabled){
update_GPS(); update_GPS();
} }
@ -646,6 +645,10 @@ static void medium_loop()
// auto_trim, uses an auto_level algorithm // auto_trim, uses an auto_level algorithm
auto_trim(); auto_trim();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
break; break;
// This case performs some navigation computations // This case performs some navigation computations
@ -654,13 +657,6 @@ static void medium_loop()
loop_step = 2; loop_step = 2;
medium_loopCounter++; medium_loopCounter++;
// hack to stop navigation in Simple mode
if (control_mode == SIMPLE){
// clear GPS data
g_gps->new_data = false;
break;
}
// Auto control modes: // Auto control modes:
if(g_gps->new_data && g_gps->fix){ if(g_gps->new_data && g_gps->fix){
loop_step = 11; loop_step = 11;
@ -696,17 +692,14 @@ static void medium_loop()
// -------------------------- // --------------------------
update_altitude(); update_altitude();
// altitude smoothing
// ------------------
//calc_altitude_smoothing_error();
altitude_error = get_altitude_error();
//camera_stabilization();
// invalidate the throttle hold value // invalidate the throttle hold value
// ---------------------------------- // ----------------------------------
invalid_throttle = true; invalid_throttle = true;
// calc boost
// ----------
boost = get_angle_boost();
break; break;
// This case deals with sending high rate telemetry // This case deals with sending high rate telemetry
@ -722,11 +715,13 @@ static void medium_loop()
} }
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) if(motor_armed){
Log_Write_Attitude(); if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN && motor_armed) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
}
#endif #endif
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
@ -785,9 +780,9 @@ static void medium_loop()
// --------------------------- // ---------------------------
static void fifty_hz_loop() static void fifty_hz_loop()
{ {
// record throttle output // moved to slower loop
// ------------------------------ // --------------------
throttle_integrator += g.rc_3.servo_out; update_throttle_mode();
// Read Sonar // Read Sonar
// ---------- // ----------
@ -1010,86 +1005,6 @@ static void update_GPS(void)
} }
} }
#ifdef OPTFLOW_ENABLED
// blend gps and optical flow location
void update_location(void)
{
//static int count = 0;
// get GPS position
if(GPS_enabled){
update_GPS();
}
if( g.optflow_enabled ) {
int32_t temp_lat, temp_lng, diff_lat, diff_lng;
// get optical flow position
optflow.read();
optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt);
// write to log
if (g.log_bitmask & MASK_LOG_OPTFLOW){
Log_Write_Optflow();
}
temp_lat = optflow_offset.lat + optflow.lat;
temp_lng = optflow_offset.lng + optflow.lng;
// if we have good GPS values, don't let optical flow position stray too far
if( GPS_enabled && g_gps->fix ) {
// ensure current location is within 3m of gps location
diff_lat = g_gps->latitude - temp_lat;
diff_lng = g_gps->longitude - temp_lng;
if( diff_lat > 300 ) {
optflow_offset.lat += diff_lat - 300;
//Serial.println("lat inc!");
}
if( diff_lat < -300 ) {
optflow_offset.lat += diff_lat + 300;
//Serial.println("lat dec!");
}
if( diff_lng > 300 ) {
optflow_offset.lng += diff_lng - 300;
//Serial.println("lng inc!");
}
if( diff_lng < -300 ) {
optflow_offset.lng += diff_lng + 300;
//Serial.println("lng dec!");
}
}
// update the current position
current_loc.lat = optflow_offset.lat + optflow.lat;
current_loc.lng = optflow_offset.lng + optflow.lng;
/*count++;
if( count >= 20 ) {
count = 0;
Serial.println();
Serial.print("lat:");
Serial.print(current_loc.lat);
Serial.print("\tlng:");
Serial.print(current_loc.lng);
Serial.print("\tr:");
Serial.print(nav_roll);
Serial.print("\tp:");
Serial.print(nav_pitch);
Serial.println();
}*/
// indicate we have a new position for nav functions
new_location = true;
}else{
// get current position from gps
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
new_location = g_gps->new_data;
}
}
#endif
void update_yaw_mode(void) void update_yaw_mode(void)
{ {
switch(yaw_mode){ switch(yaw_mode){
@ -1100,8 +1015,11 @@ void update_yaw_mode(void)
case YAW_HOLD: case YAW_HOLD:
// calcualte new nav_yaw offset // calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); if (control_mode <= STABILIZE){
//Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw); nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
}else{
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1);
}
break; break;
case YAW_LOOK_AT_HOME: case YAW_LOOK_AT_HOME:
@ -1136,6 +1054,29 @@ void update_roll_pitch_mode(void)
int control_roll = 0, control_pitch = 0; int control_roll = 0, control_pitch = 0;
if(do_simple){
simple_timer++;
if(simple_timer > 5){
simple_timer = 0;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
// pre-calc rotation (stored in real degrees)
// roll
float cos_x = sin(radians(90 - delta));
// pitch
float sin_y = cos(radians(90 - delta));
// Rotate input by the initial bearing
// roll
control_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y;
// pitch
control_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
}
switch(roll_pitch_mode){ switch(roll_pitch_mode){
case ROLL_PITCH_ACRO: case ROLL_PITCH_ACRO:
@ -1152,29 +1093,6 @@ void update_roll_pitch_mode(void)
control_pitch = g.rc_2.control_in; control_pitch = g.rc_2.control_in;
break; break;
case ROLL_PITCH_SIMPLE:
simple_timer++;
if(simple_timer > 5){
simple_timer = 0;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
// pre-calc rotation (stored in real degrees)
// roll
float cos_x = sin(radians(90 - delta));
// pitch
float sin_y = cos(radians(90 - delta));
// Rotate input by the initial bearing
// roll
nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y;
// pitch
nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x);
}
control_roll = nav_roll;
control_pitch = nav_pitch;
break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
// mix in user control with Nav control // mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll); control_roll = g.rc_1.control_mix(nav_roll);
@ -1194,7 +1112,7 @@ void update_throttle_mode(void)
{ {
switch(throttle_mode){ switch(throttle_mode){
case THROTTLE_MANUAL: case THROTTLE_MANUAL:
g.rc_3.servo_out = get_throttle(g.rc_3.control_in); g.rc_3.servo_out = g.rc_3.control_in + boost;
break; break;
case THROTTLE_HOLD: case THROTTLE_HOLD:
@ -1204,16 +1122,20 @@ void update_throttle_mode(void)
// fall through // fall through
case THROTTLE_AUTO: case THROTTLE_AUTO:
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){ if(invalid_throttle && motor_auto_armed == true){
// how far off are we
altitude_error = get_altitude_error();
// get the AP throttle // get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s
// apply throttle control
g.rc_3.servo_out = get_throttle(nav_throttle);
// clear the new data flag // clear the new data flag
invalid_throttle = false; invalid_throttle = false;
} }
// apply throttle control at 200 hz
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost;
break; break;
} }
} }
@ -1237,6 +1159,11 @@ static void update_navigation()
break; break;
case GUIDED: case GUIDED:
wp_control = WP_MODE;
update_auto_yaw();
update_nav_wp();
break;
case RTL: case RTL:
if(wp_distance > 4){ if(wp_distance > 4){
// calculates desired Yaw // calculates desired Yaw
@ -1251,7 +1178,6 @@ static void update_navigation()
//xtrack_enabled = false; //xtrack_enabled = false;
} }
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
break; break;
@ -1273,8 +1199,8 @@ static void update_navigation()
//circle_angle += dTnav; //1000 * (dTnav/1000); //circle_angle += dTnav; //1000 * (dTnav/1000);
circle_angle = wrap_360(target_bearing + 2000 + 18000); circle_angle = wrap_360(target_bearing + 2000 + 18000);
target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle)); target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(radians(90 - circle_angle)));
target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle)); target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(radians(90 - circle_angle)));
} }
// calc the lat and long error to the target // calc the lat and long error to the target
@ -1379,18 +1305,13 @@ static void update_altitude()
static void static void
adjust_altitude() adjust_altitude()
{ {
alt_timer++; if(g.rc_3.control_in <= 200){
if(alt_timer >= 2){ next_WP.alt -= 1; // 1 meter per second
alt_timer = 0; next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
if(g.rc_3.control_in <= 200){ }else if (g.rc_3.control_in > 700){
next_WP.alt -= 1; // 1 meter per second next_WP.alt += 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
}else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
}
} }
} }

31
ArduCopter/Attitude.pde
View File

@ -91,7 +91,7 @@ static int
get_nav_throttle(long z_error, int target_speed) get_nav_throttle(long z_error, int target_speed)
{ {
int rate_error; int rate_error;
int throttle; //int throttle;
float scaler = (float)target_speed/(float)ALT_ERROR_MAX; float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
// limit error to prevent I term run up // limit error to prevent I term run up
@ -101,8 +101,11 @@ get_nav_throttle(long z_error, int target_speed)
rate_error = target_speed - altitude_rate; rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -110, 110); rate_error = constrain(rate_error, -110, 110);
throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); //throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
return g.throttle_cruise + throttle; //return g.throttle_cruise + throttle;
return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
} }
@ -156,10 +159,16 @@ static void reset_hold_I(void)
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
static void reset_nav_I(void) static void reset_nav(void)
{ {
nav_throttle = 0;
invalid_throttle = true;
g.pi_nav_lat.reset_I(); g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I(); g.pi_nav_lon.reset_I();
long_error = 0;
lat_error = 0;
} }
@ -169,11 +178,11 @@ throttle control
// user input: // user input:
// ----------- // -----------
static int get_throttle(int throttle_input) //static int get_throttle(int throttle_input)
{ //{
throttle_input = (float)throttle_input * angle_boost(); // throttle_input = (float)throttle_input * angle_boost();
return max(throttle_input, 0); // return max(throttle_input, 0);
} //}
static long static long
get_nav_yaw_offset(int yaw_input, int reset) get_nav_yaw_offset(int yaw_input, int reset)
@ -188,7 +197,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
// re-define nav_yaw if we have stick input // re-define nav_yaw if we have stick input
if(yaw_input != 0){ if(yaw_input != 0){
// set nav_yaw + or - the current location // set nav_yaw + or - the current location
_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);
@ -210,7 +219,7 @@ static int alt_hold_velocity()
} }
*/ */
static float angle_boost() static float get_angle_boost()
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0); temp = 2.0 - constrain(temp, .5, 1.0);

119
ArduCopter/Log.pde
View File

@ -365,16 +365,15 @@ static void Log_Write_GPS()
DataFlash.WriteByte(LOG_GPS_MSG); DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(g_gps->time); // 1 DataFlash.WriteLong(g_gps->time); // 1
DataFlash.WriteByte(g_gps->fix); // 2 DataFlash.WriteByte(g_gps->num_sats); // 2
DataFlash.WriteByte(g_gps->num_sats); // 3
DataFlash.WriteLong(current_loc.lat); // 4 DataFlash.WriteLong(current_loc.lat); // 3
DataFlash.WriteLong(current_loc.lng); // 5 DataFlash.WriteLong(current_loc.lng); // 4
DataFlash.WriteLong(current_loc.alt); // 7 DataFlash.WriteLong(current_loc.alt); // 5
DataFlash.WriteLong(g_gps->altitude); // 6 DataFlash.WriteLong(g_gps->altitude); // 6
DataFlash.WriteInt(g_gps->ground_speed); // 8 DataFlash.WriteInt(g_gps->ground_speed); // 7
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9 DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -382,21 +381,20 @@ static void Log_Write_GPS()
// Read a GPS packet // Read a GPS packet
static void Log_Read_GPS() static void Log_Read_GPS()
{ {
Serial.printf_P(PSTR("GPS, %ld, %d, %d, " Serial.printf_P(PSTR("GPS, %ld, %d, "
"%4.7f, %4.7f, %4.4f, %4.4f, " "%4.7f, %4.7f, %4.4f, %4.4f, "
"%d, %u\n"), "%d, %u\n"),
DataFlash.ReadLong(), // 1 time DataFlash.ReadLong(), // 1 time
(int)DataFlash.ReadByte(), // 2 fix (int)DataFlash.ReadByte(), // 2 sats
(int)DataFlash.ReadByte(), // 3 sats
(float)DataFlash.ReadLong() / t7, // 4 lat (float)DataFlash.ReadLong() / t7, // 3 lat
(float)DataFlash.ReadLong() / t7, // 5 lon (float)DataFlash.ReadLong() / t7, // 4 lon
(float)DataFlash.ReadLong() / 100.0, // 6 gps alt (float)DataFlash.ReadLong() / 100.0, // 5 gps alt
(float)DataFlash.ReadLong() / 100.0, // 7 sensor alt (float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
DataFlash.ReadInt(), // 8 ground speed DataFlash.ReadInt(), // 7 ground speed
(uint16_t)DataFlash.ReadInt()); // 9 ground course (uint16_t)DataFlash.ReadInt()); // 8 ground course
} }
@ -514,7 +512,7 @@ static void Log_Write_Optflow()
} }
#endif #endif
// Read an attitude packet
static void Log_Read_Optflow() static void Log_Read_Optflow()
{ {
Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
@ -609,23 +607,18 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// Control
//DataFlash.WriteInt((int)(g.rc_4.control_in/100));
//DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw // yaw
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)(nav_yaw/100)); //2
DataFlash.WriteInt((int)yaw_error/100); DataFlash.WriteInt((int)yaw_error/100); //3
// Alt hold // Alt hold
DataFlash.WriteInt(g.rc_3.servo_out); DataFlash.WriteInt(g.rc_3.servo_out); //4
DataFlash.WriteInt(sonar_alt); // DataFlash.WriteInt(sonar_alt); //5
DataFlash.WriteInt(baro_alt); // DataFlash.WriteInt(baro_alt); //6
DataFlash.WriteInt((int)next_WP.alt); // DataFlash.WriteInt((int)next_WP.alt); //7
//DataFlash.WriteInt((int)altitude_error); // DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -635,10 +628,9 @@ static void Log_Write_Control_Tuning()
static void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()
{ {
Serial.printf_P(PSTR( "CTUN, " Serial.printf_P(PSTR( "CTUN, "
//"%d, %d, "
"%d, %d, %d, " "%d, %d, %d, "
"%d, %d, %d, " "%d, %d, %d, "
"%d, %d, %d\n"), "%d, %d\n"),
// Control // Control
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -670,21 +662,18 @@ static void Log_Write_Performance()
//* //*
DataFlash.WriteLong( millis()- perf_mon_timer); //DataFlash.WriteLong( millis()- perf_mon_timer);
DataFlash.WriteInt ( mainLoop_count); //DataFlash.WriteInt ( mainLoop_count);
DataFlash.WriteInt ( G_Dt_max); DataFlash.WriteInt ( G_Dt_max); //1
DataFlash.WriteByte( dcm.gyro_sat_count); DataFlash.WriteByte( dcm.gyro_sat_count); //2
DataFlash.WriteByte( imu.adc_constraints); DataFlash.WriteByte( imu.adc_constraints); //3
DataFlash.WriteByte( dcm.renorm_sqrt_count); DataFlash.WriteByte( dcm.renorm_sqrt_count); //4
DataFlash.WriteByte( dcm.renorm_blowup_count); DataFlash.WriteByte( dcm.renorm_blowup_count); //5
DataFlash.WriteByte( gps_fix_count); DataFlash.WriteByte( gps_fix_count); //6
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7
DataFlash.WriteLong ( throttle_integrator); DataFlash.WriteLong ( throttle_integrator); //8
//*/
//PM, 20005, 3742, 10,0,0,0,0,89,1000,
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -692,24 +681,23 @@ static void Log_Write_Performance()
// Read a performance packet // Read a performance packet
static void Log_Read_Performance() static void Log_Read_Performance()
{ {
Serial.printf_P(PSTR( "PM, %ld, %d, %d, " Serial.printf_P(PSTR( "PM, %d, %d, %d, "
"%d, %d, %d, %d, %d, " "%d, %d, %d, "
"%d, %ld\n"), "%d, %ld\n"),
// Control // Control
DataFlash.ReadLong(), //DataFlash.ReadLong(),
DataFlash.ReadInt(), //DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(), //1
DataFlash.ReadByte(), //2
DataFlash.ReadByte(), //3
DataFlash.ReadByte(), DataFlash.ReadByte(), //4
DataFlash.ReadByte(), DataFlash.ReadByte(), //5
DataFlash.ReadByte(), //6
DataFlash.ReadByte(), DataFlash.ReadInt(), //7
DataFlash.ReadByte(), DataFlash.ReadLong()); //8
DataFlash.ReadByte(),
DataFlash.ReadInt(),
DataFlash.ReadLong());
} }
// Write a command processing packet. // Write a command processing packet.
@ -760,19 +748,28 @@ static void Log_Write_Attitude()
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG); DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt((int)dcm.roll_sensor); DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)dcm.pitch_sensor); DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)g.rc_1.servo_out);
DataFlash.WriteInt((int)g.rc_2.servo_out);
DataFlash.WriteInt((int)g.rc_4.servo_out);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
// Read an attitude packet // Read an attitude packet
static void Log_Read_Attitude() static void Log_Read_Attitude()
{ {
Serial.printf_P(PSTR("ATT, %d, %d, %u\n"), Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, $d\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt()); (uint16_t)DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt());
} }
// Write a mode packet. Total length : 5 bytes // Write a mode packet. Total length : 5 bytes
@ -827,6 +824,8 @@ static void Log_Read(int start_page, int end_page)
case 0: case 0:
if(data == HEAD_BYTE1) // Head byte 1 if(data == HEAD_BYTE1) // Head byte 1
log_step++; log_step++;
else
Serial.println(".");
break; break;
case 1: case 1:

11
ArduCopter/Parameters.h
View File

@ -83,7 +83,6 @@ public:
// 160: Navigation parameters // 160: Navigation parameters
// //
k_param_crosstrack_entry_angle = 160, k_param_crosstrack_entry_angle = 160,
k_param_pitch_max,
k_param_RTL_altitude, k_param_RTL_altitude,
// //
@ -139,7 +138,7 @@ public:
k_param_flight_mode4, k_param_flight_mode4,
k_param_flight_mode5, k_param_flight_mode5,
k_param_flight_mode6, k_param_flight_mode6,
k_param_simple_modes,
// //
// 220: Waypoint data // 220: Waypoint data
@ -179,7 +178,7 @@ public:
// //
AP_Int16 sysid_this_mav; AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud; AP_Int8 serial3_baud;
// Crosstrack navigation // Crosstrack navigation
@ -213,6 +212,7 @@ public:
AP_Int8 flight_mode4; AP_Int8 flight_mode4;
AP_Int8 flight_mode5; AP_Int8 flight_mode5;
AP_Int8 flight_mode6; AP_Int8 flight_mode6;
AP_Int8 simple_modes;
// Radio settings // Radio settings
// //
@ -221,8 +221,6 @@ public:
//AP_Var_group pwm_throttle; //AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw; //AP_Var_group pwm_yaw;
AP_Int16 pitch_max;
// Misc // Misc
// //
AP_Int16 log_bitmask; AP_Int16 log_bitmask;
@ -324,8 +322,7 @@ public:
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")), flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")), flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")), flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),

13
ArduCopter/commands.pde
View File

@ -1,18 +1,5 @@
// -*- 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 -*-
/*static void init_auto()
{
//if (g.waypoint_index == g.waypoint_total) {
// do_RTL();
//}
// initialize commands
// -------------------
init_commands();
}
*/
static void init_commands() static void init_commands()
{ {
// zero is home, but we always load the next command (1), in the code. // zero is home, but we always load the next command (1), in the code.

16
ArduCopter/commands_logic.pde
View File

@ -311,13 +311,10 @@ static void do_loiter_turns()
static void do_loiter_time() static void do_loiter_time()
{ {
///* wp_control = LOITER_MODE;
wp_control = LOITER_MODE;
set_next_WP(&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);
//*/
} }
/********************************************************************************/ /********************************************************************************/
@ -464,11 +461,12 @@ static void do_change_alt()
{ {
Location temp = next_WP; Location temp = next_WP;
condition_start = current_loc.alt; condition_start = current_loc.alt;
if (next_command.options & WP_OPTION_ALT_RELATIVE) { condition_value = next_command.alt;
condition_value = next_command.alt + home.alt; //if (next_command.options & WP_OPTION_ALT_RELATIVE) {
} else { // condition_value = next_command.alt + home.alt;
condition_value = next_command.alt; //} else {
}
//}
temp.alt = condition_value; temp.alt = condition_value;
set_next_WP(&temp); set_next_WP(&temp);
} }

2
ArduCopter/config.h
View File

@ -579,7 +579,7 @@
# define LOG_ATTITUDE_FAST DISABLED # define LOG_ATTITUDE_FAST DISABLED
#endif #endif
#ifndef LOG_ATTITUDE_MED #ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED DISABLED # define LOG_ATTITUDE_MED ENABLED
#endif #endif
#ifndef LOG_GPS #ifndef LOG_GPS
# define LOG_GPS ENABLED # define LOG_GPS ENABLED

10
ArduCopter/control_modes.pde
View File

@ -13,6 +13,12 @@ static void read_control_switch()
switch_debouncer = false; switch_debouncer = false;
set_mode(flight_modes[switchPosition]); set_mode(flight_modes[switchPosition]);
// setup Simple mode
// do we enable simple mode?
do_simple = (g.simple_modes & 1 << switchPosition);
Serial.printf("do simple: %d \n", (int)do_simple);
}else{ }else{
switch_debouncer = true; switch_debouncer = true;
} }
@ -47,6 +53,10 @@ static void read_trim_switch()
do_flip = true; do_flip = true;
} }
#elif CH7_OPTION == SIMPLE_MODE_CONTROL
do_simple = (g.rc_7.control_in > 800);
#elif CH7_OPTION == DO_SET_HOVER #elif CH7_OPTION == DO_SET_HOVER
// switch is engaged // switch is engaged
if (g.rc_7.control_in > 800){ if (g.rc_7.control_in > 800){

23
ArduCopter/defines.h
View File

@ -30,6 +30,7 @@
// CH 7 control // CH 7 control
#define DO_SET_HOVER 0 #define DO_SET_HOVER 0
#define DO_FLIP 1 #define DO_FLIP 1
#define SIMPLE_MODE_CONTROL 2
// Frame types // Frame types
#define QUAD_FRAME 0 #define QUAD_FRAME 0
@ -121,14 +122,20 @@
// ---------------- // ----------------
#define STABILIZE 0 // hold level position #define STABILIZE 0 // hold level position
#define ACRO 1 // rate control #define ACRO 1 // rate control
#define SIMPLE 2 // #define ALT_HOLD 2 // AUTO control
#define ALT_HOLD 3 // AUTO control #define AUTO 3 // AUTO control
#define AUTO 4 // AUTO control #define GUIDED 4 // AUTO control
#define GUIDED 5 // AUTO control #define LOITER 5 // Hold a single location
#define LOITER 6 // Hold a single location #define RTL 6 // AUTO control
#define RTL 7 // AUTO control #define CIRCLE 7 // AUTO control
#define CIRCLE 8 // AUTO control #define NUM_MODES 8
#define NUM_MODES 9
#define SIMPLE_1 1
#define SIMPLE_2 2
#define SIMPLE_3 4
#define SIMPLE_4 8
#define SIMPLE_5 16
#define SIMPLE_6 32
// CH_6 Tuning // CH_6 Tuning
// ----------- // -----------

67
ArduCopter/setup.pde
View File

@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_flight_modes(); report_flight_modes();
report_imu(); report_imu();
report_compass(); report_compass();
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
report_optflow(); report_optflow();
#endif #endif
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
report_heli(); report_heli();
report_gyro(); report_gyro();
@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
byte _oldSwitchPosition = 0; byte _oldSwitchPosition = 0;
byte mode = 0; byte mode = 0;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off"));
print_hit_enter(); print_hit_enter();
while(1){ while(1){
@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode = constrain(mode, 0, NUM_MODES-1); mode = constrain(mode, 0, NUM_MODES-1);
// update the user // update the user
print_switch(_switchPosition, mode); print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
// Remember switch position // Remember switch position
_oldSwitchPosition = _switchPosition; _oldSwitchPosition = _switchPosition;
} }
// look for stick input // look for stick input
if (radio_input_switch() == true){ if (abs(g.rc_1.control_in) > 3000){
mode++; mode++;
if(mode >= NUM_MODES) if(mode >= NUM_MODES)
mode = 0; mode = 0;
@ -334,13 +336,31 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
flight_modes[_switchPosition] = mode; flight_modes[_switchPosition] = mode;
// print new mode // print new mode
print_switch(_switchPosition, mode); print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (abs(g.rc_4.control_in) > 3000){
g.simple_modes |= (1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (abs(g.rc_4.control_in) < 3000){
g.simple_modes &= ~(1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
} }
// escape hatch // escape hatch
if(Serial.available() > 0){ if(Serial.available() > 0){
for (mode=0; mode<6; mode++) for (mode = 0; mode < 6; mode++)
flight_modes[mode].save(); flight_modes[mode].save();
print_done(); print_done();
report_flight_modes(); report_flight_modes();
return (0); return (0);
@ -872,7 +892,7 @@ static void report_flight_modes()
print_divider(); print_divider();
for(int i = 0; i < 6; i++ ){ for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]); print_switch(i, flight_modes[i], (g.simple_modes & (1<<i)));
} }
print_blanks(2); print_blanks(2);
} }
@ -956,10 +976,15 @@ print_radio_values()
} }
static void static void
print_switch(byte p, byte m) print_switch(byte p, byte m, bool b)
{ {
Serial.printf_P(PSTR("Pos %d: "),p); Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]); Serial.print(flight_mode_strings[m]);
Serial.printf_P(PSTR(", Simple: "));
if(b)
Serial.printf_P(PSTR("T\n"));
else
Serial.printf_P(PSTR("F\n"));
} }
static void static void
@ -968,32 +993,6 @@ print_done()
Serial.printf_P(PSTR("\nSaved Settings\n\n")); Serial.printf_P(PSTR("\nSaved Settings\n\n"));
} }
// read at 50Hz
static bool
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
}else{
return 0;
}
}
static void zero_eeprom(void) static void zero_eeprom(void)
{ {

29
ArduCopter/system.pde
View File

@ -249,11 +249,6 @@ static void init_ardupilot()
start_new_log(); start_new_log();
} }
//#if(GROUND_START_DELAY > 0)
//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
// delay(GROUND_START_DELAY * 1000);
//#endif
GPS_enabled = false; GPS_enabled = false;
// Read in the GPS // Read in the GPS
@ -292,10 +287,8 @@ static void init_ardupilot()
// --------------------------- // ---------------------------
reset_control_switch(); reset_control_switch();
//delay(100);
startup_ground(); startup_ground();
//Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max);
Log_Write_Startup(); Log_Write_Startup();
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
@ -361,9 +354,7 @@ static void set_mode(byte mode)
// report the GPS and Motor arming status // report the GPS and Motor arming status
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction reset_nav();
//xtrack_enabled = false;
reset_nav_I();
switch(control_mode) switch(control_mode)
{ {
@ -381,13 +372,6 @@ static void set_mode(byte mode)
reset_hold_I(); reset_hold_I();
break; break;
case SIMPLE:
yaw_mode = SIMPLE_YAW;
roll_pitch_mode = SIMPLE_RP;
throttle_mode = SIMPLE_THR;
reset_hold_I();
break;
case ALT_HOLD: case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW; yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP; roll_pitch_mode = ALT_HOLD_RP;
@ -395,7 +379,7 @@ static void set_mode(byte mode)
reset_hold_I(); reset_hold_I();
init_throttle_cruise(); init_throttle_cruise();
next_WP.alt = current_loc.alt; next_WP = current_loc;
break; break;
case AUTO: case AUTO:
@ -407,12 +391,7 @@ static void set_mode(byte mode)
init_throttle_cruise(); init_throttle_cruise();
// loads the commands from where we left off // loads the commands from where we left off
//init_auto();
init_commands(); init_commands();
// do crosstrack correction
// XXX move to flight commands
//xtrack_enabled = true;
break; break;
case CIRCLE: case CIRCLE:
@ -491,7 +470,7 @@ static void set_failsafe(boolean mode)
static void resetPerfData(void) { static void resetPerfData(void) {
mainLoop_count = 0; //mainLoop_count = 0;
G_Dt_max = 0; G_Dt_max = 0;
gps_fix_count = 0; gps_fix_count = 0;
perf_mon_timer = millis(); perf_mon_timer = millis();
@ -541,7 +520,7 @@ static void
init_throttle_cruise() init_throttle_cruise()
{ {
// are we moving from manual throttle to auto_throttle? // are we moving from manual throttle to auto_throttle?
if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I(); g.pi_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in); g.throttle_cruise.set_and_save(g.rc_3.control_in);
} }