Rover: Implementation of the ARMING functionality

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;
 

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;

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

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),

APMrover2/Parameters.h

@@ -57,6 +57,9 @@ public:
 
         // 97: RSSI
         k_param_rssi = 97,
+
+        // 100: Arming parameters
+        k_param_arming = 100,
                 
         // 110: Telemetry control
         //

APMrover2/Rover.h

@@ -71,6 +71,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 
+#include <AP_Arming/AP_Arming.h>
 #include "compat.h"
 
 #include <AP_Notify/AP_Notify.h>      // 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);

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

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)
 

APMrover2/make.inc

@@ -46,4 +46,5 @@ LIBRARIES += AP_OpticalFlow
 LIBRARIES += AP_RSSI
 LIBRARIES += AP_Declination
 LIBRARIES += AP_RPM
+LIBRARIES += AP_Arming
 

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)

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