mirror of https://github.com/ArduPilot/ardupilot
Copter: bug fix for auto_armed logic
Zero throttle when switching into an auto flight mode would cause the motors to stop.
This commit is contained in:
parent
68051f3ea6
commit
d2bd818b2d
|
@ -365,7 +365,7 @@ static union {
|
|||
|
||||
uint8_t low_battery : 1; // 5 // Used to track if the battery is low - LED output flashes when the batt is low
|
||||
uint8_t armed : 1; // 6
|
||||
uint8_t auto_armed : 1; // 7
|
||||
uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised
|
||||
|
||||
uint8_t failsafe_radio : 1; // 8 // A status flag for the radio failsafe
|
||||
uint8_t failsafe_batt : 1; // 9 // A status flag for the battery failsafe
|
||||
|
@ -1165,6 +1165,9 @@ static void fifty_hz_loop()
|
|||
edf_toy();
|
||||
#endif
|
||||
|
||||
// check auto_armed status
|
||||
update_auto_armed();
|
||||
|
||||
#ifdef USERHOOK_50HZLOOP
|
||||
USERHOOK_50HZLOOP
|
||||
#endif
|
||||
|
@ -1967,7 +1970,7 @@ void update_throttle_mode(void)
|
|||
|
||||
case THROTTLE_AUTO:
|
||||
// auto pilot altitude controller with target altitude held in wp_nav.get_desired_alt()
|
||||
if(motors.auto_armed() == true) {
|
||||
if(ap.auto_armed) {
|
||||
get_throttle_althold_with_slew(wp_nav.get_desired_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max);
|
||||
set_target_alt_for_reporting(wp_nav.get_desired_alt()); // To-Do: return get_destination_alt if we are flying to a waypoint
|
||||
}
|
||||
|
|
|
@ -327,7 +327,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
|||
#else
|
||||
#if X_PLANE == ENABLED
|
||||
/* update by JLN for X-Plane HIL */
|
||||
if(motors.armed() && motors.auto_armed()) {
|
||||
if(motors.armed() && ap.auto_armed) {
|
||||
mavlink_msg_rc_channels_scaled_send(
|
||||
chan,
|
||||
millis(),
|
||||
|
|
|
@ -176,9 +176,6 @@ static void init_disarm_motors()
|
|||
motors.armed(false);
|
||||
set_armed(false);
|
||||
|
||||
motors.auto_armed(false);
|
||||
set_auto_armed(false);
|
||||
|
||||
compass.save_offsets();
|
||||
|
||||
g.throttle_cruise.save();
|
||||
|
|
|
@ -366,11 +366,6 @@ static void set_mode(uint8_t mode)
|
|||
control_mode = mode;
|
||||
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
|
||||
|
||||
// used to stop fly_aways
|
||||
// set to false if we have low throttle
|
||||
motors.auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
|
||||
set_auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
|
||||
|
||||
// if we change modes, we must clear landed flag
|
||||
set_land_complete(false);
|
||||
|
||||
|
@ -534,6 +529,29 @@ init_simple_bearing()
|
|||
}
|
||||
}
|
||||
|
||||
// update_auto_armed - update status of auto_armed flag
|
||||
static void update_auto_armed()
|
||||
{
|
||||
// disarm checks
|
||||
if(ap.auto_armed){
|
||||
// if motors are disarmed, auto_armed should also be false
|
||||
if(!motors.armed()) {
|
||||
set_auto_armed(false);
|
||||
return;
|
||||
}
|
||||
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
|
||||
if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
|
||||
set_auto_armed(false);
|
||||
}
|
||||
}else{
|
||||
// arm checks
|
||||
// if motors are armed and throttle is above zero auto_armed should be true
|
||||
if(motors.armed() && g.rc_3.control_in != 0) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* map from a 8 bit EEPROM baud rate to a real baud rate
|
||||
*/
|
||||
|
|
|
@ -348,7 +348,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|||
* cliSerial->printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
|
||||
* cliSerial->printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
|
||||
*
|
||||
* motors.auto_armed(false);
|
||||
* set_auto_armed(false);
|
||||
* motors.armed(true);
|
||||
*
|
||||
* while(1){
|
||||
|
|
Loading…
Reference in New Issue