Merge pull request #1281 from bitcraze/krichardsson/arming

Improved arming functionality

docs/functional-areas/crtp/crtp_localization.md

@@ -9,8 +9,8 @@ channels:
 
  | Port  | Channel  | Name|
  | ------| ---------| ----------------------|
- | 6     | 0        | External Position|
- | 6     | 1        | Generic localization|
+ | 6     | 0        | External Position     |
+ | 6     | 1        | Generic localization  |
 
 External Position
 -----------------
@@ -39,16 +39,24 @@ subsystem. It has been created to serve the Loco Positioning System
 packets but can be used for more general things like GPS NMEA or binary
 streams. The format of the packet is:
 
-|  Byte  | Value    | Note|
-|  ------| ---------| ---------------------------------------|
-|  0     | ID       | ID of the packet|
- | 1..   | Payload  | Packet payload. Format defined per ID|
-
-|  ID  | Packet |
-|  ----| ------------------------------|
-|  2  |  LPP Short packet tunnel|
-|  3  |  Enable emergency stop|
-|  4  |  Reset emergency stop timeout|
+|  Byte  | Value    | Note                                  |
+|  ------| ---------| --------------------------------------|
+|  0     | ID       | ID of the packet                      |
+|  1..   | Payload  | Packet payload. Format defined per ID |
+
+|  ID  | Packet                                     |
+|  ----| -------------------------------------------|
+|   0  | Range stream report                        |
+|   1  | Range stream report, 16 bit floating point |
+|   2  | LPP Short packet tunnel                    |
+|   3  | Enable emergency stop                      |
+|   4  | Reset emergency stop timeout               |
+|   6  | COMM GNSS NMEA                             |
+|   7  | COMM GNSS proprietary                      |
+|   8  | External pose information                  |
+|   9  | External pose information, packed          |
+|  10  | Lighthouse angle stream                    |
+|  11  | Lighthouse data persist                    |
 
 ### LPP Short packet tunnel
 

docs/functional-areas/crtp/crtp_platform.md

@@ -21,14 +21,15 @@ The first byte describes the command:
 | value | Command |
 |-------|---------|
 | 0     | [Set continuous wave](#set-continuous-wave) |
+| 1     | [Request arm/disarm the system](#armdisarm-system) |
 
 ### Set continuous wave
 
 Command and answer:
 
 | Byte | Description |
 |------|-------------|
-| 0    | setContinuousWave (0) |
+| 0    | command setContinuousWave (0) |
 | 1    | Enable |
 
 If enable is not 0, the Crazyflie radio will start transmitting a continuous sine wave at the currently setup
@@ -38,6 +39,25 @@ This command should only be sent over USB (it disables the radio communication).
 It is used in production to test the Crazyflie radio path and should not be used outside of a lab or
 other very controlled environment. It will effectively jam local radio communication on the channel.
 
+### Arm/disarm system
+
+Arm or disarm the system if possible.
+
+Command:
+
+| Byte | Description                           |
+|------|---------------------------------------|
+| 0    | command request arm/disarm system (1) |
+| 1    | 0 = disarm, non-zero = arm the system |
+
+Answer:
+
+| Byte | Description                                          |
+|------|------------------------------------------------------|
+| 0    | command request arm/disarm system (1)                |
+| 1    | success: 1 if the requested arming state was set     |
+| 2    | isArmed: 0 = system is disarmed, 1 = system is armed |
+
 ## Version commands
 
 The first byte describes the command:

docs/functional-areas/supervisor/arming.md

@@ -0,0 +1,39 @@
+---
+title: Arming
+page_id: supervisor_arming
+sort_order: 4
+---
+
+Arming is a manual action required before take off to let the system know that the pilot is in control of the system
+and is ready to fly. Arming is implemented as a condition in the supervisor and is required to transition from the
+`Pre flight checks passed` state to `Ready to fly`. The condition is satisfied by making an arming request in the
+supervisor, that is arming in an event. If the supervisor state transitions back to `Pre flight checks passed`, the
+system must be armed again to be able to fly.
+
+## Arming request
+
+Clients perform an arming request by sending a [CRTP arming message](/docs/functional-areas/crtp/crtp_platform.md#armdisarm-system)
+to the Crazyflie.
+
+Apps running in the Crazyflie request arming by calling the `supervisorRequestArming()` function.
+
+## Auto arming
+
+On small, safe platforms with brushed motors, like the Crazyflie 2.X, it is possible to configure auto arming. When
+auto arming is enabled, the system will automatically request arming when entering the `Pre flight checks passed`
+state. This will (if all other conditions are met) make the supervisor transition into the `Ready to fly` state.
+
+Auto arming is the default setting for the Crazyflie 2.X.
+
+Auto arming is configured at compile time through the `CONFIG_MOTORS_REQUIRE_ARMING=y` kconfig flag.
+
+## Idle thrust
+
+The motors are used to indicate to the pilot that the system is armed and ready to fly. Motors runs at idle thrust when
+the supervisor is in a state where flight is enabled.
+
+Note that the default
+settings for idle thrust on the Crazyflie 2.X is 0 and the motors will not spin. On platforms using brushless motors,
+an idle thrust that spins the motors should be used.
+
+Idle thrust is configured at compile time through the `CONFIG_MOTORS_DEFAULT_IDLE_THRUST` kconfig parameter.

docs/functional-areas/supervisor/conditions.md

@@ -1,6 +1,7 @@
 ---
 title: Supervisor conditions
 page_id: supervisor_conditions
+sort_order: 1
 ---
 
 A condition express the state of some part of the system, for instance if we are flying, if the system is armed or

docs/functional-areas/supervisor/index.md

@@ -21,7 +21,7 @@ The update sequence is separated into a few steps:
 ## Modifying behavior
 
 The main modification of behavior is to prevent the motors from spinning when the system is not ready to fly, for
-instance if the system is not armed or the USB cable is connected. Also the high level commander is blocked from
+instance if the system is not armed or the Crazyflie is up side down. Also the high level commander is blocked from
 running trajectories in this case.
 
 Exceptional situations when flying, for instance when tumbling is detected, are simply handled by stopping the

docs/functional-areas/supervisor/states.md

@@ -1,6 +1,7 @@
 ---
 title: Supervisor states
 page_id: supervisor_states
+sort_order: 2
 ---
 
 ## State diagram

docs/functional-areas/supervisor/transitions.md

@@ -1,6 +1,7 @@
 ---
 title: Supervisor transitions
 page_id: supervisor_transitions
+sort_order: 3
 ---
 
 A state transition takes the state machine from one state to another. The [conditions bit field](conditions.md) is used to identify

src/config/config.h

@@ -46,7 +46,7 @@
 #include "trace.h"
 #include "usec_time.h"
 
-#define PROTOCOL_VERSION 5
+#define PROTOCOL_VERSION 6
 
 #define CONFIG_BLOCK_ADDRESS    (2048 * (64-1))
 #define MCU_ID_ADDRESS          0x1FFF7A10

src/modules/interface/supervisor.h

@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2021 Bitcraze AB
+ * Copyright (C) 2021-2023 Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -82,3 +82,29 @@ bool supervisorIsFlying(void);
  * @return false
  */
 bool supervisorIsTumbled(void);
+
+/**
+ * @brief Request the system to be armed or disarmed. The system can only be armed under certain conditions, this is
+ * indicated by the supervisorCanArm() function.
+ *
+ * @param doArm  true - request arming. false - request disarming.
+ * @return true   The request was granted
+ * @return false  The request could not be granted
+ */
+bool supervisorRequestArming(const bool doArm);
+
+/**
+ * @brief Query if the system can be armed
+ *
+ * @return true
+ * @return false
+ */
+bool supervisorCanArm();
+
+/**
+ * @brief Query if the system is armed
+ *
+ * @return true
+ * @return false
+ */
+bool supervisorIsArmed();

src/modules/interface/system.h

@@ -1,6 +1,6 @@
 /*
- *    ||          ____  _ __                           
- * +------+      / __ )(_) /_______________ _____  ___ 
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
  * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
@@ -38,8 +38,6 @@ void systemLaunch(void);
 
 void systemStart();
 void systemWaitStart(void);
-void systemSetArmed(bool val);
-bool systemIsArmed();
 
 void systemRequestShutdown();
 void systemRequestNRFVersion();

src/modules/src/Kconfig

@@ -154,7 +154,7 @@ config MOTORS_DEFAULT_IDLE_THRUST
     int "Default idle thrust for motors in armed state"
     range 0 65535
     default 0
-    depends on MOTORS_START_DISARMED
+    depends on MOTORS_REQUIRE_ARMING
     help
         Default thrust for motors when idling in armed state, expressed as an
         integer in the range 0 to 65535. This can be overridden with parameters;

src/modules/src/extrx.c

@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2011-2013 Bitcraze AB
+ * Copyright (C) 2011-2023 Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -38,6 +38,7 @@
 #include "uart1.h"
 #include "cppm.h"
 #include "crtp_commander.h"
+#include "supervisor.h"
 
 #define DEBUG_MODULE  "EXTRX"
 #include "debug.h"
@@ -87,7 +88,7 @@
 bool extRxArm = false;
 bool extRxAltHold = false;
 bool extRxModeRate = false;
-  
+
 #if CONFIG_DECK_EXTRX_ARMING
   bool extRxArmPrev = false;
   int8_t arm_cnt = 0;
@@ -153,17 +154,20 @@ static void extRxTask(void *param)
 
 static void extRxDecodeChannels(void)
 {
-  
+
   #if CONFIG_DECK_EXTRX_ARMING
   if (EXTRX_SIGN_ARM * cppmConvert2Float(ch[EXTRX_CH_ARM], -1, 1, 0.0) > 0.5f) // channel needs to be 75% or more to work correctly with 2/3 way switches
   {
     if (arm_cnt < EXTRX_SWITCH_MIN_CNT) arm_cnt++;
     else extRxArm = true;
-    
+
     if (extRxArmPrev != extRxArm)
     {
-      DEBUG_PRINT("Engines armed\n");
-      systemSetArmed(extRxArm);
+      if (supervisorRequestArming(extRxArm)) {
+        DEBUG_PRINT("System armed\n");
+      } else {
+        DEBUG_PRINT("Arming failed\n");
+      }
     }
   }
   else
@@ -173,8 +177,8 @@ static void extRxDecodeChannels(void)
 
     if (extRxArmPrev != extRxArm)
     {
-      DEBUG_PRINT("Engines unarmed\n");
-      systemSetArmed(extRxArm);
+      DEBUG_PRINT("System unarmed\n");
+      supervisorRequestArming(extRxArm);
     }
   }
 
@@ -197,22 +201,22 @@ static void extRxDecodeChannels(void)
     if (extRxAltHoldPrev != extRxAltHold) DEBUG_PRINT("Althold mode OFF\n");
   }
 
-  if (extRxAltHold) 
+  if (extRxAltHold)
   {
     extrxSetpoint.thrust = 0;
     extrxSetpoint.mode.z = modeVelocity;
 
     extrxSetpoint.velocity.z = cppmConvert2Float(ch[EXTRX_CH_THRUST], -1, 1, EXTRX_DEADBAND_ZVEL);
-  } 
-  else 
+  }
+  else
   {
     extrxSetpoint.mode.z = modeDisable;
     extrxSetpoint.thrust = cppmConvert2uint16(ch[EXTRX_CH_THRUST]);
   }
-  
+
   extRxAltHoldPrev = extRxAltHold;
   #else
-  
+
   extrxSetpoint.mode.z = modeDisable;
   extrxSetpoint.thrust = cppmConvert2uint16(ch[EXTRX_CH_THRUST]);
   #endif
@@ -222,7 +226,7 @@ static void extRxDecodeChannels(void)
   {
     if (modeRate_cnt < EXTRX_SWITCH_MIN_CNT) modeRate_cnt++;
     else extRxModeRate = true;
-    
+
     if (extRxModeRatePrev != extRxModeRate)
     {
       DEBUG_PRINT("Switched to rate mode\n");
@@ -245,14 +249,14 @@ static void extRxDecodeChannels(void)
 
   extRxModeRatePrev = extRxModeRate;
   #endif
-  
+
   if (extRxModeRate)
-  {  
+  {
     extrxSetpoint.attitudeRate.roll = EXTRX_SIGN_ROLL * getCPPMRollRateScale() * cppmConvert2Float(ch[EXTRX_CH_ROLL], -1, 1, EXTRX_DEADBAND_ROLL);
     extrxSetpoint.attitudeRate.pitch = EXTRX_SIGN_PITCH * getCPPMPitchRateScale() * cppmConvert2Float(ch[EXTRX_CH_PITCH], -1, 1, EXTRX_DEADBAND_PITCH);
   }
   else
-  {  
+  {
     extrxSetpoint.attitude.roll = EXTRX_SIGN_ROLL * getCPPMRollScale() * cppmConvert2Float(ch[EXTRX_CH_ROLL], -1, 1, EXTRX_DEADBAND_ROLL);
     extrxSetpoint.attitude.pitch = EXTRX_SIGN_PITCH * getCPPMPitchScale() * cppmConvert2Float(ch[EXTRX_CH_PITCH], -1, 1, EXTRX_DEADBAND_PITCH);
   }
@@ -368,8 +372,8 @@ LOG_ADD(LOG_UINT16, ch7, &ch[7])
 LOG_GROUP_STOP(extrx_raw)
 
 /**
- * External receiver (RX) log group. This contains setpoints for 
- * thrust, roll, pitch, yaw rate, z velocity, and arming and 
+ * External receiver (RX) log group. This contains setpoints for
+ * thrust, roll, pitch, yaw rate, z velocity, and arming and
  * altitude-hold signals
  */
 LOG_GROUP_START(extrx)
@@ -410,4 +414,4 @@ LOG_ADD(LOG_UINT8, AltHold, &extRxAltHold)
  */
 LOG_ADD(LOG_UINT8, Arm, &extRxArm)
 LOG_GROUP_STOP(extrx)
-#endif
+#endif

src/modules/src/msp.c

@@ -39,6 +39,7 @@
 #include "string.h"
 #include "platform.h"
 #include "param.h"
+#include "supervisor.h"
 
 // MSP command IDs
 typedef enum {
@@ -416,7 +417,12 @@ static void mspHandleRequestMspStatus(MspObject* pMspObject)
   pData->cycleTime = 1000; // TODO: API to query this?
   pData->i2cErrors = 0; // unused
   pData->sensors = 0x0001; // no sensors supported yet, but need to report at least one to get the level bars to show
-  pData->flags = 0x00000001; // always report armed (bit zero)
+
+  pData->flags = 0x0;
+  if (supervisorIsArmed()) {
+    pData->flags |= 0x00000001; // report armed (bit zero)
+  }
+
   pData->currentSet = 0x00;
 
   mspMakeTxPacket(pMspObject, MSP_STATUS, (uint8_t*) pData, sizeof(MspStatus));