mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 08:13:56 -04:00
Removed Simple Mode
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
parent
bca934b538
commit
ae81a758de
@ -46,10 +46,17 @@
|
||||
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!
|
||||
//
|
||||
// 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
|
||||
//#define LOITER_THR THROTTLE_MANUAL
|
||||
|
||||
@ -237,7 +237,6 @@ static const char *comma = ",";
|
||||
static const char* flight_mode_strings[] = {
|
||||
"STABILIZE",
|
||||
"ACRO",
|
||||
"SIMPLE",
|
||||
"ALT_HOLD",
|
||||
"AUTO",
|
||||
"GUIDED",
|
||||
@ -275,6 +274,7 @@ static byte control_mode = STABILIZE;
|
||||
static byte old_control_mode = STABILIZE;
|
||||
static byte oldSwitchPosition; // for remembering the control mode switch
|
||||
static int motor_out[8];
|
||||
static bool do_simple = false;
|
||||
|
||||
// Heli
|
||||
// ----
|
||||
@ -338,7 +338,7 @@ static float cos_pitch_x = 1;
|
||||
static float cos_yaw_x = 1;
|
||||
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
||||
static long initial_simple_bearing; // used for Simple mode
|
||||
|
||||
static float boost; // used to give a little extra to maintain altitude
|
||||
|
||||
// Acro
|
||||
#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_loopTimeStamp; // Time Stamp when fast loop was complete
|
||||
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 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 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
|
||||
|
||||
|
||||
@ -533,12 +532,7 @@ void loop()
|
||||
//PORTK |= B00010000;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
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
|
||||
mainLoop_count++;
|
||||
|
||||
//if (delta_ms_fast_loop > 6)
|
||||
// Log_Write_Performance();
|
||||
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
@ -568,7 +562,7 @@ void loop()
|
||||
}
|
||||
|
||||
if (millis() - perf_mon_timer > 20000) {
|
||||
if (mainLoop_count != 0) {
|
||||
//if (mainLoop_count != 0) {
|
||||
|
||||
gcs.send_message(MSG_PERF_REPORT);
|
||||
|
||||
@ -576,7 +570,7 @@ void loop()
|
||||
Log_Write_Performance();
|
||||
|
||||
resetPerfData();
|
||||
}
|
||||
//}
|
||||
}
|
||||
//PORTK &= B10111111;
|
||||
}
|
||||
@ -610,7 +604,6 @@ static void fast_loop()
|
||||
// ---------------------------------------
|
||||
update_yaw_mode();
|
||||
update_roll_pitch_mode();
|
||||
update_throttle_mode();
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
@ -630,6 +623,12 @@ static void medium_loop()
|
||||
|
||||
medium_loopCounter++;
|
||||
|
||||
/*
|
||||
if(g.optflow_enabled){
|
||||
optflow.update()
|
||||
}
|
||||
*/
|
||||
|
||||
if(GPS_enabled){
|
||||
update_GPS();
|
||||
}
|
||||
@ -646,6 +645,10 @@ static void medium_loop()
|
||||
|
||||
// auto_trim, uses an auto_level algorithm
|
||||
auto_trim();
|
||||
|
||||
// record throttle output
|
||||
// ------------------------------
|
||||
throttle_integrator += g.rc_3.servo_out;
|
||||
break;
|
||||
|
||||
// This case performs some navigation computations
|
||||
@ -654,13 +657,6 @@ static void medium_loop()
|
||||
loop_step = 2;
|
||||
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:
|
||||
if(g_gps->new_data && g_gps->fix){
|
||||
loop_step = 11;
|
||||
@ -696,17 +692,14 @@ static void medium_loop()
|
||||
// --------------------------
|
||||
update_altitude();
|
||||
|
||||
// altitude smoothing
|
||||
// ------------------
|
||||
//calc_altitude_smoothing_error();
|
||||
|
||||
altitude_error = get_altitude_error();
|
||||
|
||||
//camera_stabilization();
|
||||
|
||||
// invalidate the throttle hold value
|
||||
// ----------------------------------
|
||||
invalid_throttle = true;
|
||||
|
||||
// calc boost
|
||||
// ----------
|
||||
boost = get_angle_boost();
|
||||
|
||||
break;
|
||||
|
||||
// This case deals with sending high rate telemetry
|
||||
@ -722,11 +715,13 @@ static void medium_loop()
|
||||
}
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
||||
Log_Write_Attitude();
|
||||
if(motor_armed){
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
||||
Log_Write_Attitude();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CTUN && motor_armed)
|
||||
Log_Write_Control_Tuning();
|
||||
if (g.log_bitmask & MASK_LOG_CTUN)
|
||||
Log_Write_Control_Tuning();
|
||||
}
|
||||
#endif
|
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||||
@ -785,9 +780,9 @@ static void medium_loop()
|
||||
// ---------------------------
|
||||
static void fifty_hz_loop()
|
||||
{
|
||||
// record throttle output
|
||||
// ------------------------------
|
||||
throttle_integrator += g.rc_3.servo_out;
|
||||
// moved to slower loop
|
||||
// --------------------
|
||||
update_throttle_mode();
|
||||
|
||||
// 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)
|
||||
{
|
||||
switch(yaw_mode){
|
||||
@ -1100,8 +1015,11 @@ void update_yaw_mode(void)
|
||||
|
||||
case YAW_HOLD:
|
||||
// calcualte new nav_yaw offset
|
||||
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
|
||||
//Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw);
|
||||
if (control_mode <= STABILIZE){
|
||||
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;
|
||||
|
||||
case YAW_LOOK_AT_HOME:
|
||||
@ -1136,6 +1054,29 @@ void update_roll_pitch_mode(void)
|
||||
|
||||
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){
|
||||
case ROLL_PITCH_ACRO:
|
||||
@ -1152,29 +1093,6 @@ void update_roll_pitch_mode(void)
|
||||
control_pitch = g.rc_2.control_in;
|
||||
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:
|
||||
// mix in user control with Nav control
|
||||
control_roll = g.rc_1.control_mix(nav_roll);
|
||||
@ -1194,7 +1112,7 @@ void update_throttle_mode(void)
|
||||
{
|
||||
switch(throttle_mode){
|
||||
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;
|
||||
|
||||
case THROTTLE_HOLD:
|
||||
@ -1204,16 +1122,20 @@ void update_throttle_mode(void)
|
||||
// fall through
|
||||
|
||||
case THROTTLE_AUTO:
|
||||
// 10hz, don't run up i term
|
||||
if(invalid_throttle && motor_auto_armed == true){
|
||||
// how far off are we
|
||||
altitude_error = get_altitude_error();
|
||||
|
||||
// get the AP throttle
|
||||
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
|
||||
invalid_throttle = false;
|
||||
}
|
||||
|
||||
// apply throttle control at 200 hz
|
||||
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -1237,6 +1159,11 @@ static void update_navigation()
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
wp_control = WP_MODE;
|
||||
update_auto_yaw();
|
||||
update_nav_wp();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
if(wp_distance > 4){
|
||||
// calculates desired Yaw
|
||||
@ -1251,7 +1178,6 @@ static void update_navigation()
|
||||
//xtrack_enabled = false;
|
||||
}
|
||||
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
break;
|
||||
@ -1273,8 +1199,8 @@ static void update_navigation()
|
||||
//circle_angle += dTnav; //1000 * (dTnav/1000);
|
||||
circle_angle = wrap_360(target_bearing + 2000 + 18000);
|
||||
|
||||
target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle));
|
||||
target_WP.lat = next_WP.lat + g.loiter_radius * sin(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 * 100 * sin(radians(90 - circle_angle)));
|
||||
}
|
||||
|
||||
// calc the lat and long error to the target
|
||||
@ -1379,18 +1305,13 @@ static void update_altitude()
|
||||
static void
|
||||
adjust_altitude()
|
||||
{
|
||||
alt_timer++;
|
||||
if(alt_timer >= 2){
|
||||
alt_timer = 0;
|
||||
|
||||
if(g.rc_3.control_in <= 200){
|
||||
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 = 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
|
||||
}
|
||||
if(g.rc_3.control_in <= 200){
|
||||
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 = 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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -91,7 +91,7 @@ static int
|
||||
get_nav_throttle(long z_error, int target_speed)
|
||||
{
|
||||
int rate_error;
|
||||
int throttle;
|
||||
//int throttle;
|
||||
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
|
||||
|
||||
// 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 = constrain(rate_error, -110, 110);
|
||||
|
||||
throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||
return g.throttle_cruise + throttle;
|
||||
//throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||
//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.
|
||||
// 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_lon.reset_I();
|
||||
|
||||
long_error = 0;
|
||||
lat_error = 0;
|
||||
}
|
||||
|
||||
|
||||
@ -169,11 +178,11 @@ throttle control
|
||||
|
||||
// user input:
|
||||
// -----------
|
||||
static int get_throttle(int throttle_input)
|
||||
{
|
||||
throttle_input = (float)throttle_input * angle_boost();
|
||||
return max(throttle_input, 0);
|
||||
}
|
||||
//static int get_throttle(int throttle_input)
|
||||
//{
|
||||
// throttle_input = (float)throttle_input * angle_boost();
|
||||
// return max(throttle_input, 0);
|
||||
//}
|
||||
|
||||
static long
|
||||
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
|
||||
if(yaw_input != 0){
|
||||
// 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)
|
||||
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;
|
||||
temp = 2.0 - constrain(temp, .5, 1.0);
|
||||
|
||||
@ -365,16 +365,15 @@ static void Log_Write_GPS()
|
||||
DataFlash.WriteByte(LOG_GPS_MSG);
|
||||
|
||||
DataFlash.WriteLong(g_gps->time); // 1
|
||||
DataFlash.WriteByte(g_gps->fix); // 2
|
||||
DataFlash.WriteByte(g_gps->num_sats); // 3
|
||||
DataFlash.WriteByte(g_gps->num_sats); // 2
|
||||
|
||||
DataFlash.WriteLong(current_loc.lat); // 4
|
||||
DataFlash.WriteLong(current_loc.lng); // 5
|
||||
DataFlash.WriteLong(current_loc.alt); // 7
|
||||
DataFlash.WriteLong(current_loc.lat); // 3
|
||||
DataFlash.WriteLong(current_loc.lng); // 4
|
||||
DataFlash.WriteLong(current_loc.alt); // 5
|
||||
DataFlash.WriteLong(g_gps->altitude); // 6
|
||||
|
||||
DataFlash.WriteInt(g_gps->ground_speed); // 8
|
||||
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9
|
||||
DataFlash.WriteInt(g_gps->ground_speed); // 7
|
||||
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
@ -382,21 +381,20 @@ static void Log_Write_GPS()
|
||||
// Read a GPS packet
|
||||
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, "
|
||||
"%d, %u\n"),
|
||||
|
||||
DataFlash.ReadLong(), // 1 time
|
||||
(int)DataFlash.ReadByte(), // 2 fix
|
||||
(int)DataFlash.ReadByte(), // 3 sats
|
||||
(int)DataFlash.ReadByte(), // 2 sats
|
||||
|
||||
(float)DataFlash.ReadLong() / t7, // 4 lat
|
||||
(float)DataFlash.ReadLong() / t7, // 5 lon
|
||||
(float)DataFlash.ReadLong() / 100.0, // 6 gps alt
|
||||
(float)DataFlash.ReadLong() / 100.0, // 7 sensor alt
|
||||
(float)DataFlash.ReadLong() / t7, // 3 lat
|
||||
(float)DataFlash.ReadLong() / t7, // 4 lon
|
||||
(float)DataFlash.ReadLong() / 100.0, // 5 gps alt
|
||||
(float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
|
||||
|
||||
DataFlash.ReadInt(), // 8 ground speed
|
||||
(uint16_t)DataFlash.ReadInt()); // 9 ground course
|
||||
DataFlash.ReadInt(), // 7 ground speed
|
||||
(uint16_t)DataFlash.ReadInt()); // 8 ground course
|
||||
}
|
||||
|
||||
|
||||
@ -514,7 +512,7 @@ static void Log_Write_Optflow()
|
||||
}
|
||||
#endif
|
||||
|
||||
// Read an attitude packet
|
||||
|
||||
static void Log_Read_Optflow()
|
||||
{
|
||||
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(LOG_CONTROL_TUNING_MSG);
|
||||
|
||||
// Control
|
||||
//DataFlash.WriteInt((int)(g.rc_4.control_in/100));
|
||||
//DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
|
||||
|
||||
// yaw
|
||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
|
||||
DataFlash.WriteInt((int)(nav_yaw/100));
|
||||
DataFlash.WriteInt((int)yaw_error/100);
|
||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
|
||||
DataFlash.WriteInt((int)(nav_yaw/100)); //2
|
||||
DataFlash.WriteInt((int)yaw_error/100); //3
|
||||
|
||||
// Alt hold
|
||||
DataFlash.WriteInt(g.rc_3.servo_out);
|
||||
DataFlash.WriteInt(sonar_alt); //
|
||||
DataFlash.WriteInt(baro_alt); //
|
||||
DataFlash.WriteInt(g.rc_3.servo_out); //4
|
||||
DataFlash.WriteInt(sonar_alt); //5
|
||||
DataFlash.WriteInt(baro_alt); //6
|
||||
|
||||
DataFlash.WriteInt((int)next_WP.alt); //
|
||||
//DataFlash.WriteInt((int)altitude_error); //
|
||||
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
|
||||
DataFlash.WriteInt((int)next_WP.alt); //7
|
||||
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
@ -635,10 +628,9 @@ static void Log_Write_Control_Tuning()
|
||||
static void Log_Read_Control_Tuning()
|
||||
{
|
||||
Serial.printf_P(PSTR( "CTUN, "
|
||||
//"%d, %d, "
|
||||
"%d, %d, %d, "
|
||||
"%d, %d, %d, "
|
||||
"%d, %d, %d\n"),
|
||||
"%d, %d\n"),
|
||||
|
||||
// Control
|
||||
DataFlash.ReadInt(),
|
||||
@ -670,21 +662,18 @@ static void Log_Write_Performance()
|
||||
|
||||
|
||||
//*
|
||||
DataFlash.WriteLong( millis()- perf_mon_timer);
|
||||
DataFlash.WriteInt ( mainLoop_count);
|
||||
DataFlash.WriteInt ( G_Dt_max);
|
||||
//DataFlash.WriteLong( millis()- perf_mon_timer);
|
||||
//DataFlash.WriteInt ( mainLoop_count);
|
||||
DataFlash.WriteInt ( G_Dt_max); //1
|
||||
|
||||
DataFlash.WriteByte( dcm.gyro_sat_count);
|
||||
DataFlash.WriteByte( imu.adc_constraints);
|
||||
DataFlash.WriteByte( dcm.renorm_sqrt_count);
|
||||
DataFlash.WriteByte( dcm.renorm_blowup_count);
|
||||
DataFlash.WriteByte( gps_fix_count);
|
||||
DataFlash.WriteByte( dcm.gyro_sat_count); //2
|
||||
DataFlash.WriteByte( imu.adc_constraints); //3
|
||||
DataFlash.WriteByte( dcm.renorm_sqrt_count); //4
|
||||
DataFlash.WriteByte( dcm.renorm_blowup_count); //5
|
||||
DataFlash.WriteByte( gps_fix_count); //6
|
||||
|
||||
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000));
|
||||
DataFlash.WriteLong ( throttle_integrator);
|
||||
|
||||
//*/
|
||||
//PM, 20005, 3742, 10,0,0,0,0,89,1000,
|
||||
DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7
|
||||
DataFlash.WriteLong ( throttle_integrator); //8
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
@ -692,24 +681,23 @@ static void Log_Write_Performance()
|
||||
// Read a performance packet
|
||||
static void Log_Read_Performance()
|
||||
{
|
||||
Serial.printf_P(PSTR( "PM, %ld, %d, %d, "
|
||||
"%d, %d, %d, %d, %d, "
|
||||
Serial.printf_P(PSTR( "PM, %d, %d, %d, "
|
||||
"%d, %d, %d, "
|
||||
"%d, %ld\n"),
|
||||
|
||||
// Control
|
||||
DataFlash.ReadLong(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
//DataFlash.ReadLong(),
|
||||
//DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(), //1
|
||||
DataFlash.ReadByte(), //2
|
||||
DataFlash.ReadByte(), //3
|
||||
|
||||
DataFlash.ReadByte(),
|
||||
DataFlash.ReadByte(),
|
||||
DataFlash.ReadByte(), //4
|
||||
DataFlash.ReadByte(), //5
|
||||
DataFlash.ReadByte(), //6
|
||||
|
||||
DataFlash.ReadByte(),
|
||||
DataFlash.ReadByte(),
|
||||
DataFlash.ReadByte(),
|
||||
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadLong());
|
||||
DataFlash.ReadInt(), //7
|
||||
DataFlash.ReadLong()); //8
|
||||
}
|
||||
|
||||
// Write a command processing packet.
|
||||
@ -760,19 +748,28 @@ static void Log_Write_Attitude()
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
|
||||
|
||||
DataFlash.WriteInt((int)dcm.roll_sensor);
|
||||
DataFlash.WriteInt((int)dcm.pitch_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);
|
||||
}
|
||||
|
||||
// Read an attitude packet
|
||||
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(),
|
||||
(uint16_t)DataFlash.ReadInt());
|
||||
(uint16_t)DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt());
|
||||
}
|
||||
|
||||
// Write a mode packet. Total length : 5 bytes
|
||||
@ -827,6 +824,8 @@ static void Log_Read(int start_page, int end_page)
|
||||
case 0:
|
||||
if(data == HEAD_BYTE1) // Head byte 1
|
||||
log_step++;
|
||||
else
|
||||
Serial.println(".");
|
||||
break;
|
||||
|
||||
case 1:
|
||||
|
||||
@ -83,7 +83,6 @@ public:
|
||||
// 160: Navigation parameters
|
||||
//
|
||||
k_param_crosstrack_entry_angle = 160,
|
||||
k_param_pitch_max,
|
||||
k_param_RTL_altitude,
|
||||
|
||||
//
|
||||
@ -139,7 +138,7 @@ public:
|
||||
k_param_flight_mode4,
|
||||
k_param_flight_mode5,
|
||||
k_param_flight_mode6,
|
||||
|
||||
k_param_simple_modes,
|
||||
|
||||
//
|
||||
// 220: Waypoint data
|
||||
@ -179,7 +178,7 @@ public:
|
||||
//
|
||||
AP_Int16 sysid_this_mav;
|
||||
AP_Int16 sysid_my_gcs;
|
||||
AP_Int8 serial3_baud;
|
||||
AP_Int8 serial3_baud;
|
||||
|
||||
|
||||
// Crosstrack navigation
|
||||
@ -213,6 +212,7 @@ public:
|
||||
AP_Int8 flight_mode4;
|
||||
AP_Int8 flight_mode5;
|
||||
AP_Int8 flight_mode6;
|
||||
AP_Int8 simple_modes;
|
||||
|
||||
// Radio settings
|
||||
//
|
||||
@ -221,8 +221,6 @@ public:
|
||||
//AP_Var_group pwm_throttle;
|
||||
//AP_Var_group pwm_yaw;
|
||||
|
||||
AP_Int16 pitch_max;
|
||||
|
||||
// Misc
|
||||
//
|
||||
AP_Int16 log_bitmask;
|
||||
@ -324,8 +322,7 @@ public:
|
||||
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
|
||||
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
|
||||
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
|
||||
|
||||
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
|
||||
simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")),
|
||||
|
||||
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")),
|
||||
|
||||
@ -1,18 +1,5 @@
|
||||
// -*- 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()
|
||||
{
|
||||
// zero is home, but we always load the next command (1), in the code.
|
||||
|
||||
@ -311,13 +311,10 @@ static void do_loiter_turns()
|
||||
|
||||
static void do_loiter_time()
|
||||
{
|
||||
///*
|
||||
wp_control = LOITER_MODE;
|
||||
wp_control = LOITER_MODE;
|
||||
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);
|
||||
//*/
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -464,11 +461,12 @@ static void do_change_alt()
|
||||
{
|
||||
Location temp = next_WP;
|
||||
condition_start = current_loc.alt;
|
||||
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
|
||||
condition_value = next_command.alt + home.alt;
|
||||
} else {
|
||||
condition_value = next_command.alt;
|
||||
}
|
||||
condition_value = next_command.alt;
|
||||
//if (next_command.options & WP_OPTION_ALT_RELATIVE) {
|
||||
// condition_value = next_command.alt + home.alt;
|
||||
//} else {
|
||||
|
||||
//}
|
||||
temp.alt = condition_value;
|
||||
set_next_WP(&temp);
|
||||
}
|
||||
|
||||
@ -579,7 +579,7 @@
|
||||
# define LOG_ATTITUDE_FAST DISABLED
|
||||
#endif
|
||||
#ifndef LOG_ATTITUDE_MED
|
||||
# define LOG_ATTITUDE_MED DISABLED
|
||||
# define LOG_ATTITUDE_MED ENABLED
|
||||
#endif
|
||||
#ifndef LOG_GPS
|
||||
# define LOG_GPS ENABLED
|
||||
|
||||
@ -13,6 +13,12 @@ static void read_control_switch()
|
||||
switch_debouncer = false;
|
||||
|
||||
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{
|
||||
switch_debouncer = true;
|
||||
}
|
||||
@ -47,6 +53,10 @@ static void read_trim_switch()
|
||||
do_flip = true;
|
||||
}
|
||||
|
||||
#elif CH7_OPTION == SIMPLE_MODE_CONTROL
|
||||
|
||||
do_simple = (g.rc_7.control_in > 800);
|
||||
|
||||
#elif CH7_OPTION == DO_SET_HOVER
|
||||
// switch is engaged
|
||||
if (g.rc_7.control_in > 800){
|
||||
|
||||
@ -30,6 +30,7 @@
|
||||
// CH 7 control
|
||||
#define DO_SET_HOVER 0
|
||||
#define DO_FLIP 1
|
||||
#define SIMPLE_MODE_CONTROL 2
|
||||
|
||||
// Frame types
|
||||
#define QUAD_FRAME 0
|
||||
@ -121,14 +122,20 @@
|
||||
// ----------------
|
||||
#define STABILIZE 0 // hold level position
|
||||
#define ACRO 1 // rate control
|
||||
#define SIMPLE 2 //
|
||||
#define ALT_HOLD 3 // AUTO control
|
||||
#define AUTO 4 // AUTO control
|
||||
#define GUIDED 5 // AUTO control
|
||||
#define LOITER 6 // Hold a single location
|
||||
#define RTL 7 // AUTO control
|
||||
#define CIRCLE 8 // AUTO control
|
||||
#define NUM_MODES 9
|
||||
#define ALT_HOLD 2 // AUTO control
|
||||
#define AUTO 3 // AUTO control
|
||||
#define GUIDED 4 // AUTO control
|
||||
#define LOITER 5 // Hold a single location
|
||||
#define RTL 6 // AUTO control
|
||||
#define CIRCLE 7 // AUTO control
|
||||
#define NUM_MODES 8
|
||||
|
||||
#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
|
||||
// -----------
|
||||
|
||||
@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
report_flight_modes();
|
||||
report_imu();
|
||||
report_compass();
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
report_optflow();
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
report_heli();
|
||||
report_gyro();
|
||||
@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
byte _oldSwitchPosition = 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();
|
||||
|
||||
while(1){
|
||||
@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
mode = constrain(mode, 0, NUM_MODES-1);
|
||||
|
||||
// update the user
|
||||
print_switch(_switchPosition, mode);
|
||||
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
||||
|
||||
// Remember switch position
|
||||
_oldSwitchPosition = _switchPosition;
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
if (radio_input_switch() == true){
|
||||
if (abs(g.rc_1.control_in) > 3000){
|
||||
mode++;
|
||||
if(mode >= NUM_MODES)
|
||||
mode = 0;
|
||||
@ -334,13 +336,31 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
flight_modes[_switchPosition] = 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
|
||||
if(Serial.available() > 0){
|
||||
for (mode=0; mode<6; mode++)
|
||||
for (mode = 0; mode < 6; mode++)
|
||||
flight_modes[mode].save();
|
||||
|
||||
print_done();
|
||||
report_flight_modes();
|
||||
return (0);
|
||||
@ -872,7 +892,7 @@ static void report_flight_modes()
|
||||
print_divider();
|
||||
|
||||
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);
|
||||
}
|
||||
@ -956,10 +976,15 @@ print_radio_values()
|
||||
}
|
||||
|
||||
static void
|
||||
print_switch(byte p, byte m)
|
||||
print_switch(byte p, byte m, bool b)
|
||||
{
|
||||
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
|
||||
@ -968,32 +993,6 @@ print_done()
|
||||
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)
|
||||
{
|
||||
|
||||
@ -249,11 +249,6 @@ static void init_ardupilot()
|
||||
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;
|
||||
|
||||
// Read in the GPS
|
||||
@ -292,10 +287,8 @@ static void init_ardupilot()
|
||||
// ---------------------------
|
||||
reset_control_switch();
|
||||
|
||||
//delay(100);
|
||||
startup_ground();
|
||||
|
||||
//Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max);
|
||||
Log_Write_Startup();
|
||||
|
||||
SendDebug("\nReady to FLY ");
|
||||
@ -361,9 +354,7 @@ static void set_mode(byte mode)
|
||||
// report the GPS and Motor arming status
|
||||
led_mode = NORMAL_LEDS;
|
||||
|
||||
// most modes do not calculate crosstrack correction
|
||||
//xtrack_enabled = false;
|
||||
reset_nav_I();
|
||||
reset_nav();
|
||||
|
||||
switch(control_mode)
|
||||
{
|
||||
@ -381,13 +372,6 @@ static void set_mode(byte mode)
|
||||
reset_hold_I();
|
||||
break;
|
||||
|
||||
case SIMPLE:
|
||||
yaw_mode = SIMPLE_YAW;
|
||||
roll_pitch_mode = SIMPLE_RP;
|
||||
throttle_mode = SIMPLE_THR;
|
||||
reset_hold_I();
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
yaw_mode = ALT_HOLD_YAW;
|
||||
roll_pitch_mode = ALT_HOLD_RP;
|
||||
@ -395,7 +379,7 @@ static void set_mode(byte mode)
|
||||
reset_hold_I();
|
||||
|
||||
init_throttle_cruise();
|
||||
next_WP.alt = current_loc.alt;
|
||||
next_WP = current_loc;
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
@ -407,12 +391,7 @@ static void set_mode(byte mode)
|
||||
init_throttle_cruise();
|
||||
|
||||
// loads the commands from where we left off
|
||||
//init_auto();
|
||||
init_commands();
|
||||
|
||||
// do crosstrack correction
|
||||
// XXX move to flight commands
|
||||
//xtrack_enabled = true;
|
||||
break;
|
||||
|
||||
case CIRCLE:
|
||||
@ -491,7 +470,7 @@ static void set_failsafe(boolean mode)
|
||||
|
||||
|
||||
static void resetPerfData(void) {
|
||||
mainLoop_count = 0;
|
||||
//mainLoop_count = 0;
|
||||
G_Dt_max = 0;
|
||||
gps_fix_count = 0;
|
||||
perf_mon_timer = millis();
|
||||
@ -541,7 +520,7 @@ static void
|
||||
init_throttle_cruise()
|
||||
{
|
||||
// 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.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user