From 45d10e11100f8529fac1359603cddbf1e2249866 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 8 Feb 2023 13:48:50 +0000 Subject: [PATCH] Get simple adas working, add doctstrings, add right hand rule code --- CMakeLists.txt | 26 ++--- docs/candata.dbc | 6 +- include/adas.hpp | 90 ++++++++-------- include/control/steering_control_frame.hpp | 14 ++- include/demo_ui.hpp | 7 +- include/simple_adas.hpp | 119 ++++++++++++++------- include/status/axle_torque_frame.hpp | 2 +- src/adas.cpp | 53 ++++++++- src/control/steering_control_frame.cpp | 10 +- src/demo.cpp | 4 +- src/demo_ui.cpp | 12 ++- src/simple_adas.cpp | 88 ++++++++++++++- src/status/steering_frame.cpp | 41 ++++++- 13 files changed, 349 insertions(+), 123 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9555a83..ed1dc2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,24 +134,24 @@ target_link_libraries( example_frame_reader PRIVATE candata status_frames ) #target_link_libraries( example_control PRIVATE candata control_frames status_frames ) add_executable( example_straight_line src/example_straight_line.cpp ) -target_link_libraries( example_straight_line PRIVATE simple_adas ) +target_link_libraries( example_straight_line PUBLIC simple_adas ) add_executable( example_wiggle src/example_wiggle.cpp ) -target_link_libraries( example_wiggle PRIVATE simple_adas ) +target_link_libraries( example_wiggle PUBLIC simple_adas ) # demo -add_library( demo_ui src/demo_ui.cpp ) -target_include_directories( demo_ui PUBLIC include ) +add_library( demo_ui STATIC src/demo_ui.cpp ) +target_include_directories( demo_ui PRIVATE include ) target_link_libraries( demo_ui - PRIVATE ftxui::screen - PRIVATE ftxui::dom - PRIVATE ftxui::component - PRIVATE candata adas ) + PUBLIC ftxui::screen + PUBLIC ftxui::dom + PUBLIC ftxui::component + PUBLIC candata adas ) add_executable( demo src/demo.cpp ) -target_link_libraries( demo - PRIVATE ftxui::screen - PRIVATE ftxui::dom - PRIVATE ftxui::component - PRIVATE candata adas demo_ui ) \ No newline at end of file +target_link_libraries( demo demo_ui ) + #PRIVATE ftxui::screen + #PRIVATE ftxui::dom + #PRIVATE ftxui::component + #PRIVATE candata adas demo_ui ) \ No newline at end of file diff --git a/docs/candata.dbc b/docs/candata.dbc index 5014df2..41c9ca9 100644 --- a/docs/candata.dbc +++ b/docs/candata.dbc @@ -55,9 +55,9 @@ BO_ 1312 VCU2AI_Status: 8 VCU SG_ BRAKE_PLAUSIBILITY_FAULT : 50|1@1+ (1,0) [0|1] "__F__" Vector__XXX BO_ 1315 VCU2AI_Steer: 6 VCU - SG_ ANGLE : 0|16@1- (-0.1,0) [-21|21] "deg" Vector__XXX - SG_ ANGLE_MAX : 16|16@1+ (-0.1,0) [0|21] "deg" Vector__XXX - SG_ ANGLE_REQUEST : 32|16@1- (-0.1,0) [-21|21] "deg" Vector__XXX + SG_ ANGLE : 0|16@1- (0.1,0) [-21|21] "deg" Vector__XXX + SG_ ANGLE_MAX : 16|16@1+ (0.1,0) [0|21] "deg" Vector__XXX + SG_ ANGLE_REQUEST : 32|16@1- (0.1,0) [-21|21] "deg" Vector__XXX BO_ 1313 VCU2AI_Drive_F: 6 VCU SG_ FRONT_AXLE_TRQ : 0|16@1- (0.1,0) [-195|195] "Nm" Vector__XXX diff --git a/include/adas.hpp b/include/adas.hpp index 1ff4439..39b3d5c 100644 --- a/include/adas.hpp +++ b/include/adas.hpp @@ -4,10 +4,19 @@ #include #include -#include #include namespace adas_api{ + /** + * @brief Quickly define and setup all the status and control frames + * needed for the ADAS system. + * + * @details If you want to have direct access to the individual frames + * then this is the class to use. The individual frames will prevent + * sending dangerous values to the vehicle but you are responsible + * for managing the correct sequence etc to get the vehicle to + * respond correctly. + */ class Adas { protected: @@ -30,6 +39,12 @@ namespace adas_api{ StatusControlFrame statusControl { this->frontAxle, this->rearAxle, this->status }; SteeringControlFrame steeringControl { this->steering, this->status }; + /** + * @brief A map of all the incoming frames. + * + * @details A map is used with the frame ID as the key to allow + * for fast lookup by the read() method. + */ const std::map statusFrames { { this->frontAxle.frameId, &this->frontAxle }, @@ -41,6 +56,9 @@ namespace adas_api{ { this->wheelSpeeds.frameId, &this->wheelSpeeds } }; + /** + * @brief An array of all the outgoing frames. + */ const std::array controlFrames { &this->frontControl, @@ -50,52 +68,38 @@ namespace adas_api{ &this->steeringControl }; - explicit Adas( const std::string& device ) : _canSocket( can::connect( device ) ) - { - // set socket to non-blocking - const int existingFlags = fcntl( this->_canSocket, F_GETFL, 0 ); - fcntl( this->_canSocket, F_SETFL, existingFlags | O_NONBLOCK ); - } + /** + * @brief Attempt to read the specified number of frames from the + * CAN bus. + * + * @details The Adas class sets up the CAN socket to be non-blocking + * so this method will not block. If there are no frames to read + * then the method will return early. + * + * @param n Number of frames to attempt to read. + */ + virtual void read( const int n=50 ); - Adas( const Adas& ) = delete; - virtual ~Adas() - { - try - { - can::close( this->_canSocket ); - } - catch( const can::error& ) - {} - } + /** + * @brief Write all the control frames to the CAN bus. + */ + virtual void write(); - virtual void read( const int n=50 ) - { - try - { - for( int i=0; i_canSocket ); + /** + * @brief Construct a new Adas object + * + * @param device The name of the CAN device to use. + */ + explicit Adas( const std::string& device ); - if( auto statusFrame = this->statusFrames.find( rawFrame.can_id ); - statusFrame != this->statusFrames.end() ) - { - statusFrame->second->process( rawFrame ); - } - } - } - catch( const can::error& ) - { - // no more frames to read - } - } + Adas( const Adas& ) = delete; - virtual void write() - { - for( auto controlFrame : this->controlFrames ) - { - can::write( this->_canSocket, controlFrame->process() ); - } - } + /** + * @brief Destroy the Adas object + * + * @details The destructor will attempt to close the CAN socket. + */ + virtual ~Adas(); }; }; diff --git a/include/control/steering_control_frame.hpp b/include/control/steering_control_frame.hpp index ea9e103..d3eb604 100644 --- a/include/control/steering_control_frame.hpp +++ b/include/control/steering_control_frame.hpp @@ -1,6 +1,6 @@ #ifndef STEERING_CONTROL_FRAME_HPP #define STEERING_CONTROL_FRAME_HPP -#pragma once + #include "control/control_frame.hpp" #include "status/steering_frame.hpp" #include "status/status_frame.hpp" @@ -9,13 +9,21 @@ namespace adas_api{ class SteeringControlFrame : public ControlFrame { public: + /** + * @brief Construct a new Steering Control Frame object + * + * @remarks Will use the same direction rules as the steering frame. + * I.e. if righthandrule is set for steering frame then the + * control frame will also use positive angles for CCW. + * + * @param steeringFrame + * @param statusFrame + */ SteeringControlFrame( const SteeringFrame &steeringFrame, const StatusFrame &statusFrame ); virtual ~SteeringControlFrame() override = default; void set_angle( const float angle ); - static float steering_angle_limit( const float angle ); - private: /** * @brief Generate can_frame for transmission. diff --git a/include/demo_ui.hpp b/include/demo_ui.hpp index 3f68134..f09ffa6 100644 --- a/include/demo_ui.hpp +++ b/include/demo_ui.hpp @@ -11,10 +11,10 @@ #include +using namespace ftxui; + namespace demo_ui { - using namespace ftxui; - struct MenuConfig { std::vector states; @@ -185,8 +185,7 @@ namespace demo_ui return Container::Vertical( { Slider( "", &config.value, static_cast( config.minimum*config.scaling ), - static_cast( config.limit*config.scaling ), - static_cast( config.scaling ) ), + static_cast( config.limit*config.scaling ), 1 ), Renderer( [&]() { return hbox( diff --git a/include/simple_adas.hpp b/include/simple_adas.hpp index d06cadd..948c560 100644 --- a/include/simple_adas.hpp +++ b/include/simple_adas.hpp @@ -5,47 +5,84 @@ #include -namespace adas_api{ - class SimpleAdas : protected Adas - { - public: - /** axle control */ - inline void set_front_rpm( const float rpm ) { this->frontControl.set_speed( rpm ); }; - inline void set_rear_rpm( const float rpm ) { this->rearControl.set_speed( rpm ); }; - inline void set_rpm( const float rpm ) - { - this->set_front_rpm( rpm ); - this->set_rear_rpm( rpm ); - } - - /** brake control */ - inline void set_front_brake( const float percentage ) { this->brakeControl.set_front( percentage ); } - inline void set_rear_brake( const float percentage ) { this->brakeControl.set_rear( percentage ); } - inline void set_brakes( const float percentage ) { this->brakeControl.set_brakes( percentage ); } - - /** steering control */ - inline void set_angle( const float degrees ) { this->steeringControl.set_angle( degrees ); } - - /** status control */ - inline bool go() const { return this->status().go(); } - inline void finish() { this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::FINISHED ); } - inline void ebrake( bool b=true ) { this->statusControl.set_estop(b); } - - /** status access */ - const AxleTorqueFrame& front_axle() const { return this->frontAxle; } - const AxleTorqueFrame& rear_axle() const { return this->rearAxle; } - const BrakeFrame& brakes() const { return static_cast(this)->brakes; } - const StatusFrame& status() const { return static_cast(this)->status; } - const SteeringFrame& steering() const { return static_cast(this)->steering; } - const WheelCountsFrame& wheel_counts() const { return this->wheelCounts; } - const WheelSpeedsFrame& wheel_speeds() const { return this->wheelSpeeds; } - - explicit SimpleAdas( const std::string& device ); - SimpleAdas( const SimpleAdas& ) = delete; - - virtual void read( int n=50 ) override { return Adas::read( n ); } - virtual void write() override; - }; +namespace adas_api +{ +/** + * @brief A simplified interface to setup and control the ADAS system. + * + * @details This class is a wrapper around the Adas class that provides + * a simplified interface to the ADAS system. It gives complete access + * to all the incoming status information from the vehicle but only + * provides a subset of control commands. + * The class will automatically handle the correct sequence of + * commands to get the vehicle to start moving (e.g. state transitions). + */ +class SimpleAdas : protected Adas +{ + public: + // === axle control === + /** + * @brief Set the front axle target rpm. + * + * @details Torque is automatically set. + * + * @param rpm The target rpm. + */ + void set_front_rpm( const float rpm ); + + /** + * @brief Set the rear axle target rpm. + * + * @details Torque is automatically set. + * + * @param rpm The target rpm. + */ + void set_rear_rpm( const float rpm ); + + /** + * @brief Set the target rpm for both axles. + * + * @details Torque is automatically set. + * + * @param rpm The target rpm. + */ + void set_rpm( const float rpm ); + + /** brake control */ + void set_front_brake( const float percentage ); + void set_rear_brake( const float percentage ); + void set_brakes( const float percentage ); + + // === steering control === + /** + * @brief Set the target steering angle. + * + * @param degrees + * + * @warning Steering direction is following ROS convention. + */ + void set_angle( const float degrees ); + + /** status control */ + bool go() const; + void finish(); + void ebrake( bool b = true ); + + /** status access */ + const AxleTorqueFrame &front_axle() const; + const AxleTorqueFrame &rear_axle() const; + const BrakeFrame &brakes() const; + const StatusFrame &status() const; + const SteeringFrame &steering() const; + const WheelCountsFrame &wheel_counts() const; + const WheelSpeedsFrame &wheel_speeds() const; + + using Adas::Adas; + SimpleAdas( const SimpleAdas & ) = delete; + + using Adas::read; + virtual void write() override; }; +}; // namespace adas_api #endif // SIMPLE_ADAS_HPP diff --git a/include/status/axle_torque_frame.hpp b/include/status/axle_torque_frame.hpp index f388e90..9f1a88d 100644 --- a/include/status/axle_torque_frame.hpp +++ b/include/status/axle_torque_frame.hpp @@ -1,6 +1,6 @@ #ifndef AXLE_TORQUE_FRAME_HPP #define AXLE_TORQUE_FRAME_HPP -#pragma once + #include "process_frame.hpp" #include diff --git a/src/adas.cpp b/src/adas.cpp index b5c01c0..d89ad1f 100644 --- a/src/adas.cpp +++ b/src/adas.cpp @@ -1 +1,52 @@ -#include "adas.hpp" \ No newline at end of file +#include "adas.hpp" + +#include + +namespace adas_api +{ + void Adas::read( const int n ) + { + try + { + for( int i=0; i_canSocket ); + + if( auto statusFrame = this->statusFrames.find( rawFrame.can_id ); + statusFrame != this->statusFrames.end() ) + { + statusFrame->second->process( rawFrame ); + } + } + } + catch( const can::error& ) + { + // no more frames to read + } + } + + void Adas::write() + { + for( auto controlFrame : this->controlFrames ) + { + can::write( this->_canSocket, controlFrame->process() ); + } + } + + Adas::Adas( const std::string& device ) : _canSocket( can::connect( device ) ) + { + // set socket to non-blocking + const int existingFlags = fcntl( this->_canSocket, F_GETFL, 0 ); + fcntl( this->_canSocket, F_SETFL, existingFlags | O_NONBLOCK ); + } + + Adas::~Adas() + { + try + { + can::close( this->_canSocket ); + } + catch( const can::error& ) + {} + } +}; \ No newline at end of file diff --git a/src/control/steering_control_frame.cpp b/src/control/steering_control_frame.cpp index ff13f2a..2a54fe4 100644 --- a/src/control/steering_control_frame.cpp +++ b/src/control/steering_control_frame.cpp @@ -39,7 +39,9 @@ can_frame adas_api::SteeringControlFrame::_process() const [[fallthrough]]; case StatusFrame::AsState::DRIVING: const float mx = std::abs( this->_steeringFrame.max() ); - angle = std::min( mx, std::max( -mx, this->_angle ) ); + // limit angle to max steering angle + // use same direction as steering frame + angle = std::min( mx, std::max( -mx, this->_angle * this->_steeringFrame.direction() ) ); break; } } @@ -69,7 +71,9 @@ void adas_api::SteeringControlFrame::_print( std::ostream& os ) const { } -adas_api::SteeringControlFrame::SteeringControlFrame( const SteeringFrame &steeringFrame, const StatusFrame &statusFrame ) : +adas_api::SteeringControlFrame::SteeringControlFrame( const SteeringFrame &steeringFrame, + const StatusFrame &statusFrame ) : ControlFrame( CANDATA_AI2_VCU_STEER_FRAME_ID ), _steeringFrame( steeringFrame ), - _statusFrame( statusFrame ) {} + _statusFrame( statusFrame ) +{} diff --git a/src/demo.cpp b/src/demo.cpp index 3028f13..33d4df3 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -3,7 +3,6 @@ #include #include -#include #include int main( int argc, char *argv[] ) @@ -75,6 +74,9 @@ int main( int argc, char *argv[] ) auto lastWrite = now; auto lastUpdate = now; + /* must not start posting events to the screen until it is in it's + Loop(), we have to start this thread before we start the Loop() + so just pause here and give the screen half a sec to get going */ std::this_thread::sleep_for( std::chrono::milliseconds(500) ); while( eventThreadRunning ) diff --git a/src/demo_ui.cpp b/src/demo_ui.cpp index 9f3b378..03e08a1 100644 --- a/src/demo_ui.cpp +++ b/src/demo_ui.cpp @@ -1,6 +1,10 @@ #include "demo_ui.hpp" -namespace demo_ui -{ - -}; \ No newline at end of file +//namespace demo_ui +//{ + /*float demo_ui::deg2Rad( float degree ) + { + static const float pi = 3.14159265359f; + return ( degree * ( pi / 180 ) ); + }*/ +//} \ No newline at end of file diff --git a/src/simple_adas.cpp b/src/simple_adas.cpp index 55b0fda..4ae753b 100644 --- a/src/simple_adas.cpp +++ b/src/simple_adas.cpp @@ -4,9 +4,91 @@ #include -adas_api::SimpleAdas::SimpleAdas( const std::string& device ) : - adas_api::Adas( device ) -{} +void adas_api::SimpleAdas::set_front_rpm( const float rpm ) +{ + this->frontControl.set_speed( rpm ); +} + +void adas_api::SimpleAdas::set_rear_rpm( const float rpm ) +{ + this->rearControl.set_speed( rpm ); +} + +void adas_api::SimpleAdas::set_rpm( const float rpm ) +{ + this->set_front_rpm( rpm ); + this->set_rear_rpm( rpm ); +} + +void adas_api::SimpleAdas::set_front_brake( const float percentage ) +{ + this->brakeControl.set_front( percentage ); +} + +void adas_api::SimpleAdas::set_rear_brake( const float percentage ) +{ + this->brakeControl.set_rear( percentage ); +} + +void adas_api::SimpleAdas::set_brakes( const float percentage ) +{ + this->brakeControl.set_brakes( percentage ); +} + +void adas_api::SimpleAdas::set_angle( const float degrees ) +{ + this->steeringControl.set_angle( degrees ); +} + +bool adas_api::SimpleAdas::go() const +{ + return this->status().go(); +} + +void adas_api::SimpleAdas::finish() +{ + this->statusControl.set_mission_status( + StatusControlFrame::MissionStatus::FINISHED ); +} + +void adas_api::SimpleAdas::ebrake( bool b ) +{ + this->statusControl.set_estop( b ); +} + +const adas_api::AxleTorqueFrame &adas_api::SimpleAdas::front_axle() const +{ + return Adas::frontAxle; +} +const adas_api::AxleTorqueFrame &adas_api::SimpleAdas::rear_axle() const +{ + return Adas::rearAxle; +} + +const adas_api::BrakeFrame &adas_api::SimpleAdas::brakes() const +{ + return Adas::brakes; +} + +const adas_api::StatusFrame &adas_api::SimpleAdas::status() const +{ + return Adas::status; +} + +const adas_api::SteeringFrame &adas_api::SimpleAdas::steering() const +{ + return Adas::steering; +} + +const adas_api::WheelCountsFrame &adas_api::SimpleAdas::wheel_counts() const +{ + return Adas::wheelCounts; +} + +const adas_api::WheelSpeedsFrame &adas_api::SimpleAdas::wheel_speeds() const +{ + return Adas::wheelSpeeds; +} void adas_api::SimpleAdas::write() { diff --git a/src/status/steering_frame.cpp b/src/status/steering_frame.cpp index 2e22cd5..0191f7f 100644 --- a/src/status/steering_frame.cpp +++ b/src/status/steering_frame.cpp @@ -1,5 +1,40 @@ #include "status/steering_frame.hpp" -/** - * ! TO BE COMPLETED ! - */ \ No newline at end of file +float adas_api::SteeringFrame::angle() const +{ + return this->_direction * candata_vcu2_ai_steer_angle_decode( data.angle ); +} + +float adas_api::SteeringFrame::requested() const +{ + return this->_direction * candata_vcu2_ai_steer_angle_request_decode( data.angle_request ); +} + +float adas_api::SteeringFrame::max() const +{ + return this->_direction * candata_vcu2_ai_steer_angle_max_decode( data.angle_max ); +} + +float adas_api::SteeringFrame::direction() const +{ + return this->_direction; +} + +void adas_api::SteeringFrame::_process( const can_frame& frame ) +{ + if( candata_vcu2_ai_steer_unpack( &data, frame.data, frame.can_dlc ) ) + { + throw std::runtime_error( "Failed to unpack steering frame" ); + } +} + +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 ) +{} \ No newline at end of file