Diff: main.cpp

Revision:

24:bc852aa89e7a

Parent:

23:61526647cc8a

Child:

25:88e6cccde856

--- a/main.cpp	Fri Dec 14 09:58:24 2018 +0000
+++ b/main.cpp	Mon Dec 17 14:34:37 2018 +0000
@@ -52,10 +52,6 @@
     semSensorData.release();
 }
 
-void startLLcomms() { // Send new demands to LL after receiving new target data
-    semLLcomms.release(); // Uses threadSetDemands which is normal priority
-}
-
 // This function will be called when a new transmission is received from high level
 void ReceiveAndReplan() {
     
@@ -158,6 +154,10 @@
 
 }
 
+void startLLcomms() { // Send new demands to LL after receiving new target data
+    semLLcomms.release(); // Uses threadSetDemands which is normal priority
+}
+
 void setDemandsForLL() {
     
     while(1) {
@@ -166,16 +166,13 @@
         mutPathIn.lock(); // Lock relevant mutex
         for(short int i=0; i<N_CHANNELS; i++) { // For each LL
             llcomms.mutChannel[i].lock(); // MUTEX LOCK
-            llcomms.demandPosition[i] = (int) ((dblDemandPosition_mtrs[i]/MAX_ACTUATOR_LENGTH)*511); // Convert to a 9-bit number
-            llcomms.demandPosition[i] = llcomms.demandPosition[i] & 0x1FF; // Ensure number is 9-bit
-            llcomms.demandSpeed[i] = (int) ((dblDemandVelocity_mmps[i]/MAX_SPEED_MMPS)*511);// Convert to a 9-bit number
-            llcomms.demandSpeed[i] = llcomms.demandSpeed[i] & 0x1FF; // Ensure number is 9-bit
+            llcomms.demandPosition[i] = dblDemandPosition_mtrs[i];
+            llcomms.demandSpeed[i] = dblDemandVelocity_mmps[i];
             llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
             llcomms.isDataReady[i] = 1; // Signal that data ready
         } // end for
         mutPathIn.unlock(); // Unlock relevant mutex
-
-    } // end while
+    } // end while(1)
         
 }
 
@@ -189,7 +186,7 @@
     threadSetDemands.start(setDemandsForLL); // Start planning thread
     threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
 
-    setDemandsTicker.attach(&startLLcomms, 1/LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
+    setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
     
     Thread::wait(1);
     while(1) {