Process closed loop data received over CAN
| 211 | |
| 212 | // Process closed loop data received over CAN |
| 213 | void ClosedLoop::ProcessReceivedData(CanAddress src, const CanMessageClosedLoopData& msg, size_t msgLen) noexcept |
| 214 | { |
| 215 | FileStore * const f = closedLoopFile.load(); |
| 216 | if (f != nullptr) |
| 217 | { |
| 218 | whenDataLastReceived = millis(); |
| 219 | if (msg.firstSampleNumber != expectedRemoteSampleNumber) |
| 220 | { |
| 221 | f->Write("Data lost\n"); |
| 222 | CloseDataCollectionFile(); |
| 223 | } |
| 224 | else if (dataBytesPerSample * msg.numSamples > CanMessageClosedLoopData::GetNumDataBytes(msgLen)) |
| 225 | { |
| 226 | f->Write("Bad data received\n"); |
| 227 | CloseDataCollectionFile(); |
| 228 | } |
| 229 | else |
| 230 | { |
| 231 | const uint8_t *dataPtr = msg.data; |
| 232 | for (unsigned int sampleIndex = 0; sampleIndex < msg.numSamples; ++sampleIndex) |
| 233 | { |
| 234 | // Compile the data |
| 235 | String<StringLength256> currentLine; |
| 236 | currentLine.printf("%u,%.2f", msg.firstSampleNumber + sampleIndex, (double)FetchLEF32(dataPtr)); // sample number and time stamp |
| 237 | if (filterRequested & CL_RECORD_RAW_ENCODER_READING) { currentLine.catf(",%" PRIi32, FetchLEI32(dataPtr)); } |
| 238 | if (filterRequested & CL_RECORD_CURRENT_MOTOR_STEPS) { currentLine.catf(",%.2f", (double)FetchLEF32(dataPtr)); } |
| 239 | if (filterRequested & CL_RECORD_TARGET_MOTOR_STEPS) { currentLine.catf(",%.2f", (double)FetchLEF32(dataPtr)); } |
| 240 | if (filterRequested & CL_RECORD_CURRENT_ERROR) { currentLine.catf(",%.2f", (double)FetchLEF32(dataPtr)); } |
| 241 | if (filterRequested & CL_RECORD_PID_CONTROL_SIGNAL) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 242 | if (filterRequested & CL_RECORD_PID_P_TERM) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 243 | if (filterRequested & CL_RECORD_PID_I_TERM) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 244 | if (filterRequested & CL_RECORD_PID_D_TERM) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 245 | if (filterRequested & CL_RECORD_PID_V_TERM) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 246 | if (filterRequested & CL_RECORD_PID_A_TERM) { currentLine.catf(",%.1f", (double)FetchLEF16(dataPtr)); } |
| 247 | if (filterRequested & CL_RECORD_CURRENT_STEP_PHASE) { currentLine.catf(",%u", FetchLEU16(dataPtr)); } |
| 248 | if (filterRequested & CL_RECORD_DESIRED_STEP_PHASE) { currentLine.catf(",%u", FetchLEU16(dataPtr)); } |
| 249 | if (filterRequested & CL_RECORD_PHASE_SHIFT) { currentLine.catf(",%u", FetchLEU16(dataPtr)); } |
| 250 | if (filterRequested & CL_RECORD_COIL_A_CURRENT) { currentLine.catf(",%d", FetchLEI16(dataPtr)); } |
| 251 | if (filterRequested & CL_RECORD_COIL_B_CURRENT) { currentLine.catf(",%d", FetchLEI16(dataPtr)); } |
| 252 | currentLine.cat("\n"); |
| 253 | |
| 254 | // Write the data |
| 255 | f->Write(currentLine.c_str()); |
| 256 | |
| 257 | // Increment the working variables |
| 258 | expectedRemoteSampleNumber++; |
| 259 | } |
| 260 | |
| 261 | if (msg.lastPacket) |
| 262 | { |
| 263 | if (msg.overflowed) |
| 264 | { |
| 265 | f->Write("Buffer overflowed\n"); |
| 266 | } |
| 267 | if (msg.badSample) |
| 268 | { |
| 269 | f->Write("Data contains bad sample(s)\n"); |
| 270 | } |
nothing calls this directly
no test coverage detected