Fix issue where using brakes at all when the car is moving triggered …

…ebrake. Fix issue where car wont finish.

include/adas.hpp

@@ -33,9 +33,9 @@ namespace adas_api{
         WheelSpeedsFrame wheelSpeeds;
 
         // outgoing frames
-        AxleControlFrame frontControl { this->frontAxle, this->brakes, this->wheelSpeeds, this->status };
-        AxleControlFrame rearControl { this->rearAxle, this->brakes, this->wheelSpeeds, this->status, false };
         BrakeControlFrame brakeControl;
+        AxleControlFrame frontControl { this->frontAxle, this->brakes, this->wheelSpeeds, this->status, this->brakeControl };
+        AxleControlFrame rearControl { this->rearAxle, this->brakes, this->wheelSpeeds, this->status, this->brakeControl, false };
         StatusControlFrame statusControl { this->frontAxle, this->rearAxle, this->status };
         SteeringControlFrame steeringControl { this->steering, this->status };
 

include/control/axle_control_frame.hpp

@@ -21,6 +21,7 @@ namespace adas_api{
                             const BrakeFrame &brakeFrame,
                             const WheelSpeedsFrame &speedsFrame, 
                             const StatusFrame &statusFrame,
+                            const BrakeControlFrame &brakeControl,
                             const bool front=true );
         virtual ~AxleControlFrame() = default;
 
@@ -62,6 +63,7 @@ namespace adas_api{
         const BrakeFrame &_brakeFrame;
         const WheelSpeedsFrame &_speedsFrame;
         const StatusFrame &_statusFrame;
+        const BrakeControlFrame &_brakeControl;
 
         const bool _front;
         float _speed = 0.f;

include/control/brake_control_frame.hpp

@@ -16,6 +16,7 @@ namespace adas_api{
 
         const float get_front() const;
         const float get_rear() const;
+        const bool on() const;
 
     private:
         /**

include/control/status_control_frame.hpp

@@ -36,6 +36,11 @@ namespace adas_api{
             _mission_status = status;
         }
 
+        const MissionStatus get_mission_status() const
+        {
+            return this->_mission_status;
+        }
+
     private:
         /**
         * @brief  Generate can_frame for brake

src/control/axle_control_frame.cpp

@@ -32,7 +32,8 @@ can_frame adas_api::AxleControlFrame::_process() const
     float torque = 0.f;
 
     /* check that we have up to date information otherwise set safe values */
-    if( this->_statusFrame.is_recent() && 
+    if( this->_brakeControl.on() == false && 
+        this->_statusFrame.is_recent() && 
         this->_statusFrame.as_state() == adas_api::StatusFrame::AsState::DRIVING &&   // in driving mode
         this->_brakeFrame.is_recent() && 
         this->_brakeFrame.on() == false &&                                  // brakes are off
@@ -103,12 +104,14 @@ adas_api::AxleControlFrame::AxleControlFrame( const adas_api::AxleTorqueFrame &t
                                     const adas_api::BrakeFrame &brakeFrame,
                                     const adas_api::WheelSpeedsFrame &speedsFrame,
                                     const adas_api::StatusFrame &statusFrame,
+                                    const adas_api::BrakeControlFrame &brakeControl,
                                     const bool front ) : 
     ControlFrame( front ? CANDATA_AI2_VCU_DRIVE_F_FRAME_ID : CANDATA_AI2_VCU_DRIVE_R_FRAME_ID ),
     _torqueFrame( torqueFrame ),
     _brakeFrame( brakeFrame ),
     _speedsFrame( speedsFrame ),
     _statusFrame( statusFrame ),
+    _brakeControl( brakeControl ),
     _front( front )
 {
     if( front != torqueFrame.is_front() )

src/control/brake_control_frame.cpp

@@ -29,6 +29,11 @@ const float adas_api::BrakeControlFrame::get_rear() const
     return this->_rear;
 }
 
+const bool adas_api::BrakeControlFrame::on() const
+{
+    return this->_front > 0.f || this->_rear > 0.f;
+}
+
 can_frame adas_api::BrakeControlFrame::_process() const
 {
     float front = this->_front;

src/example_straight_line.cpp

@@ -27,30 +27,38 @@ int main( int argc, char* argv[] )
         switch( state )
         {
             case IDLE:
+                std::cout << "IDLE" << std::endl;
                 adas.set_rpm( 0 );                      // don't move
-                adas.set_brakes( 100 );
+                adas.set_brakes( 0 );
 
                 if( adas.go() )                     // wait for mission to start
                     state = SAVE_START;
                 break;
 
             case SAVE_START:
+                std::cout << "SAVE_START" << std::endl;
                 for( int i=0; i<4; ++i )                // save starting wheel counts
                     startingCount[i] = adas.wheel_counts().count( i );
 
                 state = ACCELERATE;
                 break;
 
             case ACCELERATE:
-                adas.set_rpm( 10 );                     // move
+                std::cout << "ACCELERATE" << std::endl;
+                adas.set_rpm( 250 );                     // move
                 adas.set_brakes( 0 );
 
                 for( int i=0; i<4; ++i )                // check distance traveled
-                    if( std::abs( adas.wheel_counts().count( i ) - startingCount[i] ) >= 100 )
+                {
+                    std::cout << adas.wheel_counts().count( i ) - startingCount[i] << " ";
+                    if( std::abs( adas.wheel_counts().count( i ) - startingCount[i] ) >= 5000 )
                         state = DECELERATE;             // start stopping when distance is reached
+                }
+                std::cout << std::endl;
                 break;
 
             case DECELERATE:
+                std::cout << "DECELERATE" << std::endl;
                 adas.set_rpm( 0 );                      // stop
                 adas.set_brakes( 100 );
 
@@ -59,6 +67,7 @@ int main( int argc, char* argv[] )
                 break;
 
             case DONE:
+                std::cout << "DONE" << std::endl;
                 adas.finish();                          // car in safe state
                 break;
         }

src/simple_adas.cpp

@@ -7,11 +7,13 @@
 void adas_api::SimpleAdas::set_front_rpm( const float rpm )
 {
     this->frontControl.set_speed( rpm );
+    this->frontControl.set_torque( 200 );
 }
 
 void adas_api::SimpleAdas::set_rear_rpm( const float rpm )
 {
     this->rearControl.set_speed( rpm );
+    this->rearControl.set_torque( 200 );
 }
 
 void adas_api::SimpleAdas::set_rpm( const float rpm )
@@ -130,72 +132,53 @@ void adas_api::SimpleAdas::write()
     {
         case StatusFrame::AsState::OFF: 
             std::cout << "AS OFF" << std::endl;
+            if( this->status().ami_state() >= StatusFrame::AmiState::ACCELERATION && 
+                this->status().ami_state() <= StatusFrame::AmiState::AUTONOMOUS_DEMO )
+            {
+                this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED );
+            }
+            else
+            {
+                this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED );
+            }
+            this->statusControl.set_drive( false );
+            this->statusControl.set_estop( false );
             break;
         case StatusFrame::AsState::READY:
             std::cout << "AS READY" << std::endl;
+            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED );
+            this->statusControl.set_drive( false );
+            this->statusControl.set_estop( false );
             break;
         case StatusFrame::AsState::DRIVING:
             std::cout << "AS DRIVING" << std::endl;
+            if( this->statusControl.get_mission_status() != StatusControlFrame::MissionStatus::FINISHED )
+                this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED );
+            this->statusControl.set_drive( true );
+
+            this->statusControl.set_estop( false );
             break;
         case StatusFrame::AsState::FINISHED:
             std::cout << "AS FINISHED" << std::endl;
+            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED );
+            this->statusControl.set_drive( false );
+            this->statusControl.set_estop( false );
             break;
         case StatusFrame::AsState::EMERGENCY:
             std::cout << "AS EMERGENCY" << std::endl;
+            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED );
+            this->statusControl.set_drive( false );
+            this->statusControl.set_estop( true );
             break;
         default: 
             std::cout << "AS default" << std::endl;
-            break;
-    }
-
-    /* mission status */
-    switch( this->status().as_state() )
-    {
-        case StatusFrame::AsState::OFF: 
-            [[fallthrough]];
-        case StatusFrame::AsState::READY:
-            [[fallthrough]];
-        case StatusFrame::AsState::DRIVING:
-            this->statusControl.set_mission_status( this->status().ami_state() == StatusFrame::AmiState::NOT_SELECTED ?
-                                                    StatusControlFrame::MissionStatus::NOT_SELECTED :
-                                                    StatusControlFrame::MissionStatus::SELECTED );
-            // possible bug here regarding getting the vehicle into finished state    
-            break;
-
-        case StatusFrame::AsState::FINISHED:
-            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::FINISHED );
-            break;
-
-        case StatusFrame::AsState::EMERGENCY:
-            [[fallthrough]];
-        default: 
-            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::FINISHED );
-            break;
-    }
-
-    switch( this->status().as_state() )
-    {
-        case StatusFrame::AsState::OFF: 
-            [[fallthrough]];
-        case StatusFrame::AsState::READY: 
-            [[fallthrough]];
-        case StatusFrame::AsState::FINISHED:
+            this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED );
             this->statusControl.set_drive( false );
             this->statusControl.set_estop( false );
             break;
-
-        case StatusFrame::AsState::DRIVING:
-            this->statusControl.set_drive( this->status().go() );
-            this->statusControl.set_estop( false );
-            break;
-
-        case StatusFrame::AsState::EMERGENCY:
-            [[fallthrough]];
-        default: 
-            this->statusControl.set_drive( false );
-            this->statusControl.set_estop( true );
-            break;
     }
 
+    std::cout << "AMI state: " << this->status().ami_state() << std::endl;
+
     Adas::write();
 }