Revision 7:fd8e0604faaa, committed 2019-12-12

Comitter:

micallef25

Date:

Thu Dec 12 17:25:27 2019 +0000

Parent:

6:6cb13ac483e0

Commit message:

stable, passed 1000 simulation regression

Changed in this revision

--- a/AccCar.cpp	Wed Dec 11 20:12:21 2019 +0000
+++ b/AccCar.cpp	Thu Dec 12 17:25:27 2019 +0000
@@ -11,7 +11,7 @@
 
 //#define DEBUG_ACC
 #define ACC_TAG "[ACC] "
-
+#define ERROR_TAG "[ERROR] "
 // speed for a legal stop
 #define STOPPED_SPEED 0
 
@@ -68,7 +68,11 @@
             int diff = forward_car->position - position;
             int maxSafeSpeed = diff - SAFETY_GAP;
             speed = std::min(maxSafeSpeed, target_speed);
+            //if(speed < 0)
+           // {
+             //   printf(ERROR_TAG"f_pos %d pos %d id %d"DELIM,forward_car->position,position,car_id);
             assert(speed >= 0);
+            //}
         }   else {
             speed = target_speed;
         }
@@ -89,6 +93,24 @@
     this->target_speed = temp_speed;
 }
 
+/*
+* assuming the car has some way to see a stop sign
+* will kick in until a position message with the right ID comes
+* which is not implemented
+*/
+int AccCar::enter_safety_mode()
+{
+    int old_position = position;
+    // using previous speed
+    int next_position = position + speed;
+    if( old_position < 54 && next_position >= 54 ) 
+    {
+        // return new speed 0 or 
+        return 54 - position;
+    }
+    return speed;  
+}
+
 // update FSM 
 void AccCar::update()
 {
@@ -105,25 +127,28 @@
             if(c_msg == NULL)
             {
                 // test if we actually have a large delay
-                assert(c_msg != NULL);
-                //speed = enter_safety_mode();
+               assert(c_msg != NULL);
+               speed = enter_safety_mode();
             }
             else{
             //
             // do something with the control message
                 assert(c_msg->car_id == car_id );
-                speed = c_msg->speed;
-            
-                //
-                if(speed == 0)
-                {
-                    state = STOPPED_STATE;    
+                if(c_msg->speed > speed){
+                    printf(ERROR_TAG" msg speed %d , speed %d car %d"DELIM,c_msg->speed,speed, car_id);
                 }
-                else
-                {
-                    state = NORMAL_STATE;   
+                
+                else{
+                    // add wait time if we got 0 or something less than our sent speed
+                    if(speed == 0 || c_msg->speed < speed)
+                    {
+                        wait_time++;
+                        printf(ACC_TAG"car %d has waited %d"DELIM,car_id,wait_time);
+                    }
+                    speed = c_msg->speed;
                 }
                 
+                
                 //  
                 delete c_msg;
             }
@@ -132,9 +157,13 @@
 
         position = position + speed;
         
-        // ensure correctness 
-        assert(position <= this->forward_car->position - 2);
-        
+        // ensure correctness
+        if(!lead){
+            //if(position >= this->forward_car->position - 2){
+              //  printf(ERROR_TAG"position %d forward pos %d id %d speed %d"DELIM,position,this->forward_car->position, car_id,speed);
+                assert(position <= this->forward_car->position - 2);    
+           // }
+        }
         
         if( (cycles % 5) == 0)
         {
@@ -146,10 +175,6 @@
         {
             drive_normal();
         }
-        else if( state == STOPPED_STATE )
-        {
-            wait_time++;    
-        }
         else
         {   
             // case should not happen
@@ -170,20 +195,6 @@
         // stuff a message in the queue
         this->singleton->add_to_position_queue(msg);
         
-//#ifndef PUBLISH_ONLY       
-//        //
-//        // get_control message may need to malloc the message or something
-//        control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
-//        
-//        //
-//        // do something with the control message
-//        assert(c_msg->car_id == car_id );
-//        speed = c_msg->speed;
-//    
-//        //
-//        // delete the control message
-//        delete c_msg;
-//#endif
         // set flag for completion of cycle
         road->done_flags.set(this->flag);
     }
@@ -221,7 +232,7 @@
 // and stop or update speed accordingly
 void AccCar::drive_normal()
 {
-    assert( state == NORMAL_STATE );
+    //assert( state == NORMAL_STATE );
     // first update our speed
     if( !lead ) 
     {
@@ -241,3 +252,8 @@
 {
     return cycles;
 }
+
+int AccCar::get_target_speed()
+{
+    return target_speed;
+}

--- a/AccCar.h	Wed Dec 11 20:12:21 2019 +0000
+++ b/AccCar.h	Thu Dec 12 17:25:27 2019 +0000
@@ -38,6 +38,7 @@
     
     int get_cycles();
     
+    int get_target_speed();
 protected:
     int car_id;
     int target_speed;
@@ -66,6 +67,8 @@
     
     //
     void make_position_msg(position_msg_t* msg);
+    
+    int enter_safety_mode();
 
 };
 #endif

--- a/Road.cpp	Wed Dec 11 20:12:21 2019 +0000
+++ b/Road.cpp	Thu Dec 12 17:25:27 2019 +0000
@@ -14,7 +14,6 @@
     this->active_cars = 0;
     this->active_car_bits = 0x00;
     this->road_clock = 0;
-    this->msg = new road_msg;
     // clear out our pointer table
     for (int i = 0; i < MAX_CARS_ON_ROAD; ++i)
     {
@@ -108,16 +107,16 @@
 bool Road::simulating()
 {
     if(active_cars != MAX_CARS_ON_ROAD )
-        return true;
+        return 1;
     
     else
     {
         AccCar* car = car_table[active_cars-1];
         assert( car != NULL );
         if(car->position > ROADLENGTH) // TODO use macro 
-            return false; 
+            return 0; 
     }
-    return true;
+    return 1;
 }
 
 // typical getter functions
@@ -136,26 +135,34 @@
     return car_table[active_cars-1];    
 }
 
-bool Road::synchronize(bool simulating)
+int Road::synchronize(int simulating)
 {
-#ifdef ROAD_DEBUG
-            road_pc.printf(ROAD_TAG "synchronizing  cycle %d complete=%d ..."DELIM, road_clock,simulating);
-#endif 
+//#ifdef ROAD_DEBUG
+//            road_pc.printf(ROAD_TAG "synchronizing  cycle %d complete=%d ..."DELIM, road_clock,simulating);
+//#endif 
+        road_msg_t* msg = new road_msg_t;
+        msg->road_id = road_id;
+        msg->simulating = simulating;
+        msg->road_clock = road_clock;
+        this->singleton->add_to_road_to_network_queue(msg);     
 
-        this->msg->road_id = road_id;
-        this->msg->simulating = simulating;
-        this->msg->road_clock = road_clock;
-        this->singleton->add_to_road_to_network_queue(this->msg);     
-    
+   
         //
         road_msg_t* rmsg = this->singleton->get_network_to_road_msg();
-        bool rc = rmsg->simulating;
-        assert(rmsg->road_clock == road_clock);
+        int rc = rmsg->simulating;
         
 #ifdef ROAD_DEBUG
-            road_pc.printf(ROAD_TAG "clock %d id %d sync complete rc=%d"DELIM,rmsg->road_clock,rmsg->road_id,rmsg->simulating);
+            road_pc.printf(ROAD_TAG "my id %d rcvd id %d"DELIM, road_clock, rmsg->road_clock);
 #endif 
+            assert(rmsg->road_clock == road_clock);
+        
     delete rmsg;
     return rc;
 }
-
+void Road::free_msg()
+{  
+    // wait for any last emssages then clean up for next iter
+    ThisThread::sleep_for(1000);
+    this->singleton->clear_queues(); 
+    return;   
+}

--- a/Road.h	Wed Dec 11 20:12:21 2019 +0000
+++ b/Road.h	Thu Dec 12 17:25:27 2019 +0000
@@ -44,9 +44,9 @@
         
     AccCar* get_last_car();
     
-    bool synchronize(bool simulating);
+    int synchronize(int simulating);
     
-    road_msg_t* msg;
+    void free_msg();
     
 private: 
     // for sharing intersections
@@ -65,5 +65,8 @@
     
     //
     int road_clock;
+    
+    //
+    road_msg_t* msg;
 };
 #endif

--- a/main.cpp	Wed Dec 11 20:12:21 2019 +0000
+++ b/main.cpp	Thu Dec 12 17:25:27 2019 +0000
@@ -15,7 +15,11 @@
     
     // setup callbacks and mqtt and wifi connection
     mqtt::instance()->setup_network();
-
+    
+    // wait for partner 
+    ThisThread::sleep_for(2000);
+    
+    
 for(int i = 0; i < REGRESSIONS; i++)
 {
     printf("starting simulation... %d\r\n",i);

--- a/mqtt.cpp	Wed Dec 11 20:12:21 2019 +0000
+++ b/mqtt.cpp	Thu Dec 12 17:25:27 2019 +0000
@@ -146,7 +146,7 @@
     if( msg->road_id != mqtt::instance()->mqtt_id )
     {
 #ifdef DEBUG_MQTT  
-    mqtt_pc.printf(MQTT_TAG"rcvd road %d "DELIM,msg->road_id);
+    mqtt_pc.printf(MQTT_TAG"rcvd road %d "DELIM,msg->road_clock);
 #endif
         // add our message to the queue no fucking clue what happens internally to
         // the message memory thanks mbed os 5 documentation
@@ -169,19 +169,17 @@
 {
     assert(msg != NULL && client != NULL);
     
-//    MQTT::Message message;
-    
 #ifdef DEBUG_MQTT
     mqtt_pc.printf(MQTT_TAG"sending position msg %d "DELIM,msg->road_id);
 #endif
+    
+    MQTT::Message tmessage;
 
-    // might be safest to memcopy this? but well if shit breaks then well fix
-//    memcpy(message.payload,msg,sizeof(position_msg_t));
-    message.payload = (void*)msg;
-    message.payloadlen = sizeof(position_msg_t); 
-    message.qos = MQTT::QOS1;
+    tmessage.payload = (void*)msg;
+    tmessage.payloadlen = sizeof(position_msg_t); 
+    tmessage.qos = MQTT::QOS1;
 
-    int rc = client->publish(POSITION_TOPIC,message);
+    int rc = client->publish(POSITION_TOPIC,tmessage);
     assert(rc == 0);  
 
     return 0;
@@ -191,19 +189,16 @@
 {
     assert(msg != NULL && client != NULL);
     
-//    MQTT::Message message;
-    
 #ifdef DEBUG_MQTT
-    mqtt_pc.printf(MQTT_TAG"sending road msg %d "DELIM,msg->road_id);
+    mqtt_pc.printf(MQTT_TAG"sending road msg %d "DELIM,msg->road_clock);
 #endif
+    MQTT::Message tmessage;
 
-    // might be safest to memcopy thius seems to work
-    //memcpy(message.payload,msg,sizeof(position_msg_t));
-    message.payload = (void*)msg;
-    message.payloadlen = sizeof(road_msg_t); 
-    message.qos = MQTT::QOS1;
+    tmessage.payload = (void*)msg;
+    tmessage.payloadlen = sizeof(road_msg_t); 
+    tmessage.qos = MQTT::QOS1;
 
-    int rc = client->publish(ROAD_TOPIC,message);
+    int rc = client->publish(ROAD_TOPIC,tmessage);
     assert(rc == 0);  
     
     return 0;
@@ -234,7 +229,7 @@
     
     // make a road based of mqtt id
     mqtt_id = 0;
-    if(strcmp(wifi->get_mac_address(),"2c:3a:e8:0b:75:06") == 0){
+    if(strcmp(wifi->get_mac_address(),"2c:3a:e8:0b:8e:77") == 0){
         mqtt_id = 0;
     }
     else{
@@ -280,6 +275,7 @@
             road_msg_t *road_msg = (road_msg_t*)revt.value.p;
             assert(road_msg != NULL);
             send_road_msg(mqtt_message,road_msg);
+            delete road_msg;
         }
 
       client->yield(10);
@@ -317,4 +313,48 @@
     assert(thread != NULL);
     thread->start( callback(this,&mqtt::manage_network) );
     
+}
+
+// before the next run make sure
+// that everything is cleaned up properly
+// the control queue may have one entry due to 
+// the last iteration ending
+void mqtt::clear_queues()
+{
+    while(!position_queue.empty())
+    {
+        osEvent revt = position_queue.get();
+        assert(revt.status == osEventMessage);
+        //printf("cleaned up queue\n");
+            //
+        position_msg_t *msg = (position_msg_t*)revt.value.p;
+        delete msg;     
+    }
+    for(int i = 0; i < 5; i++)
+    {
+        while(!control_queue[i].empty())
+        {
+            osEvent revt = control_queue[i].get();
+            assert(revt.status == osEventMessage);
+            //printf("cleaned up control queue\n");
+            //
+            control_msg_t *msg = (control_msg_t*)revt.value.p;
+            delete msg;     
+        }  
+    }
+       
+    while(!network_to_road_queue.empty())
+    {
+        osEvent evt = network_to_road_queue.get();
+        assert(evt.status == osEventMessage);
+        road_msg_t *message = (road_msg_t*)evt.value.p;
+        delete message;     
+    }  
+    while(!road_to_network_queue.empty())
+    {
+        osEvent evt = road_to_network_queue.get();
+        assert(evt.status == osEventMessage);
+        road_msg_t *message = (road_msg_t*)evt.value.p;
+        delete message;     
+    }  
 }
\ No newline at end of file

--- a/mqtt.h	Wed Dec 11 20:12:21 2019 +0000
+++ b/mqtt.h	Thu Dec 12 17:25:27 2019 +0000
@@ -17,8 +17,8 @@
 #define DELIM "\n"
 #endif
 
-#define PUBLISH_ONLY
-
+//#define PUBLISH_ONLY
+//#define SAFE_CRITICAL
 enum drive_state_t{
  NORMAL_STATE = 0,
  STOPPED_STATE,
@@ -37,7 +37,7 @@
 typedef struct road_msg{
     int road_id;
     int road_clock;
-    bool simulating;
+    int simulating;
 }road_msg_t;
 
 typedef struct control_msg{
@@ -71,16 +71,16 @@
     Thread* thread;
     
     // queue for sending position msessages
-    Queue<position_msg_t, 5> position_queue;
+    Queue<position_msg_t, 15> position_queue;
     
     // queue for sending road msessages
-    Queue<road_msg_t, 1> road_to_network_queue;
+    Queue<road_msg_t, 3> road_to_network_queue;
     
     // queue for sending road msessages
-    Queue<road_msg_t, 1> network_to_road_queue;
+    Queue<road_msg_t, 3> network_to_road_queue;
     
     // queue for control message passsing between mqtt and car threads
-    Queue<control_msg_t, 1> control_queue[MAX_CARS_ON_ROAD];
+    Queue<control_msg_t, 3> control_queue[MAX_CARS_ON_ROAD];
 
     
 public:
@@ -98,7 +98,7 @@
     // setup and tear down API's
     void setup_network();
     void shutdown_network();
-
+    void clear_queues();
     
     static mqtt *instance()
     {
@@ -110,37 +110,39 @@
     // adding to a queue to be sent on mqtt network
     inline void add_to_position_queue(position_msg_t* msg)
     {
-        position_queue.put(msg);
+        position_queue.put(msg,osWaitForever);
     }
     
     // adding to road queue to be sent on mqtt network
     inline void add_to_road_to_network_queue(road_msg_t* msg)
     {
-        road_to_network_queue.put(msg);
+        road_to_network_queue.put(msg,osWaitForever);
     }
     
     // adding to road queue to be sent on mqtt network
     inline void add_to_network_to_road_queue(road_msg_t* msg)
     {
-        network_to_road_queue.put(msg);
+        network_to_road_queue.put(msg,osWaitForever);
     }
     
 
     // adding to a queue to be sent on mqtt network
     inline void add_to_control_queue(int car_id,control_msg_t* msg)
     {
-        control_queue[car_id].put(msg);
+        control_queue[car_id].put(msg,osWaitForever);
     }
     
     // adding to a queue to be sent on mqtt network
     inline control_msg_t* get_control_msg(int car_id)
     {
         // handle the case if no message was received in time
+#ifdef SAFETY_CRITICAL
         if(control_queue[car_id].empty())
         {
             return NULL;   
         }
-        osEvent evt = control_queue[car_id].get();
+#endif
+        osEvent evt = control_queue[car_id].get(osWaitForever);
         assert(evt.status == osEventMessage);
         control_msg_t *message = (control_msg_t*)evt.value.p;
         return message;
@@ -151,7 +153,7 @@
     inline road_msg_t* get_network_to_road_msg()
     {
         // get message from queue
-        osEvent evt = network_to_road_queue.get();
+        osEvent evt = network_to_road_queue.get(osWaitForever);
         assert(evt.status == osEventMessage);
         road_msg_t *message = (road_msg_t*)evt.value.p;
         // we handle ignoring messages in callback

--- a/simulator.cpp	Wed Dec 11 20:12:21 2019 +0000
+++ b/simulator.cpp	Thu Dec 12 17:25:27 2019 +0000
@@ -11,7 +11,7 @@
 
 #define MAGIC_NUMBER 2
 
-#define SIM_DEBUG
+//#define SIM_DEBUG
 #define SIM_TAG "[SIM] "
 
 Serial pc(USBTX, USBRX);
@@ -74,7 +74,7 @@
     {
             AccCar* car = road->get_car(i);
             assert(car != NULL);
-            pc.printf(SIM_TAG "Car %d Road %d position %d : speed %d cycles: %d car_cycles %d "DELIM, i,road->get_road_id(), car->position,car->speed, road->get_road_clock(), car->get_cycles()); 
+            pc.printf(SIM_TAG "Car %d Road %d position %d speed %d target %d cycles %d car_cycles %d "DELIM, i,road->get_road_id(), car->position,car->speed,car->get_target_speed(),road->get_road_clock(), car->get_cycles()); 
     }
 //#ifdef SIM_DEBUG
             pc.printf(SIM_TAG " -------------- "DELIM);
@@ -95,7 +95,7 @@
 
     // ------------------------------------------------------------------------------
     // seed rand number generator
-    srand(mqtt_singleton->mqtt_id);
+    //srand(mqtt_singleton->mqtt_id);
     
     // make new road based off of ID
     Road* road = new Road(mqtt_singleton->mqtt_id, mqtt_singleton);
@@ -107,8 +107,8 @@
     
     stopwatch.reset();
     
-    bool other_simulating=false;
-    bool simulating = true; 
+    int other_simulating;
+    int simulating; 
     
     // 
     //road->synchronize(simulating);
@@ -164,7 +164,7 @@
     
     // ----------------------------------------------------------------------
     road->DESTROY_ALL_CARS();
-    delete road->msg;
+    road->free_msg();
     delete road;
     return 0;
 }