Serial.println() affects Serial1 readings
I'm having trouble converting some C++ code to Arduino. Any help would be appreciated.
EDIT I have successfully done the above. However the only problem now is that the Arduino code I have reads the voltage accurately and properly but no other register. I can write the throttle too. If I call a different amount of Serial.println() statements, the readings on the other registers change and in some cases the voltage register stops working too.This is found in my code when I do
Serial.print("Voltage: );
If I printed out all of these registers the answers change. I can't figure out why this happens.
/* DEFINITIONS */
#include <math.h>
/* FLOATS */
uint8_t command[5];
uint8_t response[3];
/* INTEGERS */
byte deviceId = 0x17;
double throttleOut = 0;
double voltage = 0;
double rippleVoltage = 0;
double current = 0;
double power = 0;
double throttle = 0;
double pwm = 0;
double rpm = 0;
double temp = 0;
double becVoltage = 0;
double safeState = 0;
double linkLiveEnabled = 0;
double eStopStatus = 0;
double rawNTC = 0;
/* SETUP */
void setup() {
Serial1.begin(115200);
Serial.begin(115200);
}
void loop() {
flushPort();
ReadWriteRegister(128, 1000, true);//_throttleOut is 0[0%] to 65535[100%]
voltage = ReadWriteRegister(0, 0, false) / 2042.0 / 0.05;
rippleVoltage = ReadWriteRegister(1, 0, false) / 2042 / 0.25;
current = ReadWriteRegister(2, 0, false) / 204200 * 50;
power = voltage * current;
throttle = (ReadWriteRegister(3, 0, false) / 2042.0 / 1.0);
pwm = ReadWriteRegister(4, 0, false) / 2042.0 / 3.996735;
rpm = ReadWriteRegister(5, 0, false) / 2042.0 / 4.89796E-5;
int poleCount = 20;//Motor pole count
rpm = rpm / (poleCount / 2);
temp = ReadWriteRegister(6, 0, false) / 2042.0 * 30.0;
becVoltage = ReadWriteRegister(7, 0, false) / 2042 / 0.25;
safeState = ReadWriteRegister(26, 0, false);
linkLiveEnabled = ReadWriteRegister(25, 0, false);
eStopStatus = ReadWriteRegister(27, 0, false) == 0 ? false : true;
rawNTC = ReadWriteRegister(9, 0, false) / 2042.0 / 0.01567091;
rawNTC = 1.0 / (log(rawNTC * 10200.0 / (255.0 - rawNTC) / 10000.0 ) / 3455.0 + 1.0 / 298.0) - 273.0;
Serial.print("Voltage: ");
Serial.println(voltage);
Serial.print("Current: ");
Serial.println(current);
}
void flushPort() {
command[0] = command[1] = command[2] = command[3] = command[4] = 0;
Serial1.write(command, 5);
while (Serial1.available() > 0) {
Serial1.read();
}
}
double ReadWriteRegister(int reg, int value, bool writeMode) {
// Send read command
command[0] = (byte)(0x80 | deviceId);
command[1] = (byte)reg;
command[2] = (byte)((value >> 8) & 0xFF);
command[3] = (byte)(value & 0xFF);
command[4] = (byte)(0 - command[0] - command[1] - command[2] - command[3]);
Serial1.write(command, 5);
// Read response
if(Serial1.available() == 3) {
response[0] = (byte)Serial1.read();
response[1] = (byte)Serial1.read();
response[2] = (byte)Serial1.read();
}
if ((byte)(response[0] + response[1] + response[2]) == 0)
{
return (double)((response[0] << 8) + (response[1]));
}
else
{
Serial.println("Error communicating with device!");
}
}
EDIT 2 Some photos of a usb logic analyzer shots. [ 
] [ 
] [ 
] [ 
] [ 
] [ 
] [ 
] And all of the packets together in this one: [ 
] Maybe this will help with timeouts etc. It's all the info I have :.
There's no way that ReadWriteRegister will work. At 115200, each character takes about 87us to be sent or received. In that time, the Arduino can execute about 100 lines of code!
Look at this snippet:
Serial1.write(command, 5);
// Read response
if(Serial1.available() == 3) {
The write function only puts the command in the output buffer and starts sending the first character. It returns before all the characters have been transmitted. That will take 500us!
Then, you look to see if there a 3-character response has been received. But the command hasn't finished being transmitted, and you certainly didn't wait 258us (3 times 86us). It could even take longer if the device needs time to execute your command.
You have to do two things: wait for the command to be sent, and wait for the response to be received. Try this:
Serial1.write(command, 5);
Serial1.flush(); // wait for command to go out
// Wait for response to come back
while (Serial1.available() < 3)
; // waitin'....
// Read response
response[0] = (byte)Serial1.read();
response[1] = (byte)Serial1.read();
response[2] = (byte)Serial1.read();
This is called "blocking", because the Arduino won't do anything else while you're waiting for the response.
However, if a character is missed, your program could "hang" there, waiting for a 4th character if the 2nd character is not sent/received correctly (it happens). So you should put a 500us timeout in that while loop:
// Wait for response
uint32_t startTime = micros();
while ((Serial1.available() < 3) && (micros() - startTime < 500UL))
; // waitin'...
... or longer, if you know how quickly the device will respond. Then you can determine whether you actually got a response:
UPDATED with complete program (v2):
/* DEFINITIONS */
#include <math.h>
/* INTEGERS */
byte deviceId = 0x17;
uint8_t command[5];
uint8_t response[3];
/* FLOATS */
double throttleOut = 0.0;
double voltage = 0.0;
double rippleVoltage = 0.0;
double current = 0.0;
double power = 0.0;
double throttle = 0.0;
double pwm = 0.0;
double rpm = 0.0;
double temp = 0.0;
double becVoltage = 0.0;
uint8_t safeState = 0;
uint8_t linkLiveEnabled = 0;
bool eStopStatus = 0;
double rawNTC = 0.0;
/* SETUP */
void setup() {
Serial1.begin(115200);
Serial.begin(115200);
Serial.println( F("---------------------------") );
// According to the spec, you can synchronize with the device by writing
// five zeroes. Although I suspect this is mostly for the SPI and I2c
// interfaces (not TTL-level RS-232), it won't hurt to do it here.
Serial1.write( command, 5 );
delay( 250 ); // ms
while (Serial1.available())
Serial1.read(); // throw away
// Set the throttle just once
ReadWriteRegister(128, 1000);//_throttleOut is 0[0%] to 65535[100%]
}
// For 12-bit A/D conversions, the range is 0..4096. Values at
// the top and bottom are usually useless, so the value is limited
// to 6..4090 and then shifted down to 0..4084. The middle of this
// range will be the "1.0" value: 2042. Depending on what is being
// measured, you still need to scale the result.
const double ADC_FACTOR = 2042.0;
void loop() {
uint32_t scanTime = millis(); // mark time now so we can delay later
voltage = ReadWriteRegister( 0, 0 ) / ADC_FACTOR * 20.0;
rippleVoltage = ReadWriteRegister( 1, 0 ) / ADC_FACTOR * 4.0;
current = ReadWriteRegister( 2, 0 ) / ADC_FACTOR * 50.0;
power = voltage * current;
throttle = ReadWriteRegister( 3, 0 ) / ADC_FACTOR * 1.0;
pwm = ReadWriteRegister( 4, 0 ) / ADC_FACTOR * 0.2502;
rpm = ReadWriteRegister( 5, 0 ) / ADC_FACTOR * 20416.66;
const int poleCount = 20;//Motor pole count
rpm = rpm / (poleCount / 2);
temp = ReadWriteRegister( 6, 0 ) / ADC_FACTOR * 30.0;
becVoltage = ReadWriteRegister( 7, 0 ) / ADC_FACTOR * 4.0;
safeState = ReadWriteRegister( 26, 0 );
linkLiveEnabled = ReadWriteRegister( 25, 0 );
eStopStatus = ReadWriteRegister( 27, 0 );
rawNTC = ReadWriteRegister( 9, 0 ) / ADC_FACTOR * 63.1825;
const double R0 = 1000.0;
const double R2 = 10200.0;
const double B = 3455.0;
rawNTC = 1.0 / (log(rawNTC * R2 / (255.0 - rawNTC) / R0 ) / B + 1.0 / 298.0) - 273.0;
Serial.print( F("Voltage: ") );
Serial.println(voltage);
Serial.print( F("Current: ") );
Serial.println(current);
Serial.print( F("Throttle: ") );
Serial.println(throttle);
Serial.print( F("RPM: ") );
Serial.println(rpm);
// These prints do not actually send the characters, they only queue
// them up to be sent gradually, at 115200. The characters will be
// pulled from the output queue by a TX interrupt, and given to the
// UART one at a time.
//
// To prevent these interrupts from possibly interfering with any other
// timing, and to pace your program, we will wait *now* for all the
// characters to be sent to the Serial Monitor.
Serial.flush();
// Let's pace things a little bit more for testing: delay here until
// it's time to scan again.
const uint32_t SCAN_INTERVAL = 1000UL; // ms
while (millis() - scanTime < SCAN_INTERVAL)
; // waitin'
}
int16_t ReadWriteRegister(int reg, int value) {
// Flush input, as suggested by Gee Bee
while (Serial1.available() > 0)
Serial1.read();
// Send command (register number determines whether it is read or write)
command[0] = (byte)(0x80 | deviceId);
command[1] = (byte)reg;
command[2] = (byte)((value >> 8) & 0xFF);
command[3] = (byte)(value & 0xFF);
command[4] = (byte)(0 - command[0] - command[1] - command[2] - command[3]);
Serial1.write(command, 5);
// The command bytes are only queued for transmission, they have not
// actually gone out. You can either wait for command to go out
// with a `Serial1.flush()` *OR* add the transmission time to the
// timeout value below. However, if anything else has queued bytes
// to be sent and didn't wait for them to go out, the calculated
// timeout would be wrong. It is safer to flush now and guarantee
// that *all* bytes have been sent: anything sent earlier (I don't
// see anything else, but you may change that later) *plus*
// these 5 command bytes.
Serial1.flush();
// Now wait for response to come back, for a certain number of us
// The TIMEOUT could be as short as 3 character times @ the Serial1
// baudrate: 3 * (10 bits/char) / 115200bps = 261us. This is if
// the device responds immediately. Gee Bee says 20ms, which would
// be 20000UL. There's nothing in the spec, but 1ms seems generous
// for reading the raw NTC value, which may require an ADC conversion.
// Even the Arduino can do that in 100us. Try longer if you get
// timeout warnings.
const uint32_t TIMEOUT = 2000UL;
uint32_t startTime = micros();
while ((Serial1.available() < 3) && (micros() - startTime < TIMEOUT))
; // waitin'...
int16_t result;
if (Serial1.available() >= 3) {
response[0] = (byte)Serial1.read();
response[1] = (byte)Serial1.read();
response[2] = (byte)Serial1.read();
// Verify the checksum
if (response[0] + response[1] + response[2] != 0) {
Serial.print( reg );
Serial.println( F(" Checksum error!") );
Serial.flush(); // optional, use it for now to stay synchronous
}
// Cast to 16 bits *first*, then shift and add
result = (((int16_t) response[0]) << 8) + (int16_t) response[1];
} else {
// Must have timed out, because there aren't enough characters
Serial.print( reg );
Serial.println( F(" Timed out!") );
Serial.flush(); // optional, use it for now to stay synchronous
result = 0;
}
return result; // You must always return something
}
Comments:
double outside the addition caused you to lose the top 8 bits, I think. Calculating as above should give the correct answer. ReadWriteRegister function above. The function can tell you if it timed out or if the checksum was wrong. This would also imply that longer timeouts may be required. It's not clear if your device waits up to 480ms to get the latest value, or if it is continuously cacheing them and responds immediately with the last value recevied from the ESC. Writes, however, will not be reflected in the read values for up to 480ms, because of the time it takes the ESC to receive a command and then send the new values. See ESC Castle Link protocol. ReadWriteRegister function returns a 16-bit integer, which will be more efficient. Comparing floating-point numbers is never good. BTW, double is just single float on the 8-bit Arduinos. ReadWriteRegister function doesn't need the writemode argument, because the register number determines whether you are writing or reading the device. throttle value is only performed in setup. UPDATE 2
Your L ogic A nalyzer shots appear to show a "scan" for ESCs. It is trying each device ID, and some of them reply with a non-zero voltage. Also, it appears to be running at 9600, NOT 115200. Is this from a different setup?
Regardless, it confirms what the controller spec said: write 5 bytes, read 3. The checksum values are as expected. However, it is running 10 times slower than your program, so it doesn't provide much new information regarding timeouts. It may imply there is a small delay before the device responds, perhaps ~1 bit time, or about 100us.
Have you read the controller spec? You should compare the program to the spec to make sure you understand how the controller works.
I have modified the program above to:
setup (write 5 zero bytes and wait 250ms), double (see safeState , linkLiveStatus , and eStopStatus ), reg number when an error occurs. If you want to be successful in this area, you must learn to read a specification and translate its requirements into code that conforms. The program you started with is non-conformal at worst, or misleading at best. I am particularly amused by the comments that say "INTEGERS" and "FLOATS", yet those sections contain the opposite.
Perhaps this is a lesson in fixing someone else's code? It truly has many problems that you will encounter. If I had a nickel for every time I said:
...I would be a very rich man. :)
(end of updates)
PS
This also matches the symptoms you describe: Because you didn't wait for the transmission to complete, 0 bytes are read the first time through ( read will return a -1 or 0xFF byte).
After you've called this routine several times (and queued up several command into the output buffer), 500us have elapsed and the first command has finally been sent. The device responds by starting to send 3 characters. 87us later, the Arduino has finally received the first character. It gets read by one of your read statements, but who knows which one? It will be random, based on the elapsed time.
More commands get sent, and individual characters are received and read by one of those statements, until 64 bytes of commands OR Serial.println chars are queued up. Then the write command OR Serial.print blocks until the output has room for the newest command. (This addresses the title of your question.)
When enough command bytes or debug message chars are finally transmitted, the Serial1.write or Serial.print returns. In the meantime, received characters are going into the input buffer. (That's where they are stored until you call read .)
At this point, three read statements in a row will actually get characters that the device sent. But because of the random consumption of characters earlier, it could be the last character of one response, followed by the first two characters of the next response. You're "out-of-sync" with the 3-byte responses.
To stay "in synch" with the device, you have to wait for the send to complete with flush , and wait for the response to come back with while .
Sorry, but this code has some issues. You assume that
Use the following pattern in ReadWriteRegister:
In the code:
double ReadWriteRegister(int reg, int value, bool writeMode) {
// Send read command
command[0] = (byte)(0x80 | deviceId);
command[1] = (byte)reg;
command[2] = (byte)((value >> 8) & 0xFF);
command[3] = (byte)(value & 0xFF);
command[4] = (byte)(0 - command[0] - command[1] - command[2] - command[3]);
// discard any input
while (Serial1.available() > 0) {
Serial1.read();
}
// write out command
Serial1.write(command, 5);
// wait for 3 bytes
uint32_t startTime = micros();
while ((Serial1.available() < 3) && (micros() - startTime < 20000UL))
; // waiting for 20 ms for the answer
// Read response
if(Serial1.available() >= 3) { // >=3 accept also more bytes
response[0] = (byte)Serial1.read();
response[1] = (byte)Serial1.read();
response[2] = (byte)Serial1.read();
}
if ((byte)(response[0] + response[1] + response[2]) == 0)
{
return (double)((response[0] << 8) + (response[1]));
}
else
{
Serial.println("Error communicating with device!");
return 0;
}
}
上一篇: 仅用于*子目录的缓存？