canbus: steering wheel angle

modules/canbus/proto/chassis.proto

@@ -104,4 +104,6 @@ message Chassis {
   optional float bat_volt = 35 [default = nan];
   optional float motor_volt = 36 [default = nan];
   optional float bat_percentage = 37 [default = nan];
+  optional float front_wheel_angle = 38 [default = nan];
+  optional float rear_wheel_angle = 39 [default = nan];
 }

modules/canbus/vehicle/diamond/diamond_controller.cc

@@ -133,6 +133,8 @@ Chassis DiamondController::chassis() {
   ChassisDetail chassis_detail;
   message_manager_->GetSensorData(&chassis_detail);
 
+  auto diamond = chassis_detail.mutable_diamond();
+
   // 21, 22, previously 1, 2
   if (driving_mode() == Chassis::EMERGENCY_MODE) {
     set_chassis_error_code(Chassis::NO_ERROR);
@@ -144,17 +146,17 @@ Chassis DiamondController::chassis() {
   // 3
 
   // 4 Motor torque nm
-  if (chassis_detail.diamond().id_0x0c08a7f0().has_fmottq()) {
+  if (diamond->id_0x0c08a7f0().has_fmottq()) {
     chassis_.set_motor_torque_nm(
-        static_cast<float>(chassis_detail.diamond().id_0x0c08a7f0().fmottq()));
+        static_cast<float>(diamond->id_0x0c08a7f0().fmottq()));
   } else {
     chassis_.set_motor_torque_nm(0);
   }
 
   // 5
   // compute speed respect to motor torque
-  if (chassis_detail.diamond().id_0x0c08a7f0().has_fmotspd()) {
-    auto speed = 0.001957 * chassis_detail.diamond().id_0x0c08a7f0().fmotspd();
+  if (diamond->id_0x0c08a7f0().has_fmotspd()) {
+    auto speed = 0.001957 * diamond->id_0x0c08a7f0().fmotspd();
     chassis_.set_speed_mps(static_cast<float>(speed));
   } else {
     chassis_.set_speed_mps(0);
@@ -167,34 +169,48 @@ Chassis DiamondController::chassis() {
   chassis_.mutable_vehicle_id()->set_vin(params_.vin());
 
   // 8 engine rpm respect to motor speed by rpm
-  if (chassis_detail.diamond().id_0x0c08a7f0().has_fmotspd()) {
+  if (diamond->id_0x0c08a7f0().has_fmotspd()) {
     chassis_.set_motor_rpm(
-        static_cast<float>(chassis_detail.diamond().id_0x0c08a7f0().fmotspd()));
+        static_cast<float>(diamond->id_0x0c08a7f0().fmotspd()));
   } else {
     chassis_.set_motor_rpm(0);
   }
 
-  if (chassis_detail.diamond().id_0x1818d0f3().has_fbatvolt()) {
+  if (diamond->id_0x1818d0f3().has_fbatvolt()) {
     chassis_.set_bat_volt(static_cast<float>(
-        chassis_detail.diamond().id_0x1818d0f3().fbatvolt()));
+        diamond->id_0x1818d0f3().fbatvolt()));
   } else {
     chassis_.set_bat_volt(0);
   }
 
-  if (chassis_detail.diamond().id_0x0c09a7f0().has_fmotvolt()) {
+  if (diamond->id_0x0c09a7f0().has_fmotvolt()) {
     chassis_.set_motor_volt(static_cast<float>(
-        chassis_detail.diamond().id_0x0c09a7f0().fmotvolt()));
+        diamond->id_0x0c09a7f0().fmotvolt()));
   } else {
     chassis_.set_motor_volt(0);
   }
 
-  if (chassis_detail.diamond().id_0x1818d0f3().has_fbatsoc()) {
+  if (diamond->id_0x1818d0f3().has_fbatsoc()) {
     chassis_.set_bat_percentage(
-        static_cast<float>(chassis_detail.diamond().id_0x1818d0f3().fbatsoc()));
+        static_cast<float>(diamond->id_0x1818d0f3().fbatsoc()));
   } else {
     chassis_.set_bat_percentage(0);
   }
 
+  if (diamond->id_0x01().has_angle_sensor_front()) {
+    chassis_.set_front_wheel_angle(
+        static_cast<float>(diamond->id_0x01().angle_sensor_front()));
+  } else {
+    chassis_.set_front_wheel_angle(0);
+  }
+
+  if (diamond->id_0x02().has_angle_sensor_rear()) {
+    chassis_.set_rear_wheel_angle(
+        static_cast<float>(diamond->id_0x02().angle_sensor_rear()));
+  } else {
+    chassis_.set_rear_wheel_angle(0);
+  }
+
   return chassis_;
 }
 

modules/drivers/canbus/can_client/socket/socket_can_client_raw.cc

@@ -79,12 +79,8 @@ ErrorCode SocketCanClientRaw::Start() {
 
   // 1. for non virtual busses, set receive message_id filter, ie white list
   if (interface_ != CANCardParameter::VIRTUAL) {
-    struct can_filter filter[15];
-    /*for (int i = 0; i < 2048; ++i) {
-      filter[i].can_id = 0x000 + i;
-      filter[i].can_mask = CAN_EFF_MASK;
-    }*/
-
+    struct can_filter filter[17];
+    // TODO(ALL):REMOVE TO CONFIG Extend frame filter
     int hex_value[] = {0x0C09A79B, 0x0C0AA79C, 0x0C09A7F0, 0x0C08A7F0,
                        0x0C0BA7F0, 0x1818D0F3, 0x1819D0F3, 0x181AD0F3,
                        0x181BD0F3, 0x181CD0F3, 0x181DD0F3, 0x181ED0F3,
@@ -94,6 +90,12 @@ ErrorCode SocketCanClientRaw::Start() {
       filter[i].can_mask = CAN_EFF_MASK;
     }
 
+    // Stardard frame filter
+    for (int i = 15; i < 17; ++i) {
+      filter[i].can_id = 0x0 + (i - 14);
+      filter[i].can_mask = CAN_SFF_MASK;
+    }
+
     ret = setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter,
                      sizeof(filter));
     if (ret < 0) {
@@ -173,7 +175,12 @@ ErrorCode SocketCanClientRaw::Send(const std::vector<CanFrame> &frames,
              << CANBUS_MESSAGE_LENGTH << ").";
       return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
     }
-    send_frames_[i].can_id = CAN_EFF_FLAG | frames[i].id;
+    if(frames[i].id > 2048) {
+      send_frames_[i].can_id = CAN_EFF_FLAG | frames[i].id;
+    } else {
+       // SFF frame
+      send_frames_[i].can_id = frames[i].id;
+    }
     send_frames_[i].can_dlc = frames[i].len;
     std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len);
 
@@ -220,7 +227,11 @@ ErrorCode SocketCanClientRaw::Receive(std::vector<CanFrame> *const frames,
              << CANBUS_MESSAGE_LENGTH << ").";
       return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
     }
-    cf.id = recv_frames_[i].can_id & CAN_EFF_MASK;
+    if(recv_frames_[i].can_id > 2048){
+      cf.id = recv_frames_[i].can_id & CAN_EFF_MASK;
+    } else {
+      cf.id = recv_frames_[i].can_id;
+    }
     cf.len = recv_frames_[i].can_dlc;
     std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc);
     frames->push_back(cf);