commit 8c99e03a2c8c0d6c3f3fd5a15dcb6e9a80b96c10
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Nov 19 16:45:51 2019 +0100

    Add requestCharging() to send CAN msg to PowerManagement in order to enable the charging pins, add debugging mehtod setStrategy() in Porermanagement

diff --git a/devices/DiWheelDrive/DiWheelDrive.cpp b/devices/DiWheelDrive/DiWheelDrive.cpp
index 7286cdc..7f4ceb1 100644
--- a/devices/DiWheelDrive/DiWheelDrive.cpp
+++ b/devices/DiWheelDrive/DiWheelDrive.cpp
@@ -2,7 +2,7 @@
 #include "hal.h"
 #include "qei.h"
 #include "DiWheelDrive.h"
-#include <chprintf.h>
+// #include <chprintf.h>
 
 
 #include <global.hpp>
@@ -120,11 +120,11 @@ msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) {
       }
       break;
     case CAN::SET_LINE_FOLLOW_MSG:
-      chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
+      // chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
       if (frame->DLC == 1) {
         global.lfStrategy = frame->data8[0];
         global.msgReceived = true;
-        return RDY_OK;
+        // return RDY_OK;
       }
       break;
     case CAN::SET_KINEMATIC_CONST_ID:
@@ -150,9 +150,11 @@ msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) {
     case CAN::POWER_STATUS_ID:
       if (frame->DLC == 6) {
         // The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
+
         types::power_status::ChargingState charging_flags;
         charging_flags.value = frame->data8[0];
         global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path);
+        
         // Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
       }
     break;
@@ -190,6 +192,15 @@ msg_t DiWheelDrive::updateSensorVal() {
   return 0;
 }
 
+void DiWheelDrive::requestCharging(uint8_t power){
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::REQUEST_CHARGING_OVER_PIN);
+  frame.data8[0] = power;
+  frame.DLC = 1;
+  this->transmitMessage(&frame);
+}
+
 void DiWheelDrive::periodicBroadcast() {
   CANTxFrame frame;
   frame.SID = 0;
@@ -200,6 +211,11 @@ void DiWheelDrive::periodicBroadcast() {
   frame.data32[1] = this->actualSpeed[1];
   frame.DLC = 8;
   this->transmitMessage(&frame);
+  // Send Message for either activate or deactivate it
+
+
+
+ 
 
   // Send the valocites µm/s of the x axis and µrad/s around z axis: end
   // Send the odometry: start
diff --git a/devices/DiWheelDrive/DiWheelDrive.h b/devices/DiWheelDrive/DiWheelDrive.h
index 0ee13ed..85f24a4 100644
--- a/devices/DiWheelDrive/DiWheelDrive.h
+++ b/devices/DiWheelDrive/DiWheelDrive.h
@@ -26,12 +26,19 @@ namespace amiro {
 
     /** \brief Handle the start of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads */
     ThreadReference start(tprio_t PRIO);
+    /**
+    * Set Request to disable or enable the Charging.
+    * @param power 0 to disable, 1 to enable
+    */
+    void requestCharging(uint8_t power);
 
   protected:
     virtual msg_t receiveMessage(CANRxFrame *frame);
     virtual msg_t updateSensorVal();
     virtual void periodicBroadcast();
 
+    
+
     void  calibrateProximityFloorValues();
 
   private:
diff --git a/devices/PowerManagement/PowerManagement.cpp b/devices/PowerManagement/PowerManagement.cpp
index 92143fd..f506da2 100644
--- a/devices/PowerManagement/PowerManagement.cpp
+++ b/devices/PowerManagement/PowerManagement.cpp
@@ -57,6 +57,14 @@ msg_t PowerManagement::receiveMessage(CANRxFrame *frame) {
         return RDY_OK;
       }
       break;
+    case CAN::REQUEST_CHARGING_OVER_PIN:
+      if (frame->DLC == 1) {
+        // Error handling 
+        // TODO: Error Handling, for now just set the given value
+        this->powerStatus.charging_flags.content.diwheeldrive_enable_power_path = frame->data8[0];
+        return RDY_OK;
+      }
+      break;
 
     default:
       break;
@@ -69,6 +77,7 @@ msg_t PowerManagement::updateSensorVal() {
 
   // update charger status
   this->powerStatus.charging_flags.content.powermanagement_plugged_in = global.ltc4412.isPluggedIn();
+  // this->powerStatus.charging_flags.content.powermanagement_plugged_in = (palReadPad((GPIO_TypeDef*)GPIOC, GPIOC_PATHDC) == PAL_HIGH); 
 
   // update fuel gauges values
   const BQ27500::Driver::UpdateData* power[2] {
@@ -115,6 +124,17 @@ msg_t PowerManagement::updateSensorVal() {
   return 0;
 }
 
+void PowerManagement::setStrategy(uint8_t strategy){
+  CANTxFrame frame;
+  chprintf((BaseSequentialStream*) &SD1, "Message Triggered!\n");
+  // global.triggerCan = false;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::SET_LINE_FOLLOW_MSG);
+  frame.data8[0] = strategy;
+  frame.DLC = 1;
+  this->transmitMessage(&frame);
+}
+
 void PowerManagement::periodicBroadcast() {
   CANTxFrame frame;
   if (this->bc_counter % 10 == 0) {
@@ -127,6 +147,7 @@ void PowerManagement::periodicBroadcast() {
     frame.DLC = 6;
     this->transmitMessage(&frame);
   }
+
   for (int i = 0; i < 8; i++) {
     frame.SID = 0;
     this->encodeDeviceId(&frame, CAN::PROXIMITY_RING_ID(i));
diff --git a/devices/PowerManagement/PowerManagement.h b/devices/PowerManagement/PowerManagement.h
index 9927167..76b8a38 100644
--- a/devices/PowerManagement/PowerManagement.h
+++ b/devices/PowerManagement/PowerManagement.h
@@ -24,6 +24,7 @@ namespace amiro {
 
     /** \brief Handle the start of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads */
     ThreadReference start(tprio_t PRIO);
+    void setStrategy(uint8_t strategy);
 
     types::power_status& getPowerStatus();
 
@@ -31,7 +32,7 @@ namespace amiro {
     virtual msg_t receiveMessage(CANRxFrame *frame);
     virtual msg_t updateSensorVal();
     virtual void periodicBroadcast();
-
+    
     void  calibrateProximityRingValues();
 
   private:
