From fc28a91334c46be31d426955d12357265fcad166 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 24 Feb 2023 10:25:22 +0000 Subject: [PATCH] Added steering calibration functionality --- CMakeLists.txt | 12 ++- docs/steering_feedback.txt | 1 + docs/steering_request.txt | 1 + include/calibration.hpp | 31 +++++++ include/control/steering_control_frame.hpp | 4 + include/simple_adas.hpp | 3 + include/status/steering_frame.hpp | 7 +- src/control/steering_control_frame.cpp | 14 ++- src/example_calibration.cpp | 103 +++++++++++++++++++++ src/simple_adas.cpp | 10 ++ src/status/steering_frame.cpp | 14 ++- 11 files changed, 194 insertions(+), 6 deletions(-) create mode 100644 docs/steering_feedback.txt create mode 100644 docs/steering_request.txt create mode 100644 include/calibration.hpp create mode 100644 src/example_calibration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ed1dc2a..fd5a6b3 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/docs/steering_feedback.txt b/docs/steering_feedback.txt new file mode 100644 index 0000000..b1f53c8 --- /dev/null +++ b/docs/steering_feedback.txt @@ -0,0 +1 @@ +1.0364 0.0064 0.0002 \ No newline at end of file diff --git a/docs/steering_request.txt b/docs/steering_request.txt new file mode 100644 index 0000000..b874d05 --- /dev/null +++ b/docs/steering_request.txt @@ -0,0 +1 @@ +0.9475 -0.0039 -0.00006 diff --git a/include/calibration.hpp b/include/calibration.hpp new file mode 100644 index 0000000..d4b9088 --- /dev/null +++ b/include/calibration.hpp @@ -0,0 +1,31 @@ +#ifndef CALIBRATION_HPP + +#include +#include +#include +#include +#include +#include +#include + +std::vector read_file( std::ifstream &file ) +{ + std::vector data; + + std::copy( std::istream_iterator(file), + std::istream_iterator(), + std::back_inserter(data) ); + + return data; +} + + +float calibration( const std::vector &calib, const float input ) +{ + float output = 0.0f; + for( size_t i=0; i& calibration ); + private: /** * @brief Generate can_frame for transmission. @@ -43,6 +45,8 @@ namespace adas_api{ const StatusFrame& _statusFrame; float _angle = 0.f; + + std::vector _calibration; }; }; diff --git a/include/simple_adas.hpp b/include/simple_adas.hpp index 948c560..0297be9 100644 --- a/include/simple_adas.hpp +++ b/include/simple_adas.hpp @@ -20,6 +20,9 @@ namespace adas_api class SimpleAdas : protected Adas { public: + void set_steering_request_calibration( const std::vector& calibration ); + void set_steering_feedback_calibration( const std::vector& calibration ); + // === axle control === /** * @brief Set the front axle target rpm. diff --git a/include/status/steering_frame.hpp b/include/status/steering_frame.hpp index 13c6033..5e70ed9 100644 --- a/include/status/steering_frame.hpp +++ b/include/status/steering_frame.hpp @@ -1,9 +1,10 @@ #ifndef STEERING_FRAME_HPP #define STEERING_FRAME_HPP -#pragma once + #include "process_frame.hpp" #include +#include 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& 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 _calibration; }; }; diff --git a/src/control/steering_control_frame.cpp b/src/control/steering_control_frame.cpp index 2a54fe4..5d0f255 100644 --- a/src/control/steering_control_frame.cpp +++ b/src/control/steering_control_frame.cpp @@ -1,9 +1,15 @@ #include "control/steering_control_frame.hpp" #include +#include #include +void adas_api::SteeringControlFrame::set_calibration( const std::vector& 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; } } diff --git a/src/example_calibration.cpp b/src/example_calibration.cpp new file mode 100644 index 0000000..dfa191a --- /dev/null +++ b/src/example_calibration.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "simple_adas.hpp" + +std::vector 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 data; + + std::copy( std::istream_iterator(file), + std::istream_iterator(), + 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; +} diff --git a/src/simple_adas.cpp b/src/simple_adas.cpp index 103034a..92d78f7 100644 --- a/src/simple_adas.cpp +++ b/src/simple_adas.cpp @@ -4,6 +4,16 @@ #include +void adas_api::SimpleAdas::set_steering_request_calibration( const std::vector& calibration ) +{ + this->steeringControl.set_calibration( calibration ); +} + +void adas_api::SimpleAdas::set_steering_feedback_calibration( const std::vector& calibration ) +{ + static_cast(this)->steering.set_calibration( calibration ); +} + void adas_api::SimpleAdas::set_front_rpm( const float rpm ) { this->frontControl.set_speed( rpm ); diff --git a/src/status/steering_frame.cpp b/src/status/steering_frame.cpp index 0191f7f..b8d166c 100644 --- a/src/status/steering_frame.cpp +++ b/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& 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 )