Hi everyone,
i am trying to integrate a wind sensor into Pixhawks hard- and software. All the work is done within my masters thesis.
Since now things worked good and I build a 5kg octokopter controlled by the pixhawk running arducopter on it. I already mounted the sensor on the copter and connected it to the serial 5 port. The sensor outputs the data as an RS232 signal which is converted via MAX3232 to 5V TTL level. So all the physical work is done and I have to do the software integration now. I spent a lot of work on changing the sf0x driver (also connected via serial port if you use it) from the pixhawk code base to my needs without success. What I wanted to do is reading the sensor data through the serial port and log it on the sd card. If it is possible I want to use the arducopter code base to be still able to connect to mission planner.
The message from the sensor looks like this:
<STX><ID>,11.111,22.222,33.333,44.444<ETX>CC<CR><LF>
Is there anybody who has done something similar and can share some code or help me ???
I´m really looking forward to hearing fom you!
Replies
Thank you!
Today I searched through the code and commented out the appropriate lines. Tomorrow I´ll give it a try. I´m curious and sure that it will work now.
I´ll keep you up to date.
Cheers!
YES you were right!
Everything works fine now. Thank you very much.
To summarize it:
- I commented out this lines in the code (ArduCopter 3.2) to enable UartE (Serial 4) on pixhawk.
// HAL_PX4_Class.cpp : l. 292-301
// AP_GPS.cpp : l. 76-80
// AP_GPS.cpp : l. 107-119
// AP_GPS.cpp : l. 131
// GCS_serial_control.cpp : l. 58-61
// SITL_State.cpp : l. 654
// system.pde : l. 106-110
and changed in:
// HAL_PX4_Class.cpp : l.125
the baudrate to that one I´m using.
I did not get a lot of time to spend on this but I did get my set up reading the serial data. Just to get you going while I fix up the logging - this code reads input at 115200 baud on serial port 4 in the format 11111,22222,33333,44444<LF> (4 comma separated unsigned integers of variable character length terminated by '\n') and updates the user variables, temp, xVal, yVal, zVal
UserCode.pde: (you must also uncomment the appropriate USERHOOK_XXX defines in APM_Config.h)
void check_sensor()
{
static char incoming[10];
static uint8_t index=0;
static uint8_t value_index=0;
uint8_t data;
int16_t numc;
numc = sensor_port->available(); // Number of bytes available in rx buffer
for (int16_t i = 0; i < numc; i++) { // Process bytes received
data = sensor_port->read(); // Read the next byte
// hal.console->printf("%c",data);
if(data==','){ // seperator
incoming[index]='\0'; // Place null char to mark end of string
switch(value_index++){
case 0: temp=strtol(incoming,NULL,10); break;
case 1: xVal=strtol(incoming,NULL,10); break;
case 2: yVal=strtol(incoming,NULL,10); break;
} // Switch
index=0;
}
else if(data=='\n') { // Look for newline char separately to determine the end of current data
incoming[index]='\0'; // Place null to mark end of string
zVal=strtol(incoming,NULL,10); // Convert string to float and assign to global sensor_value
index=0; // Reset indexes for next line reading
value_index=0;
//hal.console->printf("Temp: %i, xVal: %i, yVal: %i, zVal: %i\n",temp, xVal, yVal,zVal);
} else //continue accumulating bytes
incoming[index++]=data; // Add next byte to string
}
}
void userhook_init()
{
sensor_port->begin(115200,128, 0); //115200 baud, 128 byte input buffer, 0 byte output buffer
sensor_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); //Disable flow control
}
void userhook_MediumLoop()
{
check_sensor(); //Check serial input at 10Hz
}
UserVariables.h:
#define sensor_port hal.uartE
uint16_t temp, xVal, yVal, zVal=0;
James,
thank you VERY much so far! I'm going to test it on my copter immediately. I'll kepp you up to date.
Hi Marvin,
You are welcome. I managed to get the logging sorted before I left work. I need to gather data tomorrow to calibrate the sensor. Hopefully we have a relatively wind free day so I can mount the sensor along with the Pixhawk + GPS on a vehicle to gather wind data across the speed/direction ranges I require.... I will post all the altered source files in the morning.
Hi Marvin,
Attached are the altered files (from current master) including logging to dataflash (SENS,temp,xVal,yVal,zVal). Did you have any luck with the code to parse your input?
Cheers,
James
sensor.zip
Hi James,
great job thanks again!!!
The last days a played around with your code and modified it for my needs. Finally I got it working quite good. The code below could be used to process serial data in form of:
xxx,-111.11,+222.22,-333.33,x,xxx<LF>
(where " x " is stuff which isn't needed)
The data is read now from uartD (/dev/ttyS2) because uartE didn't work. The values are defined and parsed as floats and saved in the dataflash logs.
Moreover I tried to display data in the real time plot in Mission Planner as well as saving them to the telemetry logs by modifying the sonar_alt log as mentioned here:
Custom sensors and Real-Time logging
And this is the final code I am using (today):
UserCode.pde:
// Write a sensors packet
static void Log_Write_Sensor()
{
struct log_Sensor pkt =
{
LOG_PACKET_HEADER_INIT(LOG_SENSOR_MSG),
xVal : xVal,
yVal : yVal,
zVal : zVal
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void check_sensor()
{
static char incoming[100];
static uint8_t index = 0;
static uint8_t value_index = 0;
char data;
int16_t numc;
numc = sensor_port->available(); // Number of bytes available in rx buffer
for (int16_t i = 0; i < numc; i++) // Process bytes received
{
data = sensor_port->read(); // Read the next byte
// hal.console->printf("Incoming:");
for(int j = 0; j < 100; j++)
{
//hal.console->printf("%c", incoming[j]);
}
//hal.console->printf("\n");
if(data == ',')
{ // Value seperator
incoming[index]='\0'; // Place null char to mark end of string
for(int k = 0; k < (99 - index); k++) // write zeros after the sensor values in the buffer
{
incoming[index + 1 + k] = 0;
}
value_index++;
switch(value_index)
{
case 2:
xVal = atof(incoming);
break;
case 3:
yVal = atof(incoming);
break;
case 4:
zVal = atof(incoming);
break;
} // Switch
index=0;
}
else if(data=='\n') // Look for newline char to mark end of current data
{
index = 0; // Reset indexes for next line reading
value_index = 0;
// hal.console->printf("xVal: %f, yVal: %f, zVal: %f\n", xVal, yVal, zVal);
Log_Write_Sensor();
}
else
{
incoming[index] = data; // Add next byte to string
if(index == 99)
{
index = 0;
}
index++;
}
}
}
#ifdef USERHOOK_INIT
void userhook_init()
{
sensor_port->begin(19200,128,0);
sensor_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); //Disable flow control
}
#endif
#ifdef USERHOOK_FASTLOOP
void userhook_FastLoop()
{
// put your 100Hz code here
check_sensor(); // Log_Write_Sensor() is called from check_sensor()
sonar_alt = xVal * 1000;
hal.console->printf("%f\n",xVal);
hal.console->printf("%i\n",sonar_alt);
}
#endif
#ifdef USERHOOK_50HZLOOP
void userhook_50Hz()
{
// put your 50Hz code here
}
#endif
#ifdef USERHOOK_MEDIUMLOOP
void userhook_MediumLoop()
{
// 10 Hz Code
// sonar_alt = xVal * 1000;
// hal.console->printf("%f\n",xVal);
// hal.console->printf("%i\n",sonar_alt);
}
#endif
#ifdef USERHOOK_SLOWLOOP
void userhook_SlowLoop()
{
// put your 3.3Hz code here
}
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
void userhook_SuperSlowLoop()
{
// put your 1Hz code here
}
#endif
UserVariables.h:
#ifdef USERHOOK_VARIABLES
#define sensor_port hal.uartD
float xVal;
float yVal;
float zVal;
#define LOG_SENSOR_MSG 0xF0
struct PACKED log_Sensor {
LOG_PACKET_HEADER;
float xVal;
float yVal;
float zVal;
};
#endif // USERHOOK_VARIABLES
And line 715-716 in Log.pde:
{ LOG_SENSOR_MSG, sizeof(log_Sensor),
"SENS", "fff", "xVal,yVal,zVal" },
Moreover I uncommented the 10Hz- and the 100Hz-Loop as well as the UserVariables.h in APMConfig.h.
So there are still some small issues but all in all I made a huge step forward!
Hi Marvin,
Looks good. I am glad that you found my code useful. Just a couple of comments on your changes:
The variable incoming[] only needs to hold enough characters for one field + a null, so from your spec above, -123.45 + null or 8 chars. You have allocated 100 bytes for this which seems an unnecessary waste. I see that you are detecting an overrun on incoming[] by wrapping back to a zero index when > 99. Although I believe you do not need quite so much space in your buffer, this is something I should also have done in my code. If for some reason the serial stream is not what we expect and has no ',' in the stream (i.e. different baud rate) then my code would over run the array and randomly write over memory assigned to other vars with potentially disastrous consequences... I have altered my code from:
incoming[index++]=data; // Add next byte to string
to:
incoming[index++]=data; // Add next byte to string
if(index==10)index=9;
Also, you should not need to write all zeros to pad out the end of incoming[] after each time it is used as you fill it up sequentially from the start and mark the end with a null. A single null char is all that C/C++ needs to mark the end of a string. The atof() function never sees anything it is not supposed to.
You are calling Log_Write_Sensor() from within check_sensor() which you have being called at 100Hz. When check_sensor() is called we have no idea where in the sensor_port byte stream we are. This is why we have to process it byte by byte and determine our position by aligning to <LF> chars. If you output the serial data received to the console and add a marker char every time you call check_sensor() you will see that it appears pretty randomly in the data. The point is, you do not necessarily have a complete data sentence present at the end of every call to check_sensor(). In your case where you are looking at a serial port at 19200 baud and expecting 34 bytes, the maximum throughput would be 34*8 (bits/byte) + 34 (stop bits)=306bits. 19200/306 or slightly more than 62 sentences/second. This is the absolute max and does not take into account how often the attached sensor actually transmits the data. Unless you are using an ultrasonic sensor, I can't imagine that your wind sensor has anything like an effective temporal resolution that could make use of 100Hz logging. check_sensor() needs to be called at least as often as required to keep up with the data and not over run the buffer - and there is no harm in calling it more frequently if you have the processing overhead. I would place this code in the slowest loop that gives you the logging frequency required.
You obviously picked up that the format field of LOG_SENSOR_MSG needed altering from 'llll' to 'fff' - good spotting :)
BTW - what sensor are you using?
-
1
-
2
of 2 Next