Simple Rearview Camera Signal

// XirallicBolts
// Basic example of sending Reverse signal when car is stopped in Sport gear.

#include <Canbus.h>
#include <defaults.h>
#include <global.h>
#include <mcp2515.h>
#include <mcp2515_defs.h>

void setup(){
  Serial.begin(500000);
  //Initialise MCP2515 CAN controller at the specified speed.  CANSPEED_500 for HS-CAN or I-CAN,  CANSPEED_125 for MS-CAN
  if(Canbus.init(CANSPEED_500))
    Serial.println("CAN Init ok");
  else
   Serial.println("Can't Init CAN");
  delay(1000);
}


void loop(){
tCAN message;

if (mcp2515_check_message())
{
    if (mcp2515_get_message(&message))
    {
        if(message.id == 0x109 and message.data[2] == 0x41  and message.data[4] < 0x02)
        {
            Serial.println("Car is stopped in S,  sending reverse signal");
            message.data[2] = 0x11;    // Change the gear byte to 11 (Reverse)
            mcp2515_bit_modify(CANCTRL, (1<<REQOP2)|(1<<REQOP1)|(1<<REQOP0), 0);
            mcp2515_send_message(&message);
        }
     }
}
}