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!

Views: 6342

Reply to This

Replies to This Discussion

I have interfaced a number of sensors with the Pixhawk including using the serial ports. I have previously posted code on how I did this including logging via telemetry and dataflash. I just tried to re-compile some of these examples against the current code but there are conflicting changes I need to resolve. It just so happens that this very day I planned to interface a wind sensor to the Pixhawk (3D velocity). I am wondering how you plan to mount your sensor so as to avoid what must be significant turbulence around the copter due to the rotors? I find it hard to imagine any setup that would give reasonable wind data... In my case I have the Pixhawk + sensors mounted on a kite so do not have to deal with the altered airflow due to the rotors. I still have taken care to place the wind sensor in a location that minimises the influence of the kite on the airflow. I am happy to share my code. Please stand by....

Hi James,

that sounds good and is exactly what I want to do. Yes you are right it is quite difficult to find the best place for mounting the sensor. I did some cfd simulations on this and I can tell you that on the upper side of the copter there is the best place to position the sensor.

Moreover you have to calculate the influence of the motion produced by the attitude control all the time.

Thank you so far. Hope you get things done soon because I'm curious to have a look at your code.

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

Attachments:

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?

Thank you very much James!

It´s quite a long time ago since I joint this thread but now I´ll work further on my drone.

I´m really sorry but at the moment I´m not allowed to tell any details about the sensor.

Maybe you would like to have a look at my other thread?

Uploading and Compiling ArduCopter V3.2 with Eclipse

Greets Marvin

I´m in big trouble !!!

Hi everyone, I added and changed the code like James told above. If I start the pixhawk there are only some bytes coming into the serial port. But these are not the bytes beeing sent from the sensor. There are only some random characters and special signs arriving in the console. (I´m using putty connected to the usb port of pixhawk at 38400 baud to display the console; I prited out "numc" and "data" from the UserCode.pde) But I´m sure that the sensor is configured at 19200 baud and sending data at 10Hz.

I also tried the code posted above with an arduino and it worked very well without modification. So everthing is hooked up correctly and the sensor is working. TX and RX are correctly connected and the baudrate is the same on both ends. 

Moreover I tried to send a copy of the sensor data string from the arduino to the pixhawk port which gave the same corrupted input in pixhawk.

It is like the data is not arriving the right way in the buffer or something like that. 

So I think there is a problem with the serial 4 (I also tried telemetry 2) -port on pixhawk. Is there a trick to unlock this ports for serial input ???

Do I have to change the initialisation of this ports somewhere else in the code ???

Is my code interfering with mavlink and do I have to disable/ enable mavlink on the used port?

I REALLY dont know how to get things done because I know the code is able to work on an arduino and handling time of my master thesis is going out.

I guess there are also other people out there having the same problem as I read in some threads, but nobody posted a solution.

If someone could help here I think this thread would be the first step by step instruction for a pixhawk custom sensor integration and reading serial data into it.  

James how did you managed this problem?

Thanks in advance !!!

Greets Marvin

Marvin,

What version of the Arducopter code are you using? In 3.2, both serial 4 and 5 have already been assigned other purposes, a 3rd telemetry port and a 2nd gps. You'll have to search through the code and repurpose whichever serial port you were planning to use.

-Chris

Thank you Chris!

I´m using ArduCopter 3.2.

I found in ArduCopter   >  system.pde (l.106 - 110)

and in ArduCopter   >  ArduCopter.pde the same (l. 15468 - 15472)

#if GPS2_ENABLE
    if (hal.uartE != NULL) {
        hal.uartE->begin(38400, 256, 16);
    }
#endif

I´ll change this to my baudrate of 19200 so it will work if I enable GPS2.

Do you think these modifications on ArduCopter are enough or do I have to change something on PX4 flight stack too?

Reply to Discussion

RSS

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service