move energy to electrical subsystem

also publish ENERGY message on rotorcrafts

conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml

@@ -216,6 +216,7 @@
     <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
     <define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
     <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="30000"/>
   </section>
 
 </airframe>

conf/telemetry/default_rotorcraft.xml

@@ -19,6 +19,7 @@
       <message name="I2C_ERRORS"             period="4.1"/>
       <message name="UART_ERRORS"            period="3.1"/>
       <message name="SUPERBITRF"             period="3"/>
+      <message name="ENERGY"                 period="2.5"/>
     </mode>
 
     <mode name="ppm">

sw/airborne/firmwares/fixedwing/autopilot.h

@@ -93,8 +93,8 @@ extern uint16_t vsupply;
  */
 extern int32_t current;	// milliAmpere
 
-/** Fuel consumption (mAh)
- * TODO: move to electrical subsystem
+/** Energy consumption (mAh)
+ * This is the ap copy of the measurement from fbw
  */
 extern float energy;
 

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -420,6 +420,7 @@ static inline void telecommand_task( void ) {
 
   vsupply = fbw_state->vsupply;
   current = fbw_state->current;
+  energy = fbw_state->energy;
 
 #ifdef RADIO_CONTROL
   if (!autopilot_flight_time) {
@@ -517,7 +518,6 @@ void navigation_task( void ) {
 
     // climb_loop(); //4Hz
   }
-  energy += ((float)current) / 3600.0f * 0.25f;	// mAh = mA * dt (4Hz -> hours)
 }
 
 

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -132,6 +132,14 @@ static void send_status(void) {
       &electrical.vsupply, &time_sec);
 }
 
+static void send_energy(void) {
+  const int16_t e = electrical.energy;
+  const float vsup = ((float)electrical.vsupply) / 10.0f;
+  const float curs = ((float)electrical.current) / 1000.0f;
+  const float power = vsup * curs;
+  DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power);
+}
+
 static void send_fp(void) {
   int32_t carrot_up = -guidance_v_z_sp;
   DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice,
@@ -222,6 +230,7 @@ void autopilot_init(void) {
 
   register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
+  register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
   register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);

sw/airborne/inter_mcu.h

@@ -60,6 +60,7 @@ struct fbw_state {
   uint8_t nb_err;
   uint16_t vsupply; ///< 1e-1 V
   int32_t current;  ///< milliAmps
+  float energy;     ///< mAh
 };
 
 struct ap_state {
@@ -123,6 +124,7 @@ static inline void inter_mcu_fill_fbw_state (void) {
 
   fbw_state->vsupply = electrical.vsupply;
   fbw_state->current = electrical.current;
+  fbw_state->energy = electrical.energy;
 #if defined SINGLE_MCU
   /**Directly set the flag indicating to AP that shared buffer is available*/
   inter_mcu_received_fbw = TRUE;

sw/airborne/subsystems/electrical.c

@@ -85,6 +85,7 @@ static struct {
 void electrical_init(void) {
   electrical.vsupply = 0;
   electrical.current = 0;
+  electrical.energy = 0;
 
   electrical.bat_low = FALSE;
   electrical.bat_critical = FALSE;
@@ -135,6 +136,8 @@ void electrical_periodic(void) {
   electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
 #endif /* ADC_CHANNEL_CURRENT */
 
+  // mAh = mA * dt (10Hz -> hours)
+  electrical.energy += ((float)electrical.current) / 3600.0f / ELECTRICAL_PERIODIC_FREQ;
 
   if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
     if (bat_low_counter > 0)

sw/airborne/subsystems/electrical.h

@@ -48,6 +48,7 @@ struct Electrical {
   uint16_t vsupply;       ///< supply voltage in decivolts
   int32_t  current;       ///< current in milliamps
   int32_t  consumed;      ///< consumption in mAh
+  float    energy;        ///< consumed energy in mAh
   bool_t   bat_low;       ///< battery low status
   bool_t   bat_critical;  ///< battery critical status