From 2de9e6a9295028e68a226a0ab7daa7bee4bbad90 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Tue, 27 Sep 2011 07:30:28 +0800 Subject: [PATCH 1/4] heli hil --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/Mavlink_Common.h | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f574099119..6f4d76e752 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -769,7 +769,7 @@ static void fifty_hz_loop() sonar_alt = sonar.read(); } - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED + #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME // HIL for a copter needs very fast update of the servo values hil.send_message(MSG_RADIO_OUT); #endif diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index fea60d35c4..9bc57ca801 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -135,6 +135,23 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) const uint8_t rssi = 1; // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers + + #if FRAME_CONFIG == HELI_FRAME + + mavlink_msg_rc_channels_scaled_send( + chan, + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out + 0, + 0, + 0, + 0, + rssi); + + #else + mavlink_msg_rc_channels_scaled_send( chan, 10000 * g.rc_1.norm_output(), @@ -146,6 +163,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) 0, 0, rssi); + + #endif } static void NOINLINE send_radio_in(mavlink_channel_t chan) From 3e09ed08fbda9ce60b79bf2287af9f0442a27abb Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Tue, 27 Sep 2011 07:34:32 +0800 Subject: [PATCH 2/4] heli hil config --- ArduCopter/GCS_Mavlink.pde | 2 +- ArduCopter/Mavlink_Common.h | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 9dd7ee4a32..8e471c9bc9 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -989,6 +989,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } #endif +*/ #if HIL_MODE == HIL_MODE_SENSORS case MAVLINK_MSG_ID_RAW_IMU: @@ -1036,7 +1037,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } #endif // HIL_MODE -*/ } // end switch } // end handle mavlink diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 9bc57ca801..00a2a79e67 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -154,14 +154,14 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) mavlink_msg_rc_channels_scaled_send( chan, - 10000 * g.rc_1.norm_output(), + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 10000 * g.rc_1.norm_output(), 10000 * g.rc_2.norm_output(), 10000 * g.rc_3.norm_output(), 10000 * g.rc_4.norm_output(), - 0, - 0, - 0, - 0, rssi); #endif From e0714ec88c6be68eddfecb9af359791b3483c487 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Tue, 27 Sep 2011 07:36:12 +0800 Subject: [PATCH 3/4] heli config --- ArduCopter/APM_Config.h | 44 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e95fdddeb1..ccd7e5c548 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -6,11 +6,11 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define GCS_PROTOCOL GCS_PROTOCOL_NONE -//#define HIL_MODE HIL_MODE_ATTITUDE +#define HIL_MODE HIL_MODE_ATTITUDE //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) -#define FRAME_CONFIG QUAD_FRAME +#define FRAME_CONFIG HELI_FRAME /* options: QUAD_FRAME @@ -48,3 +48,43 @@ //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 + + +#define AUTO_RESET_LOITER 0 +#define FRAME_CONFIG HELI_FRAME + +// DEFAULT PIDS + +// roll +#define STABILIZE_ROLL_P 0.70 +#define STABILIZE_ROLL_I 0.025 +#define STABILIZE_ROLL_D 0.04 +#define STABILIZE_ROLL_IMAX 7 + +//pitch +#define STABILIZE_PITCH_P 0.70 +#define STABILIZE_PITCH_I 0.025 +#define STABILIZE_PITCH_D 0.04 +#define STABILIZE_PITCH_IMAX 7 + +// yaw stablise +#define STABILIZE_YAW_P 0.7 +#define STABILIZE_YAW_I 0.02 +#define STABILIZE_YAW_D 0.0 + +// yaw rate +#define RATE_YAW_P 0.135 +#define RATE_YAW_I 0.0 +#define RATE_YAW_D 0.0 + +// throttle +#define THROTTLE_P 0.2 +#define THROTTLE_I 0.001 +#define THROTTLE_IMAX 100 + +// navigation +#define NAV_LOITER_P 1.1 +#define NAV_LOITER_I 0.03 +#define NAV_LOITER_D 0.02 +#define NAV_LOITER_IMAX 10 + From 3d67018cb9ef87c0cfa924e72947ad3e0880a3c8 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 8 Oct 2011 21:13:53 +0800 Subject: [PATCH 4/4] undo config change --- ArduCopter/APM_Config.h | 44 ++----------------------------------- ArduCopter/Mavlink_Common.h | 2 +- 2 files changed, 3 insertions(+), 43 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 802c4c0881..4f5574ab86 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -6,11 +6,11 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define GCS_PROTOCOL GCS_PROTOCOL_NONE -#define HIL_MODE HIL_MODE_ATTITUDE +//#define HIL_MODE HIL_MODE_ATTITUDE //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) -#define FRAME_CONFIG HELI_FRAME +#define FRAME_CONFIG QUAD_FRAME /* options: QUAD_FRAME @@ -53,43 +53,3 @@ //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 - - -#define AUTO_RESET_LOITER 0 -#define FRAME_CONFIG HELI_FRAME - -// DEFAULT PIDS - -// roll -#define STABILIZE_ROLL_P 0.70 -#define STABILIZE_ROLL_I 0.025 -#define STABILIZE_ROLL_D 0.04 -#define STABILIZE_ROLL_IMAX 7 - -//pitch -#define STABILIZE_PITCH_P 0.70 -#define STABILIZE_PITCH_I 0.025 -#define STABILIZE_PITCH_D 0.04 -#define STABILIZE_PITCH_IMAX 7 - -// yaw stablise -#define STABILIZE_YAW_P 0.7 -#define STABILIZE_YAW_I 0.02 -#define STABILIZE_YAW_D 0.0 - -// yaw rate -#define RATE_YAW_P 0.135 -#define RATE_YAW_I 0.0 -#define RATE_YAW_D 0.0 - -// throttle -#define THROTTLE_P 0.2 -#define THROTTLE_I 0.001 -#define THROTTLE_IMAX 100 - -// navigation -#define NAV_LOITER_P 1.1 -#define NAV_LOITER_I 0.03 -#define NAV_LOITER_D 0.02 -#define NAV_LOITER_IMAX 10 - diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 00a2a79e67..e2f4531fb4 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -143,7 +143,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, - g.rc_4.servo_out + g.rc_4.servo_out, 0, 0, 0,