From 3636b5331310b417db38c467154f05929ff250f3 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Fri, 30 Oct 2015 16:56:41 +1100 Subject: [PATCH] Rover: Implementation of the ARMING functionality --- APMrover2/APMrover2.cpp | 12 +++--- APMrover2/GCS_Mavlink.cpp | 19 +++++++++ APMrover2/Log.cpp | 21 ++++++++++ APMrover2/Parameters.cpp | 4 ++ APMrover2/Parameters.h | 3 ++ APMrover2/Rover.h | 20 ++++++++++ APMrover2/Steering.cpp | 23 +++++++++++ APMrover2/defines.h | 2 + APMrover2/make.inc | 1 + APMrover2/radio.cpp | 81 ++++++++++++++++++++++++++++++++++++++- APMrover2/system.cpp | 53 +++++++++++++++++++++++++ 11 files changed, 233 insertions(+), 6 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 955b211a97..f0e0bcee99 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -96,10 +96,6 @@ void Rover::setup() notify.init(false); - // rover does not use arming nor pre-arm checks - AP_Notify::flags.armed = true; - AP_Notify::flags.pre_arm_check = true; - AP_Notify::flags.pre_arm_gps_check = true; AP_Notify::flags.failsafe_battery = false; rssi.init(); @@ -147,7 +143,8 @@ void Rover::loop() // update AHRS system void Rover::ahrs_update() { - hal.util->set_soft_armed(hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); + hal.util->set_soft_armed(arming.is_armed() && + hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); #if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update @@ -292,6 +289,11 @@ void Rover::one_second_loop(void) // cope with changes to aux functions update_aux(); + // update notify flags + AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); + AP_Notify::flags.pre_arm_gps_check = true; + AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO; + // cope with changes to mavlink system ID mavlink_system.sysid = g.sysid_this_mav; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 045f35f571..ee2d5f4c1d 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1075,6 +1075,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_COMPONENT_ARM_DISARM: + if (is_equal(packet.param1,1.0f)) { + // run pre_arm_checks and arm_checks and display failures + if (rover.arm_motors(AP_Arming::MAVLINK)) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + } else if (is_zero(packet.param1)) { + if (rover.disarm_motors()) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_FAILED; + } + } else { + result = MAV_RESULT_UNSUPPORTED; + } + break; + case MAV_CMD_GET_HOME_POSITION: send_home(rover.ahrs.get_home()); break; diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index c09cab0612..022df2fd0e 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -337,6 +337,23 @@ void Rover::Log_Write_Current() DataFlash.Log_Write_Power(); } +struct PACKED log_Arm_Disarm { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t arm_state; + uint16_t arm_checks; +}; + +void Rover::Log_Arm_Disarm() { + struct log_Arm_Disarm pkt = { + LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), + time_us : hal.scheduler->micros64(), + arm_state : arming.is_armed(), + arm_checks : arming.get_enabled_checks() + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + void Rover::Log_Write_RC(void) { DataFlash.Log_Write_RCIN(); @@ -380,6 +397,8 @@ const LogStructure Rover::log_structure[] = { "NTUN", "QHfHHb", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr" }, { LOG_SONAR_MSG, sizeof(log_Sonar), "SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, + { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), + "ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, { LOG_STEERING_MSG, sizeof(log_Steering), "STER", "Qff", "TimeUS,Demanded,Achieved" }, }; @@ -402,6 +421,8 @@ void Rover::log_init(void) if (g.log_bitmask != 0) { start_logging(); } + + arming.set_logging_available(DataFlash.CardInserted()); } #if CLI_ENABLED == ENABLED diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 0423a409c8..bc9f2bce30 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -496,6 +496,10 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(camera_mount, "MNT", AP_Mount), #endif + // @Group: ARMING_ + // @Path: ../libraries/AP_Arming/AP_Arming.cpp + GOBJECT(arming, "ARMING_", AP_Arming), + // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp GOBJECT(battery, "BATT", AP_BattMonitor), diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 742d5daece..df2f7fd7aa 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -57,6 +57,9 @@ public: // 97: RSSI k_param_rssi = 97, + + // 100: Arming parameters + k_param_arming = 100, // 110: Telemetry control // diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 1a25281cac..d799087f98 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -71,6 +71,7 @@ #include #include +#include #include "compat.h" #include // Notify library @@ -96,6 +97,7 @@ class Rover : public AP_HAL::HAL::Callbacks { public: friend class GCS_MAVLINK; friend class Parameters; + friend class AP_Arming; Rover(void); @@ -154,6 +156,9 @@ private: AP_AHRS_DCM ahrs {ins, barometer, gps}; #endif + // Arming/Disarming mangement class + AP_Arming arming {ahrs, barometer, compass, home_is_set}; + AP_L1_Control L1_controller; // selected navigation controller @@ -363,6 +368,14 @@ private: float distance_past_wp; // record the distance we have gone past the wp + // time that rudder/steering arming has been running + uint32_t rudder_arm_timer; + + // true if we are in an auto-throttle mode, which means + // we need to run the speed/height controller + bool auto_throttle_mode; + + private: // private member functions void ahrs_update(); @@ -418,6 +431,7 @@ private: void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void log_init(void); void start_logging() ; + void Log_Arm_Disarm(); void load_parameters(void); void throttle_slew_limit(int16_t last_throttle); @@ -504,6 +518,12 @@ private: void do_digicam_configure(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd); void init_capabilities(void); + void rudder_arm_disarm_check(); + void change_arm_state(void); + bool disarm_motors(void); + bool arm_motors(AP_Arming::ArmingMethod method); + bool is_moving(); + void update_home(); public: bool print_log_menu(void); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 8d9e3e6748..d3f92c122d 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -248,6 +248,10 @@ void Rover::set_servos(void) channel_throttle->servo_out = 0; } + if (!hal.util->get_soft_armed()) { + channel_throttle->servo_out = 0; + } + // convert 0 to 100% into PWM channel_throttle->calc_pwm(); @@ -277,6 +281,25 @@ void Rover::set_servos(void) channel_throttle->calc_pwm(); } + if (!arming.is_armed()) { + //Some ESCs get noisy (beep error msgs) if PWM == 0. + //This little segment aims to avoid this. + switch (arming.arming_required()) { + case AP_Arming::NO: + //keep existing behavior: do nothing to radio_out + //(don't disarm throttle channel even if AP_Arming class is) + break; + + case AP_Arming::YES_ZERO_PWM: + channel_throttle->radio_out = 0; + break; + + case AP_Arming::YES_MIN_PWM: + default: + channel_throttle->radio_out = channel_throttle->radio_trim; + break; + } + } #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS // send values to the PWM timers for output diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 2d886c6150..594851b155 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -67,6 +67,7 @@ enum mode { #define LOG_PERFORMANCE_MSG 0x03 #define LOG_STARTUP_MSG 0x06 #define LOG_SONAR_MSG 0x07 +#define LOG_ARM_DISARM_MSG 0x08 #define LOG_STEERING_MSG 0x0D #define TYPE_AIRSTART_MSG 0x00 @@ -88,6 +89,7 @@ enum mode { #define MASK_LOG_CAMERA (1<<12) #define MASK_LOG_STEERING (1<<13) #define MASK_LOG_RC (1<<14) +#define MASK_LOG_ARM_DISARM (1<<15) #define MASK_LOG_WHEN_DISARMED (1UL<<16) #define MASK_LOG_IMU_RAW (1UL<<19) diff --git a/APMrover2/make.inc b/APMrover2/make.inc index ef3cc71e77..f88df179cf 100644 --- a/APMrover2/make.inc +++ b/APMrover2/make.inc @@ -46,4 +46,5 @@ LIBRARIES += AP_OpticalFlow LIBRARIES += AP_RSSI LIBRARIES += AP_Declination LIBRARIES += AP_RPM +LIBRARIES += AP_Arming diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 0463e323a4..f3e8fec371 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -13,7 +13,12 @@ void Rover::set_control_channels(void) // set rc channel ranges channel_steer->set_angle(SERVO_MAX); - channel_throttle->set_angle(100); + channel_throttle->set_angle(100); + + // For a rover safety is TRIM throttle + if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_trim); + } // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. @@ -34,10 +39,81 @@ void Rover::init_rc_out() { RC_Channel::rc_channel(CH_1)->enable_out(); RC_Channel::rc_channel(CH_3)->enable_out(); + + if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { + channel_throttle->enable_out(); + } + RC_Channel::output_trim_all(); // setup PWM values to send if the FMU firmware dies RC_Channel::setup_failsafe_trim_all(); + + // setup PX4 to output the min throttle when safety off if arming + // is setup for min on disarm + if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_trim); + } +} + +/* + check for driver input on rudder/steering stick for arming/disarming +*/ +void Rover::rudder_arm_disarm_check() +{ + // In Rover we need to check that its set to the throttle trim and within the DZ + // if throttle is not within trim dz, then pilot cannot rudder arm/disarm + if (!channel_throttle->in_trim_dz()) { + rudder_arm_timer = 0; + return; + } + + // if not in a manual throttle mode then disallow rudder arming/disarming + if (auto_throttle_mode) { + rudder_arm_timer = 0; + return; + } + + if (!arming.is_armed()) { + // when not armed, full right rudder starts arming counter + if (channel_steer->control_in > 4000) { + uint32_t now = millis(); + + if (rudder_arm_timer == 0 || + now - rudder_arm_timer < 3000) { + + if (rudder_arm_timer == 0) { + rudder_arm_timer = now; + } + } else { + //time to arm! + arm_motors(AP_Arming::RUDDER); + rudder_arm_timer = 0; + } + } else { + // not at full right rudder + rudder_arm_timer = 0; + } + } else if (!is_moving()) { + // when armed and not moving, full left rudder starts disarming counter + if (channel_steer->control_in < -4000) { + uint32_t now = millis(); + + if (rudder_arm_timer == 0 || + now - rudder_arm_timer < 3000) { + if (rudder_arm_timer == 0) { + rudder_arm_timer = now; + } + } else { + //time to disarm! + disarm_motors(); + rudder_arm_timer = 0; + } + } else { + // not at full left rudder + rudder_arm_timer = 0; + } + } } void Rover::read_radio() @@ -95,6 +171,9 @@ void Rover::read_radio() channel_steer->set_pwm(steer); channel_throttle->set_pwm(thr); } + + rudder_arm_disarm_check(); + } void Rover::control_failsafe(uint16_t pwm) diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 1feb0a2415..7a63b4acab 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -285,18 +285,22 @@ void Rover::set_mode(enum mode mode) case HOLD: case LEARNING: case STEERING: + auto_throttle_mode = false; break; case AUTO: + auto_throttle_mode = true; rtl_complete = false; restart_nav(); break; case RTL: + auto_throttle_mode = true; do_RTL(); break; case GUIDED: + auto_throttle_mode = true; rtl_complete = false; /* when entering guided mode we set the target as the current @@ -307,6 +311,7 @@ void Rover::set_mode(enum mode mode) break; default: + auto_throttle_mode = true; do_RTL(); break; } @@ -491,3 +496,51 @@ void Rover::frsky_telemetry_send(void) frsky_telemetry.send_frames((uint8_t)control_mode); } #endif + +/* + update AHRS soft arm state and log as needed + */ +void Rover::change_arm_state(void) +{ + Log_Arm_Disarm(); + hal.util->set_soft_armed(arming.is_armed() && + hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); +} + +/* + arm motors + */ +bool Rover::arm_motors(AP_Arming::ArmingMethod method) +{ + if (!arming.arm(method)) { + return false; + } + + // only log if arming was successful + channel_throttle->enable_out(); + + change_arm_state(); + return true; +} + +/* + disarm motors + */ +bool Rover::disarm_motors(void) +{ + if (!arming.disarm()) { + return false; + } + if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) { + channel_throttle->disable_out(); + } + if (control_mode != AUTO) { + // reset the mission on disarm if we are not in auto + mission.reset(); + } + + //only log if disarming was successful + change_arm_state(); + + return true; +}