axle_control_frame.h

#ifndef AXLE_CONTROL_FRAME_H
#define AXLE_CONTROL_FRAME_H

#include "status_frames.h"
#include "control_frames.h"

#include <candata.h>

#include <array>
#include <iostream>
#include <stdexcept>
#include <variant>

class AxleControlFrame : public ControlFrame
{
public:
    bool is_front() const;

    AxleControlFrame( const AxleTorqueFrame &torqueFrame, 
                        const WheelSpeedsFrame &speedsFrame, 
                        const bool front=true );
    virtual ~AxleControlFrame() = default;

    void set_speed( const float speed );

    /**
     * @brief Limit torque based on rpm.
     * 
     * Limit the torque based on the wheel rpm to help maintain constant electrical 
     * power and minimise the risk of an over-current trip. These limits are copied
     * from the official FS-AI_API code.
     * 
     * @param torque 
     * @param rpm 
     * @return float Limited torque.
     */
    static float torque_rpm_limit( const float torque, const float rpm );

private:
    virtual can_frame _process() const override;
    virtual void _print( std::ostream& os ) const override;

    const AxleTorqueFrame& _torqueFrame;
    const WheelSpeedsFrame& _speedsFrame;

    const bool _front;
    float _speed = 0.f;
};

void AxleControlFrame::set_speed( const float speed )
{
    _speed = speed;
}

float AxleControlFrame::torque_rpm_limit( const float torque, const float rpm )
{
    if( rpm > 700 )
        return 50.f;
    else if( rpm > 600 )
        return 85.f;
    else if( rpm > 500 )
        return 100.f;
    else if( rpm > 400 )
        return 120.f;
    else if( rpm > 300 )
        return 150.f;

    return torque;
}

can_frame AxleControlFrame::_process() const
{
    float speed = 0.f;
    float torque = 0.f;

    /* check that we have up to date information otherwise set safe values */
    if( this->_torqueFrame.is_recent() )
    {
        speed = this->_speed;
        torque = this->_torqueFrame.get_max();
    }

    if( this->_speedsFrame.is_recent() )
        torque = torque_rpm_limit( torque, this->_speedsFrame.fastest() );

    can_frame frame;
    frame.can_id = this->frameId;
    frame.can_dlc = 8;

    if( is_front() )
    {
        candata_ai2_vcu_drive_f_t data
            {
                .front_axle_trq_request = candata_ai2_vcu_drive_f_front_axle_trq_request_encode( torque ),
                .front_motor_speed_max = candata_ai2_vcu_drive_f_front_motor_speed_max_encode( speed )
            };
        //candata_ai2_vcu_drive_f_front_motor_speed_max_is_in_range( data.front_motor_speed_max );
        candata_ai2_vcu_drive_f_pack( frame.data, &data, frame.can_dlc );
    }
    else
    {
        candata_ai2_vcu_drive_r_t data
            {
                .rear_axle_trq_request = candata_ai2_vcu_drive_r_rear_axle_trq_request_encode( torque ),
                .rear_motor_speed_max = candata_ai2_vcu_drive_r_rear_motor_speed_max_encode( speed )
            };
        candata_ai2_vcu_drive_r_pack( frame.data, &data, frame.can_dlc );
    }

    return frame;
}

bool AxleControlFrame::is_front() const
{
    return _front;
}

void AxleControlFrame::_print( std::ostream& os ) const
{
}

AxleControlFrame::AxleControlFrame( const AxleTorqueFrame &torqueFrame,
                                    const WheelSpeedsFrame &speedsFrame,
                                    const bool front ) : 
    ControlFrame( front ? CANDATA_AI2_VCU_DRIVE_F_FRAME_ID : CANDATA_AI2_VCU_DRIVE_R_FRAME_ID ),
    _torqueFrame( torqueFrame ),
    _speedsFrame( speedsFrame ),
    _front( front )
{
}

#endif // AXLE_CONTROL_FRAME_H