From c8fd23ac4582823b4b483ff6a9ab719a5f72d620 Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Mon, 21 Mar 2011 07:30:38 +0000 Subject: [PATCH] allow configuration of motor arming condition This allows the user to configure their own motor arming condition in APM_Config.h. For example, the user could set it up for MODE1 transmitters, by using: #define MOTOR_ARM_CONDITION (g.rc_3.control_in == 0 && g.rc_1.control_in > 2700) #define MOTOR_DISARM_CONDITION (g.rc_3.control_in == 0 && g.rc_1.control_in < -2700) You could also set it up to use the "throttle cut" switch on a transmitter, by mapping that to a spare channel. The default (throttle minimum and maximum/minimum yaw) is the same as it was before This patch also adds reporting of motor arming to the GCS, which can be useful for testing. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1802 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/config.h | 13 +++++++++++++ ArduCopterMega/motors.pde | 34 +++++++++++++++++++--------------- 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index d669500c6e..f308697808 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -558,3 +558,16 @@ #ifndef ALLOW_RC_OVERRIDE # define ALLOW_RC_OVERRIDE DISABLED #endif + +////////////////////////////////////////////////////////////////////////////// +// Motor arming +// +// The default is for MODE2 setups. We wait for throttle to reach +// zero, and rudder to be at maximum extent. +#ifndef MOTOR_ARM_CONDITION +# define MOTOR_ARM_CONDITION (g.rc_3.control_in == 0 && g.rc_4.control_in > 2700) +#endif + +#ifndef MOTOR_DISARM_CONDITION +# define MOTOR_DISARM_CONDITION (g.rc_3.control_in == 0 && g.rc_4.control_in < -2700) +#endif diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 386136396b..cf0a6f98eb 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -6,24 +6,28 @@ void arm_motors() { static byte arming_counter; - + // Serial.printf("rc3: %u rc4: %u\n", g.rc_3.control_in, g.rc_4.control_in); // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (g.rc_3.control_in == 0){ - if (g.rc_4.control_in > 2700) { - if (arming_counter > ARM_DELAY) { - motor_armed = true; - } else{ - arming_counter++; + if (MOTOR_ARM_CONDITION) { + if (arming_counter > ARM_DELAY) { + if (!motor_armed) { + gcs.send_text(SEVERITY_MEDIUM, "Arming motors"); } - }else if (g.rc_4.control_in < -2700) { - if (arming_counter > DISARM_DELAY){ - motor_armed = false; - }else{ - arming_counter++; - } - }else{ - arming_counter = 0; + motor_armed = true; + } else{ + arming_counter++; } + } else if (MOTOR_DISARM_CONDITION) { + if (arming_counter > DISARM_DELAY){ + if (motor_armed) { + gcs.send_text(SEVERITY_MEDIUM, "Dis-arming motors"); + } + motor_armed = false; + } else { + arming_counter++; + } + } else { + arming_counter = 0; } }