Pixhawk custom sensor integration

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!

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • 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....
    • I wouldn't just change the baud rate and set GPS2_ENABLE to true because there is probably other code that is checking that same code.
      • 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?

This reply was deleted.

Activity

Joseph Udofia liked Joseph Udofia's profile
Wednesday
David Hori liked Isabella Domi's profile
Jan 12
More…