toyota.h

/*
 Author: Matt Bunting
 Copyright (c) 2020 Arizona Board of Regents
 All rights reserved.

 Permission is hereby granted, without written agreement and without
 license or royalty fees, to use, copy, modify, and distribute this
 software and its documentation for any purpose, provided that the
 above copyright notice and the following two paragraphs appear in
 all copies of this software.

 IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
 FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
 ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
 IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
 SUCH DAMAGE.

 THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
 IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
 TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

 */

#ifndef TOYOTA_H
#define TOYOTA_H


#include "mogi/thread.h"

#include "panda.h"

#define TIME_HEARTBEAT_FAIL_STEERING (1.0)      // In seconds, time until a heartbeat fails from not receiving a new steering command
#define TIME_HEARTBEAT_FAIL_ACCELERATION (1.0)  // In seconds, time until a heartbeat fails from not receiving a new acceleration command

#define TOYOTA_COMMAND_THREAD_RATE (600.0)  // Defines the rate of the thread, not for any particular command to be sent.
#define TOYOTA_RATE_HEARTBEAT (1.0)         // This is for the panda in general, not Toyota specific
#define TOYOTA_RATE_LKA (1.0)               // Rate of the LKAS_HUD command
#define TOYOTA_RATE_TRACK_B (40.0)          // Rate of the TRACK_B_1 command
#define TOYOTA_RATE_STEER (100.0)           // Rate of the STEERING_LKA command
#define TOYOTA_RATE_ACC (30.0)              // Rate of the ACC_CONTROL command

#define TOYOTA_DECIMATOR_MAX_HEARTBEAT (TOYOTA_COMMAND_THREAD_RATE/TOYOTA_RATE_HEARTBEAT)
#define TOYOTA_DECIMATOR_MAX_LKA (TOYOTA_COMMAND_THREAD_RATE/TOYOTA_RATE_LKA)
#define TOYOTA_DECIMATOR_MAX_TRACK_B (TOYOTA_COMMAND_THREAD_RATE/TOYOTA_RATE_TRACK_B)
#define TOYOTA_DECIMATOR_MAX_STEER (TOYOTA_COMMAND_THREAD_RATE/TOYOTA_RATE_STEER)
#define TOYOTA_DECIMATOR_MAX_ACC (TOYOTA_COMMAND_THREAD_RATE/TOYOTA_RATE_ACC)

namespace Panda {

CanFrame buildLkasHud(bool lkaAlert, unsigned char leftLane, unsigned char rightLane, bool barrier, bool twoBeeps, bool repeatedBeeps);

CanFrame buildSteeringLKA( unsigned char count, int16_t steerTorque, bool steerRequest, unsigned char lkaState );

CanFrame buildACC_CONTROL(double acc, bool permitBraking, bool releaseStandstill, bool miniCar, bool cancelRequest);

CanFrame buildTRACK_B_1(unsigned char count);

CanFrame buildPCM_CRUISE_2(unsigned char SET_SPEED);

CanFrame buildDSU_CRUISE(unsigned char SET_SPEED);

uint8_t toyotaChecksum(Panda::CanFrame& frame);

void printFrame( Panda::CanFrame frame );

class ToyotaHandler : public Mogi::Thread {
private:
    
    // Overloaded from Mogi::Thread.
    // This will enable the required power save mode for vehicle control.
    void entryAction();
    void exitAction();
    
    // Overloaded from Mogi::Thread.
    // This handles the constant updates.
    void doAction();
    
    // All of the following are called from doAction()
    void sendHeartBeat();
    void sendLka();
    void sendTrackB();
    void sendSteer();
    void sendAcc();
    
    // This will max the LKA decimator to trigger an instant send.
    void triggerInstantLkaSend();

    // Helper functions to check whether the hearbeats currently pass.
    // These will pass if a command was sent within the corresponding defined times:
    // TIME_HEARTBEAT_FAIL_STEERING
    // TIME_HEARTBEAT_FAIL_ACCELERATION
    bool heartbeatSteeringPass();
    bool heartbeatAccelerationPass();
    
    Panda::Handler* pandaHandler;
    PandaHealth health;
    
    int decimatorHeartbeat;
    int decimatorLka;
    int decimatorTrackB;
    int decimatorSteer;
    int decimatorAcc;
    
    // For building proper CanFrames for steering and track B:
    unsigned char counterSteer;
    unsigned char counterTrackB;
    
    // HUD:
    bool hudLdaAlert;
    bool hudBarrier;
    unsigned char hudLeftLane;
    unsigned char hudRightLane;
    bool hudTwoBeeps;
    bool hudRepeatedBeeps;
    
    // Steering Control:
    double steerTorqueControl;
    bool steerRequest;
    unsigned char steerLkaState;
    
    // Acceleration Control:
    double accelerationControl;
    bool permitBraking;
    bool releaseStandstill;
    bool miniCar;
    bool cancelRequest;
    
    // Safety management, only send control commands if they are continuously updated
    int heartBeatSteer;
    int heartBeatAcceleration;
    
public:
    
    ToyotaHandler(Panda::Handler* handler);
    
    void setHudLdaAlert( bool enable );
    
    void setHudBarrier( bool enable );
    
    void setHudLanes( unsigned char laneLeft, unsigned char laneRight );
    
    void setHudTwoBeeps( bool enable );
    
    void setHudRepeatedBeeps( bool enable );
    
    void setHudMiniCar( bool enable );
    
    void setHudCruiseCancelRequest( bool enable );
    
    void setSteerTorque( int steerTorque );
    
    void setAcceleration( double acceleration );
    
    bool getIgnitionOn();
    
    bool getControlsAllowed();
    
    const PandaHealth& getPandaHealth() const;
};

}

#endif