car chassis
Dependencies: Servo mbed-rtos mbed
can.cpp@2:7dfc8dd6aab3, 2015-10-08 (annotated)
- Committer:
- mariob
- Date:
- Thu Oct 08 13:36:17 2015 +0000
- Revision:
- 2:7dfc8dd6aab3
- Parent:
- 1:79b1ee0f97ef
- Child:
- 3:bfc20ec72b15
added body implementation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mariob | 0:ce6055872f4e | 1 | #include "mbed.h" |
mariob | 2:7dfc8dd6aab3 | 2 | #include "car_config.hpp" |
mariob | 2:7dfc8dd6aab3 | 3 | #include "net.hpp" |
mariob | 1:79b1ee0f97ef | 4 | #include "can.hpp" |
mariob | 0:ce6055872f4e | 5 | #include "rtos.h" |
mariob | 0:ce6055872f4e | 6 | |
mariob | 2:7dfc8dd6aab3 | 7 | /*** GLOBAL DATA ***/ |
mariob | 2:7dfc8dd6aab3 | 8 | |
mariob | 2:7dfc8dd6aab3 | 9 | //message buffers |
mariob | 2:7dfc8dd6aab3 | 10 | can_cmd_body_t can_cmd_body; |
mariob | 2:7dfc8dd6aab3 | 11 | can_sts_body_t can_sts_body; |
mariob | 2:7dfc8dd6aab3 | 12 | can_cmd_engine_t can_cmd_engine; |
mariob | 2:7dfc8dd6aab3 | 13 | can_cmd_time_t can_cmd_time; |
mariob | 2:7dfc8dd6aab3 | 14 | can_cmd_driver_t can_cmd_driver; |
mariob | 2:7dfc8dd6aab3 | 15 | can_sts_driver_t can_sts_driver; |
mariob | 2:7dfc8dd6aab3 | 16 | can_cmd_camera_t can_cmd_camera; |
mariob | 2:7dfc8dd6aab3 | 17 | |
mariob | 2:7dfc8dd6aab3 | 18 | /*** LOCAL DATA ***/ |
mariob | 0:ce6055872f4e | 19 | |
mariob | 2:7dfc8dd6aab3 | 20 | //transceiver |
mariob | 2:7dfc8dd6aab3 | 21 | static CAN can(HW_CAN_TX, HW_CAN_RX); |
mariob | 2:7dfc8dd6aab3 | 22 | |
mariob | 2:7dfc8dd6aab3 | 23 | //countdown for missing messages |
mariob | 2:7dfc8dd6aab3 | 24 | uint8 can_missing[CAN_RX_PERIODIC_MSG]; |
mariob | 2:7dfc8dd6aab3 | 25 | |
mariob | 2:7dfc8dd6aab3 | 26 | /*** LOCAL FUNCTION PROTOTYPES ***/ |
mariob | 0:ce6055872f4e | 27 | |
mariob | 2:7dfc8dd6aab3 | 28 | //read if there are messages in the buffer |
mariob | 2:7dfc8dd6aab3 | 29 | void can_rx(); |
mariob | 2:7dfc8dd6aab3 | 30 | |
mariob | 2:7dfc8dd6aab3 | 31 | //send pending messages |
mariob | 2:7dfc8dd6aab3 | 32 | void can_tx(); |
mariob | 2:7dfc8dd6aab3 | 33 | |
mariob | 2:7dfc8dd6aab3 | 34 | /*** GLOBAL FUNCTIONS ***/ |
mariob | 0:ce6055872f4e | 35 | |
mariob | 0:ce6055872f4e | 36 | void init_can() |
mariob | 0:ce6055872f4e | 37 | { |
mariob | 2:7dfc8dd6aab3 | 38 | for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++) |
mariob | 2:7dfc8dd6aab3 | 39 | can_missing[i] = CAN_MISSING_DETECTION; |
mariob | 0:ce6055872f4e | 40 | } |
mariob | 0:ce6055872f4e | 41 | |
mariob | 0:ce6055872f4e | 42 | void thread_can (void const *args) |
mariob | 0:ce6055872f4e | 43 | { |
mariob | 2:7dfc8dd6aab3 | 44 | while(1) { |
mariob | 2:7dfc8dd6aab3 | 45 | can_rx(); |
mariob | 2:7dfc8dd6aab3 | 46 | can_tx(); |
mariob | 2:7dfc8dd6aab3 | 47 | Thread::wait(CAN_THREAD_PERIOD); |
mariob | 2:7dfc8dd6aab3 | 48 | } |
mariob | 0:ce6055872f4e | 49 | } |
mariob | 0:ce6055872f4e | 50 | |
mariob | 0:ce6055872f4e | 51 | bool can_msg_is_missing (uint8 msg_id) |
mariob | 0:ce6055872f4e | 52 | { |
mariob | 2:7dfc8dd6aab3 | 53 | if (msg_id < CAN_RX_PERIODIC_MSG) |
mariob | 2:7dfc8dd6aab3 | 54 | return (can_missing[msg_id] == 0); |
mariob | 2:7dfc8dd6aab3 | 55 | return true; |
mariob | 0:ce6055872f4e | 56 | } |
mariob | 2:7dfc8dd6aab3 | 57 | |
mariob | 2:7dfc8dd6aab3 | 58 | /*** LOCAL FUNCTIONS ***/ |
mariob | 2:7dfc8dd6aab3 | 59 | |
mariob | 2:7dfc8dd6aab3 | 60 | void can_rx () { |
mariob | 2:7dfc8dd6aab3 | 61 | CANMessage message; |
mariob | 2:7dfc8dd6aab3 | 62 | |
mariob | 2:7dfc8dd6aab3 | 63 | do { |
mariob | 2:7dfc8dd6aab3 | 64 | if (can.read(message) == 0) |
mariob | 2:7dfc8dd6aab3 | 65 | break; |
mariob | 2:7dfc8dd6aab3 | 66 | switch(message.id) { |
mariob | 2:7dfc8dd6aab3 | 67 | #ifdef NET_RX_CMD_BODY |
mariob | 2:7dfc8dd6aab3 | 68 | case CAN_CMD_BODY_ID: |
mariob | 2:7dfc8dd6aab3 | 69 | can_cmd_body.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 70 | can_cmd_body.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 71 | can_cmd_body.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 72 | can_cmd_body.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 73 | can_cmd_body.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 74 | can_missing[CAN_MISSING_CMD_BODY_ID] = CAN_MISSING_DETECTION+1; |
mariob | 2:7dfc8dd6aab3 | 75 | break; |
mariob | 2:7dfc8dd6aab3 | 76 | #endif |
mariob | 2:7dfc8dd6aab3 | 77 | #ifdef NET_RX_STS_BODY |
mariob | 2:7dfc8dd6aab3 | 78 | case CAN_STS_BODY_ID: |
mariob | 2:7dfc8dd6aab3 | 79 | can_sts_body.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 80 | can_sts_body.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 81 | can_sts_body.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 82 | can_sts_body.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 83 | can_sts_body.payload.buf[4] = message.data[4]; |
mariob | 2:7dfc8dd6aab3 | 84 | can_sts_body.payload.buf[5] = message.data[5]; |
mariob | 2:7dfc8dd6aab3 | 85 | can_sts_body.payload.buf[6] = message.data[6]; |
mariob | 2:7dfc8dd6aab3 | 86 | can_sts_body.payload.buf[7] = message.data[7]; |
mariob | 2:7dfc8dd6aab3 | 87 | can_sts_body.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 88 | can_missing[CAN_MISSING_STS_BODY_ID] = CAN_MISSING_DETECTION+1; |
mariob | 2:7dfc8dd6aab3 | 89 | break; |
mariob | 2:7dfc8dd6aab3 | 90 | #endif |
mariob | 2:7dfc8dd6aab3 | 91 | #ifdef NET_RX_CMD_ENGINE |
mariob | 2:7dfc8dd6aab3 | 92 | case CAN_CMD_ENGINE_ID: |
mariob | 2:7dfc8dd6aab3 | 93 | can_cmd_engine.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 94 | can_cmd_engine.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 95 | can_cmd_engine.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 96 | can_cmd_engine.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 97 | can_cmd_engine.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 98 | can_missing[CAN_MISSING_CMD_ENGINE_ID] = CAN_MISSING_DETECTION+1; |
mariob | 2:7dfc8dd6aab3 | 99 | break; |
mariob | 2:7dfc8dd6aab3 | 100 | #endif |
mariob | 2:7dfc8dd6aab3 | 101 | #ifdef NET_RX_CMD_TIME |
mariob | 2:7dfc8dd6aab3 | 102 | case CAN_CMD_TIME_ID: |
mariob | 2:7dfc8dd6aab3 | 103 | can_cmd_time.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 104 | can_cmd_time.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 105 | can_cmd_time.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 106 | can_cmd_time.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 107 | can_cmd_time.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 108 | break; |
mariob | 2:7dfc8dd6aab3 | 109 | #endif |
mariob | 2:7dfc8dd6aab3 | 110 | #ifdef NET_RX_CMD_DRIVER |
mariob | 2:7dfc8dd6aab3 | 111 | case CAN_CMD_DRIVER_ID: |
mariob | 2:7dfc8dd6aab3 | 112 | can_cmd_driver.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 113 | can_cmd_driver.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 114 | can_cmd_driver.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 115 | can_cmd_driver.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 116 | can_cmd_driver.payload.buf[4] = message.data[4]; |
mariob | 2:7dfc8dd6aab3 | 117 | can_cmd_driver.payload.buf[5] = message.data[5]; |
mariob | 2:7dfc8dd6aab3 | 118 | can_cmd_driver.payload.buf[6] = message.data[6]; |
mariob | 2:7dfc8dd6aab3 | 119 | can_cmd_driver.payload.buf[7] = message.data[7]; |
mariob | 2:7dfc8dd6aab3 | 120 | can_cmd_driver.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 121 | break; |
mariob | 2:7dfc8dd6aab3 | 122 | #endif |
mariob | 2:7dfc8dd6aab3 | 123 | #ifdef NET_RX_STS_DRIVER |
mariob | 2:7dfc8dd6aab3 | 124 | case CAN_STS_DRIVER_ID: |
mariob | 2:7dfc8dd6aab3 | 125 | can_sts_driver.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 126 | can_sts_driver.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 127 | can_sts_driver.payload.buf[2] = message.data[2]; |
mariob | 2:7dfc8dd6aab3 | 128 | can_sts_driver.payload.buf[3] = message.data[3]; |
mariob | 2:7dfc8dd6aab3 | 129 | can_sts_driver.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 130 | break; |
mariob | 2:7dfc8dd6aab3 | 131 | #endif |
mariob | 2:7dfc8dd6aab3 | 132 | #ifdef NET_RX_CMD_CAMERA |
mariob | 2:7dfc8dd6aab3 | 133 | case CAN_CMD_CAMERA_ID: |
mariob | 2:7dfc8dd6aab3 | 134 | can_cmd_camera.payload.buf[0] = message.data[0]; |
mariob | 2:7dfc8dd6aab3 | 135 | can_cmd_camera.payload.buf[1] = message.data[1]; |
mariob | 2:7dfc8dd6aab3 | 136 | can_cmd_camera.flag = CAN_FLAG_RECEIVED; |
mariob | 2:7dfc8dd6aab3 | 137 | can_missing[CAN_MISSING_CMD_CAMERA_ID] = CAN_MISSING_DETECTION+1; |
mariob | 2:7dfc8dd6aab3 | 138 | break; |
mariob | 2:7dfc8dd6aab3 | 139 | #endif |
mariob | 2:7dfc8dd6aab3 | 140 | default: |
mariob | 2:7dfc8dd6aab3 | 141 | break; |
mariob | 2:7dfc8dd6aab3 | 142 | }; |
mariob | 2:7dfc8dd6aab3 | 143 | } while(1); |
mariob | 2:7dfc8dd6aab3 | 144 | |
mariob | 2:7dfc8dd6aab3 | 145 | for (int i = 0; i < CAN_RX_PERIODIC_MSG; i++) |
mariob | 2:7dfc8dd6aab3 | 146 | if (can_missing[i] > 0) |
mariob | 2:7dfc8dd6aab3 | 147 | can_missing[i]--; |
mariob | 2:7dfc8dd6aab3 | 148 | } |
mariob | 2:7dfc8dd6aab3 | 149 | |
mariob | 2:7dfc8dd6aab3 | 150 | void can_tx () { |
mariob | 2:7dfc8dd6aab3 | 151 | /** periodic messages */ |
mariob | 2:7dfc8dd6aab3 | 152 | #ifdef NET_TX_CMD_BODY |
mariob | 2:7dfc8dd6aab3 | 153 | if (can_cmd_body.flag == 0) { |
mariob | 2:7dfc8dd6aab3 | 154 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 155 | if (!can.write(CANMessage(CAN_CMD_BODY_ID, |
mariob | 2:7dfc8dd6aab3 | 156 | (char*)(&(can_cmd_body.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 157 | CAN_CMD_PAYLOAD_BODY))) |
mariob | 2:7dfc8dd6aab3 | 158 | printf("SEND CMD_BODY NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 159 | can_cmd_body.flag = CAN_CMD_BODY_PERIOD; |
mariob | 2:7dfc8dd6aab3 | 160 | } |
mariob | 2:7dfc8dd6aab3 | 161 | can_cmd_body.flag--; |
mariob | 2:7dfc8dd6aab3 | 162 | #endif |
mariob | 2:7dfc8dd6aab3 | 163 | #ifdef NET_TX_STS_BODY |
mariob | 2:7dfc8dd6aab3 | 164 | if (can_sts_body.flag == 0) { |
mariob | 2:7dfc8dd6aab3 | 165 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 166 | if (!can.write(CANMessage(CAN_STS_BODY_ID, |
mariob | 2:7dfc8dd6aab3 | 167 | (char*)(&(can_sts_body.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 168 | CAN_STS_PAYLOAD_BODY))) |
mariob | 2:7dfc8dd6aab3 | 169 | printf("SEND STS_BODY NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 170 | can_sts_body.flag = CAN_STS_BODY_PERIOD; |
mariob | 2:7dfc8dd6aab3 | 171 | } |
mariob | 2:7dfc8dd6aab3 | 172 | can_sts_body.flag--; |
mariob | 2:7dfc8dd6aab3 | 173 | #endif |
mariob | 2:7dfc8dd6aab3 | 174 | #ifdef NET_TX_CMD_ENGINE |
mariob | 2:7dfc8dd6aab3 | 175 | if (can_cmd_engine.flag == 0) { |
mariob | 2:7dfc8dd6aab3 | 176 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 177 | if (!can.write(CANMessage(CAN_CMD_ENGINE_ID, |
mariob | 2:7dfc8dd6aab3 | 178 | (char*)(&(can_cmd_engine.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 179 | CAN_CMD_PAYLOAD_ENGINE))) |
mariob | 2:7dfc8dd6aab3 | 180 | printf("SEND CMD_ENGINE NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 181 | can_cmd_engine.flag = CAN_CMD_ENGINE_PERIOD; |
mariob | 2:7dfc8dd6aab3 | 182 | } |
mariob | 2:7dfc8dd6aab3 | 183 | can_cmd_engine.flag--; |
mariob | 2:7dfc8dd6aab3 | 184 | #endif |
mariob | 2:7dfc8dd6aab3 | 185 | #ifdef NET_TX_CMD_CAMERA |
mariob | 2:7dfc8dd6aab3 | 186 | if (can_cmd_camera.flag == 0) { |
mariob | 2:7dfc8dd6aab3 | 187 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 188 | if (!can.write(CANMessage(CAN_CMD_CAMERA_ID, |
mariob | 2:7dfc8dd6aab3 | 189 | (char*)(&(can_cmd_camera.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 190 | CAN_CMD_PAYLOAD_CAMERA))) |
mariob | 2:7dfc8dd6aab3 | 191 | printf("SEND CMD_CAMERA NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 192 | can_cmd_camera.flag = CAN_CMD_CAMERA_PERIOD; |
mariob | 2:7dfc8dd6aab3 | 193 | } |
mariob | 2:7dfc8dd6aab3 | 194 | can_cmd_camera.flag--; |
mariob | 2:7dfc8dd6aab3 | 195 | #endif |
mariob | 2:7dfc8dd6aab3 | 196 | |
mariob | 2:7dfc8dd6aab3 | 197 | /** event messages */ |
mariob | 2:7dfc8dd6aab3 | 198 | #ifdef NET_TX_CMD_DRIVER |
mariob | 2:7dfc8dd6aab3 | 199 | if (can_cmd_driver.flag == CAN_FLAG_SEND) { |
mariob | 2:7dfc8dd6aab3 | 200 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 201 | if (!can.write(CANMessage(CAN_CMD_DRIVER_ID, |
mariob | 2:7dfc8dd6aab3 | 202 | (char*)(&(can_cmd_driver.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 203 | CAN_CMD_PAYLOAD_DRIVER))) |
mariob | 2:7dfc8dd6aab3 | 204 | printf("SEND CMD_DRIVER NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 205 | can_cmd_driver.flag = CAN_FLAG_EMPTY; |
mariob | 2:7dfc8dd6aab3 | 206 | } |
mariob | 2:7dfc8dd6aab3 | 207 | #endif |
mariob | 2:7dfc8dd6aab3 | 208 | #ifdef NET_TX_STS_DRIVER |
mariob | 2:7dfc8dd6aab3 | 209 | if (can_sts_driver.flag == CAN_FLAG_SEND) { |
mariob | 2:7dfc8dd6aab3 | 210 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 211 | if (!can.write(CANMessage(CAN_STS_DRIVER_ID, |
mariob | 2:7dfc8dd6aab3 | 212 | (char*)(&(can_sts_driver.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 213 | CAN_STS_PAYLOAD_DRIVER))) |
mariob | 2:7dfc8dd6aab3 | 214 | printf("SEND STS_DRIVER NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 215 | can_sts_driver.flag = CAN_FLAG_EMPTY; |
mariob | 2:7dfc8dd6aab3 | 216 | } |
mariob | 2:7dfc8dd6aab3 | 217 | #endif |
mariob | 2:7dfc8dd6aab3 | 218 | #ifdef NET_TX_CMD_TIME |
mariob | 2:7dfc8dd6aab3 | 219 | if (can_cmd_time.flag == CAN_FLAG_SEND) { |
mariob | 2:7dfc8dd6aab3 | 220 | wait(2); //wait 2 milliseconds between 2 consecutive transmissions |
mariob | 2:7dfc8dd6aab3 | 221 | if (!can.write(CANMessage(CAN_CMD_TIME_ID, |
mariob | 2:7dfc8dd6aab3 | 222 | (char*)(&(can_cmd_time.payload.buf)), |
mariob | 2:7dfc8dd6aab3 | 223 | CAN_CMD_PAYLOAD_TIME))) |
mariob | 2:7dfc8dd6aab3 | 224 | printf("SEND CMD_TIME NOT OK\r\n"); |
mariob | 2:7dfc8dd6aab3 | 225 | can_cmd_time.flag = CAN_FLAG_EMPTY; |
mariob | 2:7dfc8dd6aab3 | 226 | } |
mariob | 2:7dfc8dd6aab3 | 227 | #endif |
mariob | 2:7dfc8dd6aab3 | 228 | } |