Mario Bambagini
car chassis
Dependencies: Servo mbed-rtos mbed
Diff: can.cpp
- Revision:
- 2:7dfc8dd6aab3
- Parent:
- 1:79b1ee0f97ef
- Child:
- 3:bfc20ec72b15
--- a/can.cpp Mon Aug 31 22:25:57 2015 +0000
+++ b/can.cpp Thu Oct 08 13:36:17 2015 +0000
@@ -1,130 +1,228 @@
#include "mbed.h"
+#include "car_config.hpp"
+#include "net.hpp"
#include "can.hpp"
#include "rtos.h"
-/*
- * COMMON
- */
+/*** GLOBAL DATA ***/
+
+//message buffers
+can_cmd_body_t can_cmd_body;
+can_sts_body_t can_sts_body;
+can_cmd_engine_t can_cmd_engine;
+can_cmd_time_t can_cmd_time;
+can_cmd_driver_t can_cmd_driver;
+can_sts_driver_t can_sts_driver;
+can_cmd_camera_t can_cmd_camera;
+
+/*** LOCAL DATA ***/
-CAN can(p9, p10);
+//transceiver
+static CAN can(HW_CAN_TX, HW_CAN_RX);
+
+//countdown for missing messages
+uint8 can_missing[CAN_RX_PERIODIC_MSG];
+
+/*** LOCAL FUNCTION PROTOTYPES ***/
-uint8 can_missing[CAN_RX_PERIODIC_MSG];
+//read if there are messages in the buffer
+void can_rx();
+
+//send pending messages
+void can_tx();
+
+/*** GLOBAL FUNCTIONS ***/
void init_can()
{
- printf("RECOVERY: %d\r\n", CAN_MISSING_DETECTION);
- for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
- can_missing[i] = CAN_MISSING_DETECTION;
+ for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
+ can_missing[i] = CAN_MISSING_DETECTION;
}
-/*
- * RECEIVER
- */
-
-can_cmd_engine_t can_cmd_engine;
-can_cmd_body_t can_cmd_body;
-can_cmd_time_t can_cmd_time;
-can_cmd_driver_t can_cmd_driver;
-
-void can_rx()
-{
- bool ret = false;
- CANMessage message;
-
- do{
- ret = can.read(message);
-
- if (ret == 0)
- break;
-
- switch(message.id) {
- case CAN_CMD_ENGINE_ID:
- can_cmd_engine.payload.buf[0] = message.data[0];
- can_cmd_engine.payload.buf[1] = message.data[1];
- can_cmd_engine.payload.buf[2] = message.data[2];
- can_cmd_engine.payload.buf[3] = message.data[3];
- can_cmd_engine.flag = CAN_FLAG_RECEIVED;
- can_missing[CAN_MISSING_ENGINE_ID] = CAN_MISSING_DETECTION+1;
- break;
- case CAN_CMD_BODY_ID:
- can_cmd_body.payload.buf[0] = message.data[0];
- can_cmd_body.payload.buf[1] = message.data[1];
- can_cmd_body.payload.buf[2] = message.data[2];
- can_cmd_body.payload.buf[3] = message.data[3];
- can_cmd_body.flag = CAN_FLAG_RECEIVED;
- can_missing[CAN_MISSING_BODY_ID] = CAN_MISSING_DETECTION+1;
- break;
- case CAN_CMD_TIME_ID:
- can_cmd_time.payload.buf[0] = message.data[0];
- can_cmd_time.payload.buf[1] = message.data[1];
- can_cmd_time.payload.buf[2] = message.data[2];
- can_cmd_time.payload.buf[3] = message.data[3];
- can_cmd_time.flag = CAN_FLAG_RECEIVED;
- can_missing[CAN_MISSING_TIME_ID] = CAN_MISSING_DETECTION+1;
- break;
- case CAN_CMD_DRIVER_ID:
- can_cmd_driver.payload.buf[0] = message.data[0];
- can_cmd_driver.payload.buf[1] = message.data[1];
- can_cmd_driver.payload.buf[2] = message.data[2];
- can_cmd_driver.payload.buf[3] = message.data[3];
- can_cmd_driver.payload.buf[4] = message.data[4];
- can_cmd_driver.payload.buf[5] = message.data[5];
- can_cmd_driver.payload.buf[6] = message.data[6];
- can_cmd_driver.payload.buf[7] = message.data[7];
- can_cmd_driver.flag = CAN_FLAG_RECEIVED;
- break;
- };
- } while(1);
-
- for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
- if (can_missing[i]>0)
- can_missing[i]--;
-}
-
-/*
- * SENDER
- */
-can_sts_body_t can_sts_body;
-can_sts_driver_t can_sts_driver;
-
-void can_tx ()
-{
- /** periodic messages */
- if (can_sts_body.flag == 0) {
- if (!can.write(CANMessage(CAN_STS_BODY_ID, (char*)(&(can_sts_body.payload.buf)), CAN_STS_PAYLOAD_BODY)))
-// printf("SEND STS_BODY OK\r\n");
-// else
- printf("SEND STS_BODY NOT OK\r\n");
- can_sts_body.flag = CAN_STS_BODY_PERIOD;
- }
- can_sts_body.flag--;
-
- /** event messages */
- if (can_sts_driver.flag == CAN_FLAG_SEND) {
- if (!can.write(CANMessage(CAN_STS_DRIVER_ID, (char*)(&(can_sts_driver.payload.buf)), CAN_STS_PAYLOAD_DRIVER)))
-// printf("SEND STS_DRIVER OK\r\n");
-// else
- printf("SEND STS_DRIVER NOT OK\r\n");
- can_sts_driver.flag = CAN_FLAG_EMPTY;
- }
-}
-
-/*
- * THREAD
- */
-
void thread_can (void const *args)
{
- while(1) {
- can_rx();
- can_tx();
- Thread::wait(CAN_THREAD_PERIOD);
- }
+ while(1) {
+ can_rx();
+ can_tx();
+ Thread::wait(CAN_THREAD_PERIOD);
+ }
}
bool can_msg_is_missing (uint8 msg_id)
{
- if (msg_id < CAN_RX_PERIODIC_MSG)
- return (can_missing[msg_id]==0);
- return true;
+ if (msg_id < CAN_RX_PERIODIC_MSG)
+ return (can_missing[msg_id] == 0);
+ return true;
}
+
+/*** LOCAL FUNCTIONS ***/
+
+void can_rx () {
+ CANMessage message;
+
+ do {
+ if (can.read(message) == 0)
+ break;
+ switch(message.id) {
+#ifdef NET_RX_CMD_BODY
+ case CAN_CMD_BODY_ID:
+ can_cmd_body.payload.buf[0] = message.data[0];
+ can_cmd_body.payload.buf[1] = message.data[1];
+ can_cmd_body.payload.buf[2] = message.data[2];
+ can_cmd_body.payload.buf[3] = message.data[3];
+ can_cmd_body.flag = CAN_FLAG_RECEIVED;
+ can_missing[CAN_MISSING_CMD_BODY_ID] = CAN_MISSING_DETECTION+1;
+ break;
+#endif
+#ifdef NET_RX_STS_BODY
+ case CAN_STS_BODY_ID:
+ can_sts_body.payload.buf[0] = message.data[0];
+ can_sts_body.payload.buf[1] = message.data[1];
+ can_sts_body.payload.buf[2] = message.data[2];
+ can_sts_body.payload.buf[3] = message.data[3];
+ can_sts_body.payload.buf[4] = message.data[4];
+ can_sts_body.payload.buf[5] = message.data[5];
+ can_sts_body.payload.buf[6] = message.data[6];
+ can_sts_body.payload.buf[7] = message.data[7];
+ can_sts_body.flag = CAN_FLAG_RECEIVED;
+ can_missing[CAN_MISSING_STS_BODY_ID] = CAN_MISSING_DETECTION+1;
+ break;
+#endif
+#ifdef NET_RX_CMD_ENGINE
+ case CAN_CMD_ENGINE_ID:
+ can_cmd_engine.payload.buf[0] = message.data[0];
+ can_cmd_engine.payload.buf[1] = message.data[1];
+ can_cmd_engine.payload.buf[2] = message.data[2];
+ can_cmd_engine.payload.buf[3] = message.data[3];
+ can_cmd_engine.flag = CAN_FLAG_RECEIVED;
+ can_missing[CAN_MISSING_CMD_ENGINE_ID] = CAN_MISSING_DETECTION+1;
+ break;
+#endif
+#ifdef NET_RX_CMD_TIME
+ case CAN_CMD_TIME_ID:
+ can_cmd_time.payload.buf[0] = message.data[0];
+ can_cmd_time.payload.buf[1] = message.data[1];
+ can_cmd_time.payload.buf[2] = message.data[2];
+ can_cmd_time.payload.buf[3] = message.data[3];
+ can_cmd_time.flag = CAN_FLAG_RECEIVED;
+ break;
+#endif
+#ifdef NET_RX_CMD_DRIVER
+ case CAN_CMD_DRIVER_ID:
+ can_cmd_driver.payload.buf[0] = message.data[0];
+ can_cmd_driver.payload.buf[1] = message.data[1];
+ can_cmd_driver.payload.buf[2] = message.data[2];
+ can_cmd_driver.payload.buf[3] = message.data[3];
+ can_cmd_driver.payload.buf[4] = message.data[4];
+ can_cmd_driver.payload.buf[5] = message.data[5];
+ can_cmd_driver.payload.buf[6] = message.data[6];
+ can_cmd_driver.payload.buf[7] = message.data[7];
+ can_cmd_driver.flag = CAN_FLAG_RECEIVED;
+ break;
+#endif
+#ifdef NET_RX_STS_DRIVER
+ case CAN_STS_DRIVER_ID:
+ can_sts_driver.payload.buf[0] = message.data[0];
+ can_sts_driver.payload.buf[1] = message.data[1];
+ can_sts_driver.payload.buf[2] = message.data[2];
+ can_sts_driver.payload.buf[3] = message.data[3];
+ can_sts_driver.flag = CAN_FLAG_RECEIVED;
+ break;
+#endif
+#ifdef NET_RX_CMD_CAMERA
+ case CAN_CMD_CAMERA_ID:
+ can_cmd_camera.payload.buf[0] = message.data[0];
+ can_cmd_camera.payload.buf[1] = message.data[1];
+ can_cmd_camera.flag = CAN_FLAG_RECEIVED;
+ can_missing[CAN_MISSING_CMD_CAMERA_ID] = CAN_MISSING_DETECTION+1;
+ break;
+#endif
+ default:
+ break;
+ };
+ } while(1);
+
+ for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++)
+ if (can_missing[i] > 0)
+ can_missing[i]--;
+}
+
+void can_tx () {
+ /** periodic messages */
+#ifdef NET_TX_CMD_BODY
+ if (can_cmd_body.flag == 0) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_CMD_BODY_ID,
+ (char*)(&(can_cmd_body.payload.buf)),
+ CAN_CMD_PAYLOAD_BODY)))
+ printf("SEND CMD_BODY NOT OK\r\n");
+ can_cmd_body.flag = CAN_CMD_BODY_PERIOD;
+ }
+ can_cmd_body.flag--;
+#endif
+#ifdef NET_TX_STS_BODY
+ if (can_sts_body.flag == 0) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_STS_BODY_ID,
+ (char*)(&(can_sts_body.payload.buf)),
+ CAN_STS_PAYLOAD_BODY)))
+ printf("SEND STS_BODY NOT OK\r\n");
+ can_sts_body.flag = CAN_STS_BODY_PERIOD;
+ }
+ can_sts_body.flag--;
+#endif
+#ifdef NET_TX_CMD_ENGINE
+ if (can_cmd_engine.flag == 0) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_CMD_ENGINE_ID,
+ (char*)(&(can_cmd_engine.payload.buf)),
+ CAN_CMD_PAYLOAD_ENGINE)))
+ printf("SEND CMD_ENGINE NOT OK\r\n");
+ can_cmd_engine.flag = CAN_CMD_ENGINE_PERIOD;
+ }
+ can_cmd_engine.flag--;
+#endif
+#ifdef NET_TX_CMD_CAMERA
+ if (can_cmd_camera.flag == 0) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_CMD_CAMERA_ID,
+ (char*)(&(can_cmd_camera.payload.buf)),
+ CAN_CMD_PAYLOAD_CAMERA)))
+ printf("SEND CMD_CAMERA NOT OK\r\n");
+ can_cmd_camera.flag = CAN_CMD_CAMERA_PERIOD;
+ }
+ can_cmd_camera.flag--;
+#endif
+
+ /** event messages */
+#ifdef NET_TX_CMD_DRIVER
+ if (can_cmd_driver.flag == CAN_FLAG_SEND) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_CMD_DRIVER_ID,
+ (char*)(&(can_cmd_driver.payload.buf)),
+ CAN_CMD_PAYLOAD_DRIVER)))
+ printf("SEND CMD_DRIVER NOT OK\r\n");
+ can_cmd_driver.flag = CAN_FLAG_EMPTY;
+ }
+#endif
+#ifdef NET_TX_STS_DRIVER
+ if (can_sts_driver.flag == CAN_FLAG_SEND) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_STS_DRIVER_ID,
+ (char*)(&(can_sts_driver.payload.buf)),
+ CAN_STS_PAYLOAD_DRIVER)))
+ printf("SEND STS_DRIVER NOT OK\r\n");
+ can_sts_driver.flag = CAN_FLAG_EMPTY;
+ }
+#endif
+#ifdef NET_TX_CMD_TIME
+ if (can_cmd_time.flag == CAN_FLAG_SEND) {
+ wait(2); //wait 2 milliseconds between 2 consecutive transmissions
+ if (!can.write(CANMessage(CAN_CMD_TIME_ID,
+ (char*)(&(can_cmd_time.payload.buf)),
+ CAN_CMD_PAYLOAD_TIME)))
+ printf("SEND CMD_TIME NOT OK\r\n");
+ can_cmd_time.flag = CAN_FLAG_EMPTY;
+ }
+#endif
+}