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
This commit is contained in:
tridge60@gmail.com 2011-03-21 07:30:38 +00:00
parent 828f97f474
commit c8fd23ac45
2 changed files with 32 additions and 15 deletions

View File

@ -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

View File

@ -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;
}
}