Added steering calibration functionality

CMakeLists.txt

@@ -35,7 +35,10 @@ SET( LIBRARY_OUTPUT_PATH lib )
 # autogenerate code from the DBC file
 file( MAKE_DIRECTORY src_autogen )    
 add_custom_command( OUTPUT src_autogen/candata.h src_autogen/candata.c
-        COMMAND python3 -m cantools generate_c_source ${CMAKE_CURRENT_SOURCE_DIR}/docs/candata.dbc -o src_autogen
+        COMMAND python3 -m cantools generate_c_source 
+            --use-float
+            ${CMAKE_CURRENT_SOURCE_DIR}/docs/candata.dbc 
+            -o src_autogen
         MAIN_DEPENDENCY docs/candata.dbc
         COMMENT "Autogenerate code from DBC file" )
 
@@ -136,10 +139,15 @@ target_link_libraries( example_frame_reader PRIVATE candata status_frames )
 add_executable( example_straight_line src/example_straight_line.cpp )
 target_link_libraries( example_straight_line PUBLIC simple_adas )
 
-
 add_executable( example_wiggle src/example_wiggle.cpp )
 target_link_libraries( example_wiggle PUBLIC simple_adas )
 
+add_executable( example_calibration src/example_calibration.cpp )
+target_link_libraries( example_calibration PUBLIC simple_adas )
+
+add_executable( accel src/accel.cpp )
+target_link_libraries( accel PUBLIC simple_adas )
+
 # demo
 add_library( demo_ui STATIC src/demo_ui.cpp )
 target_include_directories( demo_ui PRIVATE include )

docs/steering_feedback.txt

@@ -0,0 +1 @@
+1.0364 0.0064 0.0002

docs/steering_request.txt

@@ -0,0 +1 @@
+0.9475 -0.0039 -0.00006

include/calibration.hpp

@@ -0,0 +1,31 @@
+#ifndef CALIBRATION_HPP
+
+#include <fstream>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <iterator>
+#include <sstream>
+#include <cmath>
+
+std::vector<float> read_file( std::ifstream &file )
+{
+    std::vector<float> data;
+
+    std::copy( std::istream_iterator<float>(file), 
+               std::istream_iterator<float>(), 
+               std::back_inserter(data) );
+
+    return data;
+}
+
+
+float calibration( const std::vector<float> &calib, const float input )
+{
+    float output = 0.0f;
+    for( size_t i=0; i<calib.size(); ++i )
+        output += calib[i] * powf( input, i );
+    return output;
+}
+
+#endif // CALIBRATION_HPP

include/control/steering_control_frame.hpp

@@ -24,6 +24,8 @@ namespace adas_api{
 
         void set_angle( const float angle );
 
+        void set_calibration( const std::vector<float>& calibration );
+
     private:
         /**
          * @brief Generate can_frame for transmission.
@@ -43,6 +45,8 @@ namespace adas_api{
         const StatusFrame& _statusFrame;
 
         float _angle = 0.f;
+
+        std::vector<float> _calibration;
     };
 };
 

include/simple_adas.hpp

@@ -20,6 +20,9 @@ namespace adas_api
 class SimpleAdas : protected Adas
 {
   public:
+    void set_steering_request_calibration( const std::vector<float>& calibration );
+    void set_steering_feedback_calibration( const std::vector<float>& calibration );
+
     // === axle control ===
     /**
      * @brief Set the front axle target rpm.

include/status/steering_frame.hpp

@@ -1,9 +1,10 @@
 #ifndef STEERING_FRAME_HPP
 #define STEERING_FRAME_HPP
-#pragma once
+
 #include "process_frame.hpp"
 
 #include <candata.h>
+#include <vector>
 
 namespace adas_api{
     class SteeringFrame : public ProcessFrame
@@ -14,6 +15,8 @@ namespace adas_api{
         float max() const;
         float direction() const;
 
+        void set_calibration( const std::vector<float>& calibration );
+
         /**
          * @brief Construct a new Steering Frame object
          * 
@@ -29,6 +32,8 @@ namespace adas_api{
 
         candata_vcu2_ai_steer_t data;
         const float _direction;
+
+        std::vector<float> _calibration;
     };
 };
 

src/control/steering_control_frame.cpp

@@ -1,9 +1,15 @@
 #include "control/steering_control_frame.hpp"
 
 #include <candata.h>
+#include <calibration.hpp>
 
 #include <stdexcept>
 
+void adas_api::SteeringControlFrame::set_calibration( const std::vector<float>& calibration )
+{
+    this->_calibration = calibration;
+}
+
 /**
  * @brief 
  * 
@@ -41,7 +47,13 @@ can_frame adas_api::SteeringControlFrame::_process() const
                 const float mx = std::abs( this->_steeringFrame.max() );
                 // limit angle to max steering angle
                 // use same direction as steering frame
-                angle = std::min( mx, std::max( -mx, this->_angle * this->_steeringFrame.direction() ) );
+
+                if( this->_calibration.empty() )
+                    angle = this->_angle;
+                else
+                    angle = calibration( this->_calibration, this->_angle );
+
+                angle = std::min( mx, std::max( -mx, angle * this->_steeringFrame.direction() ) );
                 break;
         }
     }

src/example_calibration.cpp

@@ -0,0 +1,103 @@
+#include <iostream>
+#include <string>
+#include <thread>
+#include <vector>
+#include <sstream>
+#include <algorithm>
+#include <iterator>
+#include <fstream>
+
+#include "simple_adas.hpp"
+
+std::vector<float> read_calibration_file( const std::string filename )
+{
+    std::ifstream file( filename );
+    if( !file.good() )
+        throw std::runtime_error( "Failed to open file: " + filename );
+
+    std::vector<float> data;
+
+    std::copy( std::istream_iterator<float>(file), 
+               std::istream_iterator<float>(), 
+               std::back_inserter(data) );
+
+    return data;
+}
+
+int main( int argc, char* argv[] )
+{
+    adas_api::SimpleAdas adas { argc >= 2 ? argv[1] : "can0" };
+
+    auto steeringRequestCalib = read_calibration_file( "docs/steering_request.txt" );
+    auto steeringFeedbackCalib = read_calibration_file( "docs/steering_feedback.txt" );
+
+    adas.set_steering_request_calibration( steeringRequestCalib );
+    adas.set_steering_feedback_calibration( steeringFeedbackCalib );
+
+    enum States
+    {
+        IDLE,
+        LEFT,
+        RIGHT,
+        CENTER,
+        DONE
+    } state = IDLE;
+
+    using clock = std::chrono::steady_clock;
+    clock::time_point timer;
+
+    const auto timeout = std::chrono::seconds( 3 );
+
+    int count = 0;
+
+    while( true )
+    {
+        States prev = state;
+
+        adas.read();
+
+        if( adas.go() )
+        {
+            switch( state )
+            {
+                case IDLE:
+                    state = LEFT;
+                    break;
+
+                case LEFT:
+                    if( clock::now() - timer >= timeout )
+                        state = RIGHT;
+                    adas.set_angle( 20.f );
+                    break;
+
+                case RIGHT:
+                    if( clock::now() - timer >= timeout )
+                        state = CENTER;
+                    adas.set_angle( -20.f );
+                    break;
+
+                case CENTER:
+                    if( clock::now() - timer >= timeout )
+                        state = DONE;
+                    adas.set_angle( 0.f );
+                    break;
+
+                case DONE:
+                    adas.finish();
+                    break;
+            }
+        }
+        else
+            state = IDLE;
+
+        if( state != prev )
+            timer = clock::now();
+
+
+        adas.write();
+
+        std::this_thread::sleep_for( std::chrono::milliseconds(10) );
+    }
+
+	return 0;
+}

src/simple_adas.cpp

@@ -4,6 +4,16 @@
 
 #include <thread>
 
+void adas_api::SimpleAdas::set_steering_request_calibration( const std::vector<float>& calibration )
+{
+    this->steeringControl.set_calibration( calibration );
+}
+
+void adas_api::SimpleAdas::set_steering_feedback_calibration( const std::vector<float>& calibration )
+{
+    static_cast<adas_api::Adas*>(this)->steering.set_calibration( calibration );
+}
+
 void adas_api::SimpleAdas::set_front_rpm( const float rpm )
 {
     this->frontControl.set_speed( rpm );

src/status/steering_frame.cpp

@@ -1,8 +1,19 @@
 #include "status/steering_frame.hpp"
+#include "calibration.hpp"
+
+void adas_api::SteeringFrame::set_calibration( const std::vector<float>& calibration )
+{
+    this->_calibration = calibration;
+}
 
 float adas_api::SteeringFrame::angle() const
 {
-    return this->_direction * candata_vcu2_ai_steer_angle_decode( data.angle );
+    const float raw = candata_vcu2_ai_steer_angle_decode( data.angle );
+
+    if( this->_calibration.empty() )
+        return this->_direction * raw;
+
+    return this->_direction * calibration( this->_calibration, raw );
 }
 
 float adas_api::SteeringFrame::requested() const
@@ -33,7 +44,6 @@ void adas_api::SteeringFrame::_print( std::ostream& os ) const
     os << "Angle " << angle() << " of " << requested() << "/" << max();
 }
 
-
 adas_api::SteeringFrame::SteeringFrame( bool righthandrule ) : 
             ProcessFrame( CANDATA_VCU2_AI_STEER_FRAME_ID ),
             _direction( righthandrule ? -1.0f : 1.0f )