Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-15 20:34:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fixes Alt hold mistake

Browse Source 
Moves tuning into runtime definable param
Added more IMax to throttle

Signed-off-by: Jason Short <jasonshort@gmail.com>
This commit is contained in:
Jason Short 2011-09-10 15:16:51 -07:00
parent 6e5a9c49b2
commit 02bfd43390
9 changed files with 99 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
ArduCopter/ArduCopter.pde
View File

@ -296,6 +296,7 @@ static boolean motor_auto_armed; // if true,
//int max_stabilize_dampener; // //int max_stabilize_dampener; //
//int max_yaw_dampener; // //int max_yaw_dampener; //
static Vector3f omega; static Vector3f omega;
float tuning_value;
// LED output // LED output
// ---------- // ----------
@ -507,7 +508,7 @@ static byte simple_timer; // for limiting the execution of flight mode thi
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
static float load; // % MCU cycles used //static float load; // % MCU cycles used
static byte counter_one_herz; static byte counter_one_herz;
static bool GPS_enabled = false; static bool GPS_enabled = false;
@ -530,7 +531,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; //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++; mainLoop_count++;
@ -907,16 +908,15 @@ static void slow_loop()
// between 1 and 5 Hz // between 1 and 5 Hz
#else #else
gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100); //gcs.send_message(MSG_CPU_LOAD, load*100);
#endif #endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(1,5); hil.data_stream_send(1,5);
#endif #endif
#if CHANNEL_6_TUNING != CH6_NONE if(g.radio_tuning > 0)
tuning(); tuning();
#endif
// filter out the baro offset. // filter out the baro offset.
//if(baro_alt_offset > 0) baro_alt_offset--; //if(baro_alt_offset > 0) baro_alt_offset--;
@ -1301,7 +1301,7 @@ static void read_AHRS(void)
hil.update(); hil.update();
#endif #endif
dcm.update_DCM(G_Dt); dcm.update_DCM(G_Dt);//, _tog);
omega = dcm.get_gyro(); omega = dcm.get_gyro();
} }
@ -1396,63 +1396,65 @@ adjust_altitude()
} }
static void tuning(){ static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0;
//Outer Loop : Attitude switch(g.radio_tuning){
#if CHANNEL_6_TUNING == CH6_STABILIZE_KP case CH6_STABILIZE_KP:
g.pi_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(0,8000); // 0 to 8
g.pi_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_roll.kP(tuning_value);
g.pi_stabilize_pitch.kP(tuning_value);
break;
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI case CH6_STABILIZE_KI:
g.pi_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(0,300); // 0 to .3
g.pi_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); tuning_value = (float)g.rc_6.control_in / 1000.0;
g.pi_stabilize_roll.kI(tuning_value);
g.pi_stabilize_pitch.kI(tuning_value);
break;
#elif CHANNEL_6_TUNING == CH6_YAW_KP case CH6_RATE_KP:
g.pi_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 g.rc_6.set_range(0,300); // 0 to .3
g.pi_rate_roll.kP(tuning_value);
g.pi_rate_pitch.kP(tuning_value);
break;
#elif CHANNEL_6_TUNING == CH6_YAW_KI case CH6_RATE_KI:
g.pi_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(0,300); // 0 to .3
g.pi_rate_roll.kI(tuning_value);
g.pi_rate_pitch.kI(tuning_value);
break;
case CH6_YAW_KP:
g.rc_6.set_range(0,1000);
g.pi_stabilize_yaw.kP(tuning_value);
break;
//Inner Loop : Rate case CH6_YAW_RATE_KP:
#elif CHANNEL_6_TUNING == CH6_RATE_KP g.rc_6.set_range(0,1000);
g.pi_rate_roll.kP((float)g.rc_6.control_in / 1000.0); g.pi_rate_yaw.kP(tuning_value);
g.pi_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); break;
#elif CHANNEL_6_TUNING == CH6_RATE_KI case CH6_THROTTLE_KP:
g.pi_rate_roll.kI((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(0,1000);
g.pi_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); g.pi_throttle.kP(tuning_value);
break;
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP case CH6_TOP_BOTTOM_RATIO:
g.pi_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(800,1000); // .8 to 1
g.top_bottom_ratio = tuning_value;
break;
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI case CH6_RELAY:
g.pi_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); g.rc_6.set_range(0,1000);
//Altitude Hold
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
g.pi_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
g.pi_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1
//Extras
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED
g.waypoint_speed_max = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_PMAX
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
// Simple relay control
#elif CHANNEL_6_TUNING == CH6_RELAY
if (g.rc_6.control_in <= 600) relay_on(); if (g.rc_6.control_in <= 600) relay_on();
if (g.rc_6.control_in >= 400) relay_off(); if (g.rc_6.control_in >= 400) relay_off();
break;
#endif case CH6_TRAVERSE_SPEED:
g.rc_6.set_range(0,1000);
g.waypoint_speed_max = g.rc_6.control_in;
break;
}
} }
static void update_nav_wp() static void update_nav_wp()

2
ArduCopter/Attitude.pde
View File

@ -102,7 +102,7 @@ get_nav_throttle(long z_error, int target_speed)
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 + rate_error; return g.throttle_cruise + throttle;
} }

2
ArduCopter/Mavlink_Common.h
View File

@ -87,7 +87,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
mode, mode,
nav_mode, nav_mode,
status, status,
load * 1000, 0,
battery_voltage * 1000, battery_voltage * 1000,
battery_remaining, battery_remaining,
packet_drops); packet_drops);

4
ArduCopter/Parameters.h
View File

@ -105,6 +105,7 @@ public:
k_param_throttle_fs_value, k_param_throttle_fs_value,
k_param_throttle_cruise, k_param_throttle_cruise,
k_param_esc_calibrate, k_param_esc_calibrate,
k_param_radio_tuning,
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
@ -231,6 +232,8 @@ public:
AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled; AP_Int8 compass_enabled;
AP_Int8 esc_calibrate; AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int8 frame_orientation; AP_Int8 frame_orientation;
AP_Float top_bottom_ratio; AP_Float top_bottom_ratio;
AP_Int8 optflow_enabled; AP_Int8 optflow_enabled;
@ -324,6 +327,7 @@ public:
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")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),

4
ArduCopter/config.h
View File

@ -513,7 +513,7 @@
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.35 // # define THROTTLE_P 0.6 //
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup # define THROTTLE_I 0.10 // with 4m error, 12.5s windup
@ -522,7 +522,7 @@
//# define THROTTLE_D 0.6 // upped with filter //# define THROTTLE_D 0.6 // upped with filter
//#endif //#endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 150 # define THROTTLE_IMAX 300
#endif #endif

19
ArduCopter/defines.h
View File

@ -138,13 +138,6 @@
#define CIRCLE 8 // AUTO control #define CIRCLE 8 // AUTO control
#define NUM_MODES 9 #define NUM_MODES 9
// YAW debug
// ---------
#define YAW_HOLD 0
#define YAW_BRAKE 1
#define YAW_RATE 2
// CH_6 Tuning // CH_6 Tuning
// ----------- // -----------
#define CH6_NONE 0 #define CH6_NONE 0
@ -152,20 +145,16 @@
#define CH6_STABILIZE_KP 1 #define CH6_STABILIZE_KP 1
#define CH6_STABILIZE_KI 2 #define CH6_STABILIZE_KI 2
#define CH6_YAW_KP 3 #define CH6_YAW_KP 3
#define CH6_YAW_KD 4
// Rate // Rate
#define CH6_RATE_KP 5 #define CH6_RATE_KP 5
#define CH6_RATE_KI 6 #define CH6_RATE_KI 6
#define CH6_YAW_RATE_KP 7 #define CH6_YAW_RATE_KP 7
#define CH6_YAW_RATE_KD 8
// Altitude // Altitude
#define CH6_THROTTLE_KP 9 #define CH6_THROTTLE_KP 8
#define CH6_THROTTLE_KD 10
// Extras // Extras
#define CH6_TOP_BOTTOM_RATIO 11 #define CH6_TOP_BOTTOM_RATIO 9
#define CH6_PMAX 12 #define CH6_RELAY 10
#define CH6_RELAY 13 #define CH6_TRAVERSE_SPEED 11
#define CH6_TRAVERSE_SPEED 14
// nav byte mask // nav byte mask
// ------------- // -------------

24
ArduCopter/radio.pde
View File

@ -35,30 +35,6 @@ static void init_rc_in()
g.rc_7.set_range(0,1000); g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);
#if CHANNEL_6_TUNING == CH6_RATE_KP
g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_RATE_KI
g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
g.rc_6.set_range(0,8000); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
g.rc_6.set_range(0,800); // 0 to .8
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
g.rc_6.set_range(0,500); // 0 to .5
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
g.rc_6.set_range(800,1000); // .8 to 1
/* #elif CHANNEL_6_TUNING == CH6_RELAY
g.rc_6.set_range(0,1000); // 0 to 1 */
#endif
} }
static void init_rc_out() static void init_rc_out()

23
ArduCopter/setup.pde
View File

@ -13,6 +13,7 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_tune (uint8_t argc, const Menu::arg *argv);
//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
@ -41,6 +42,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"battery", setup_batt_monitor}, {"battery", setup_batt_monitor},
{"sonar", setup_sonar}, {"sonar", setup_sonar},
{"compass", setup_compass}, {"compass", setup_compass},
{"tune", setup_tune},
// {"offsets", setup_mag_offset}, // {"offsets", setup_mag_offset},
{"declination", setup_declination}, {"declination", setup_declination},
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
@ -354,6 +356,15 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
static int8_t
setup_tune(uint8_t argc, const Menu::arg *argv)
{
g.radio_tuning.set_and_save(argv[1].i);
report_tuning();
return 0;
}
static int8_t static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv) setup_erase(uint8_t argc, const Menu::arg *argv)
@ -1131,3 +1142,15 @@ static void report_version()
print_blanks(2); print_blanks(2);
} }
static void report_tuning()
{
Serial.printf_P(PSTR("\nTUNE:\n"));
print_divider();
if (g.radio_tuning == 0){
print_enabled(g.radio_tuning.get());
}else{
Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get());
}
print_blanks(2);
}

57
ArduCopter/test.pde
View File

@ -636,59 +636,8 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
while(1){ while(1){
delay(200); delay(200);
read_radio(); read_radio();
tuning();
//Outer Loop : Attitude Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value);
#if CHANNEL_6_TUNING == CH6_NONE
Serial.printf_P(PSTR("disabled\n"));
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_KP
Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_YAW_KI
Serial.printf_P(PSTR("yaw Hold kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Inner Loop : Rate
#elif CHANNEL_6_TUNING == CH6_RATE_KP
Serial.printf_P(PSTR("rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_RATE_KI
Serial.printf_P(PSTR("rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI
Serial.printf_P(PSTR("yaw rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Altitude Hold
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
Serial.printf_P(PSTR("throttle kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED
Serial.printf_P(PSTR("traverse: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Extras
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_PMAX
Serial.printf_P(PSTR("Y6: %d\n"), (g.rc_6.control_in * 2));
#elif CHANNEL_6_TUNING == CH6_RELAY
Serial.printf_P(PSTR(" %d\n"), (g.rc_6.control_in ));
#endif
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -1009,10 +958,12 @@ test_mission(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
} }
*/ */
static void print_hit_enter() static void print_hit_enter()
{ {
Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
} }
/* /*
static void fake_out_gps() static void fake_out_gps()
{ {