mavnative.c
Go to the documentation of this file.
00001 /*
00002     Native mavlink glue for python.
00003     Author: kevinh@geeksville.com
00004 */
00005 
00006 #undef NDEBUG
00007 
00008 #include <Python.h>
00009 #include <structmember.h>
00010 
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <stdint.h>
00014 #include <assert.h>
00015 #include <stddef.h>
00016 #include <setjmp.h>
00017 
00018 #if PY_MAJOR_VERSION >= 3
00019 // In python3 it only has longs, not 32 bit ints
00020 #define PyInt_AsLong PyLong_AsLong
00021 #define PyInt_FromLong PyLong_FromLong
00022 
00023 // We returns strings for byte arreays in python2, but bytes objects in python3
00024 #define PyByteString_FromString PyBytes_FromString
00025 #define PyByteString_FromStringAndSize PyBytes_FromStringAndSize
00026 #define PyByteString_ConcatAndDel PyBytes_ConcatAndDel
00027 #else
00028 #define PyByteString_FromString PyString_FromString
00029 #define PyByteString_FromStringAndSize PyString_FromStringAndSize
00030 #define PyByteString_ConcatAndDel PyString_ConcatAndDel
00031 #endif
00032 
00033 #include "mavlink_defaults.h"
00034 
00035 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
00036 
00037 #include <mavlink_types.h>
00038 
00039 // Mavlink send support
00040 // Not currently used, but keeps mavlink_helpers send code happy
00041 static mavlink_system_t mavlink_system = {42,11,};
00042 static void comm_send_ch(mavlink_channel_t chan, uint8_t c) {
00043     // Sending not supported yet in native code
00044     assert(0);
00045 }
00046 
00047 #define MAVLINK_ASSERT(x) assert(x)
00048 
00049 // static mavlink_message_t last_msg;
00050 
00051 /*
00052   default message crc function. You can override this per-system to
00053   put this data in a different memory segment
00054 */
00055 #define MAVLINK_MESSAGE_CRC(msgid) py_message_info[msgid].crc_extra
00056 
00057 /* Enable this option to check the length of each message.
00058  This allows invalid messages to be caught much sooner. Use if the transmission
00059  medium is prone to missing (or extra) characters (e.g. a radio that fades in
00060  and out). Only use if the channel will only contain messages types listed in
00061  the headers.
00062 */
00063 
00064 #define MAVLINK_MESSAGE_LENGTH(msgid) py_message_info[msgid].len
00065 
00066 // #include <mavlink.h>
00067 
00068 #define TRUE 1
00069 #define FALSE 0
00070 
00071 typedef struct {
00072         PyObject                *name;               // name of this field
00073         mavlink_message_type_t  type;                // type of this field
00074         unsigned int            array_length;        // if non-zero, field is an array
00075         unsigned int            wire_offset;         // offset of each field in the payload
00076 } py_field_info_t;
00077 
00078 // note that in this structure the order of fields is the order
00079 // in the XML file, not necessary the wire order
00080 typedef struct {
00081     PyObject            *id;                                          // The int id for this msg
00082     PyObject            *name;                                        // name of the message
00083     unsigned            len;                                          // the raw message length of this message - not including headers & CRC
00084     uint8_t             crc_extra;                                    // the CRC extra for this message
00085     unsigned            num_fields;                                   // how many fields in this message
00086     PyObject            *fieldnames;                                  // fieldnames in the correct order expected by user (not wire order)
00087     py_field_info_t     fields[MAVLINK_MAX_FIELDS];                   // field information
00088 } py_message_info_t;
00089 
00090 static py_message_info_t py_message_info[256];
00091 static uint8_t           info_inited = FALSE; // We only do the init once (assuming only one dialect in use)
00092 
00093 #include <protocol.h>
00094 
00098 typedef struct {
00099     mavlink_message_t   msg;
00100     int                 numBytes;
00101     uint8_t             bytes[MAVLINK_MAX_PACKET_LEN];
00102 } py_message_t;
00103 
00104 typedef struct {
00105     PyObject_HEAD
00106 
00107     PyObject            *MAVLinkMessage;
00108     mavlink_status_t    mav_status;
00109     py_message_t        msg;
00110 } NativeConnection;
00111 
00112 // #define MAVNATIVE_DEBUG
00113 #ifdef MAVNATIVE_DEBUG
00114 #  define mavdebug    printf
00115 #else
00116 #  define mavdebug(x...)
00117 #endif
00118 
00119 
00120 // My exception type
00121 static PyObject *MAVNativeError;
00122 
00123 static jmp_buf python_entry;
00124 
00125 #define PYTHON_ENTRY if(!setjmp(python_entry)) {
00126 #define PYTHON_EXIT  } else { return NULL; }   // Used for routines thar return ptrs
00127 #define PYTHON_EXIT_INT  } else { return -1; } // Used for routines that return ints
00128 
00129 
00147 MAVLINK_HELPER uint8_t py_mavlink_parse_char(uint8_t c, py_message_t* pymsg, mavlink_status_t* status)
00148 {
00149     mavlink_message_t *rxmsg = &pymsg->msg;
00150 
00151     int bufferIndex = 0;
00152 
00153     status->msg_received = 0;
00154 
00155     switch (status->parse_state)
00156     {
00157     case MAVLINK_PARSE_STATE_UNINIT:
00158     case MAVLINK_PARSE_STATE_IDLE:
00159         if (c == MAVLINK_STX)
00160         {
00161             status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00162             rxmsg->len = 0;
00163             pymsg->numBytes = 0;
00164             rxmsg->magic = c;
00165             mavlink_start_checksum(rxmsg);
00166             pymsg->bytes[pymsg->numBytes++] = c;
00167         }
00168         break;
00169 
00170     case MAVLINK_PARSE_STATE_GOT_STX:
00171             if (status->msg_received 
00172 /* Support shorter buffers than the
00173    default maximum packet size */
00174 #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
00175                 || c > MAVLINK_MAX_PAYLOAD_LEN
00176 #endif
00177                 )
00178         {
00179             status->buffer_overrun++;
00180             status->parse_error++;
00181             status->msg_received = 0;
00182             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00183         }
00184         else
00185         {
00186             // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
00187             rxmsg->len = c;
00188             status->packet_idx = 0;
00189             mavlink_update_checksum(rxmsg, c);
00190             pymsg->bytes[pymsg->numBytes++] = c;
00191             status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
00192         }
00193         break;
00194 
00195     case MAVLINK_PARSE_STATE_GOT_LENGTH:
00196         rxmsg->seq = c;
00197         mavlink_update_checksum(rxmsg, c);
00198         pymsg->bytes[pymsg->numBytes++] = c;
00199         status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
00200         break;
00201 
00202     case MAVLINK_PARSE_STATE_GOT_SEQ:
00203         rxmsg->sysid = c;
00204         mavlink_update_checksum(rxmsg, c);
00205         pymsg->bytes[pymsg->numBytes++] = c;
00206         status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
00207         break;
00208 
00209     case MAVLINK_PARSE_STATE_GOT_SYSID:
00210         rxmsg->compid = c;
00211         mavlink_update_checksum(rxmsg, c);
00212         pymsg->bytes[pymsg->numBytes++] = c;
00213         status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
00214         break;
00215 
00216     case MAVLINK_PARSE_STATE_GOT_COMPID:
00217 #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
00218             if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
00219         {
00220             status->parse_error++;
00221             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00222             break;
00223         }
00224 #endif
00225         rxmsg->msgid = c;
00226         mavlink_update_checksum(rxmsg, c);
00227         pymsg->bytes[pymsg->numBytes++] = c;
00228         if (rxmsg->len == 0)
00229         {
00230             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00231         }
00232         else
00233         {
00234             status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
00235         }
00236         break;
00237 
00238     case MAVLINK_PARSE_STATE_GOT_MSGID:
00239         _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
00240         mavlink_update_checksum(rxmsg, c);
00241         pymsg->bytes[pymsg->numBytes++] = c;
00242         if (status->packet_idx == rxmsg->len)
00243         {
00244             status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
00245         }
00246         break;
00247 
00248     case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00249 #if MAVLINK_CRC_EXTRA
00250         mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
00251 #endif
00252         pymsg->bytes[pymsg->numBytes++] = c;
00253         if (c != (rxmsg->checksum & 0xFF)) {
00254             // Check first checksum byte
00255             status->parse_error++;
00256             status->msg_received = 0;
00257             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00258             if (c == MAVLINK_STX)
00259             {
00260                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00261                 rxmsg->len = 0;
00262                 mavlink_start_checksum(rxmsg);
00263             }
00264         }
00265         else
00266         {
00267             status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
00268             _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
00269         }
00270         break;
00271 
00272     case MAVLINK_PARSE_STATE_GOT_CRC1:
00273         pymsg->bytes[pymsg->numBytes++] = c;
00274         if (c != (rxmsg->checksum >> 8)) {
00275             // Check second checksum byte
00276             status->parse_error++;
00277             status->msg_received = 0;
00278             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00279             if (c == MAVLINK_STX)
00280             {
00281                 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
00282                 rxmsg->len = 0;
00283                 mavlink_start_checksum(rxmsg);
00284             }
00285         }
00286         else
00287         {
00288             // Successfully got message
00289             status->msg_received = 1;
00290             status->parse_state = MAVLINK_PARSE_STATE_IDLE;
00291             _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
00292         }
00293         break;
00294     }
00295 
00296     bufferIndex++;
00297     // If a message has been sucessfully decoded, check index
00298     if (status->msg_received == 1)
00299     {
00300         //while(status->current_seq != rxmsg->seq)
00301         //{
00302         //  status->packet_rx_drop_count++;
00303         //               status->current_seq++;
00304         //}
00305         status->current_rx_seq = rxmsg->seq;
00306         // Initial condition: If no packet has been received so far, drop count is undefined
00307         if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
00308         // Count this packet as received
00309         status->packet_rx_success_count++;
00310     }
00311 
00312     return status->msg_received;
00313 }
00314 
00315 
00316 // Raise a python exception
00317 static void set_pyerror(const char *msg) {
00318     PyErr_SetString(MAVNativeError,  msg);
00319 }
00320 
00321 // Pass assertion failures back to python (if we can)
00322 extern void __assert_fail(const char *__assertion, const char *__file, unsigned int __line, const char *__function)
00323 {
00324     char msg[256];
00325 
00326     sprintf(msg, "Assertion failed: %s, %s:%d", __assertion, __file, __line);
00327 
00328     set_pyerror(msg);
00329     longjmp(python_entry, 1);
00330 }
00331 
00332 
00333 static unsigned get_field_size(int field_type) {
00334     unsigned fieldSize;
00335 
00336     switch(field_type) 
00337     {
00338     case MAVLINK_TYPE_CHAR: 
00339         fieldSize = 1;
00340         break;
00341     case MAVLINK_TYPE_UINT8_T:
00342         fieldSize = 1;
00343         break;
00344     case MAVLINK_TYPE_INT8_T:
00345         fieldSize = 1;
00346         break;
00347     case MAVLINK_TYPE_UINT16_T:
00348         fieldSize = 2;
00349         break;
00350     case MAVLINK_TYPE_INT16_T:
00351         fieldSize = 2;
00352         break;
00353     case MAVLINK_TYPE_UINT32_T:
00354         fieldSize = 4;
00355         break;
00356     case MAVLINK_TYPE_INT32_T:
00357         fieldSize = 4;
00358         break;
00359     case MAVLINK_TYPE_UINT64_T:
00360         fieldSize = 8;
00361         break;
00362     case MAVLINK_TYPE_INT64_T:                 
00363         fieldSize = 8;
00364         break;
00365     case MAVLINK_TYPE_FLOAT:              
00366         fieldSize = 4;
00367         break;
00368     case MAVLINK_TYPE_DOUBLE:      
00369         fieldSize = 8;
00370         break;            
00371     default:
00372         mavdebug("BAD MAV TYPE %d\n", field_type);
00373         set_pyerror("Unexpected mavlink type");
00374         fieldSize = 1;
00375     }
00376 
00377     return fieldSize;
00378 }
00379 
00380 
00386 static int get_py_typeinfo(char type_char, int array_size, unsigned *wire_offset)
00387 {
00388     int type_code;
00389 
00390     switch(type_char) 
00391     {
00392     case 'f': type_code = MAVLINK_TYPE_FLOAT; break;
00393     case 'd': type_code = MAVLINK_TYPE_DOUBLE; break;
00394     case 'c': type_code = MAVLINK_TYPE_CHAR; break;
00395     case 'v': type_code = MAVLINK_TYPE_UINT8_T; break;
00396     case 'b': type_code = MAVLINK_TYPE_INT8_T; break;
00397     case 'B': type_code = MAVLINK_TYPE_UINT8_T; break;
00398     case 'h': type_code = MAVLINK_TYPE_INT16_T; break;
00399     case 'H': type_code = MAVLINK_TYPE_UINT16_T; break;
00400     case 'i': type_code = MAVLINK_TYPE_INT32_T; break;
00401     case 'I': type_code = MAVLINK_TYPE_UINT32_T; break;
00402     case 'q': type_code = MAVLINK_TYPE_INT64_T; break;
00403     case 'Q': type_code = MAVLINK_TYPE_UINT64_T; break;
00404     default:
00405         assert(0);
00406     }
00407 
00408     int total_len = get_field_size(type_code) * (array_size == 0 ? 1 : array_size);
00409 
00410     *wire_offset += total_len;
00411 
00412     return type_code;
00413 }
00414 
00422 static void init_message_info(PyObject *mavlink_map) {
00423     // static const mavlink_message_info_t src[256] = MAVLINK_MESSAGE_INFO;
00424     
00425     PyObject *items_list = PyDict_Values(mavlink_map);
00426     assert(items_list); // A list of the tuples in mavlink_map
00427 
00428     Py_ssize_t numMsgs = PyList_Size(items_list);
00429 
00430     int i;
00431     for(i = 0; i < numMsgs; i++) {
00432         PyObject *type_class = PyList_GetItem(items_list, i); // returns a _borrowed_ reference
00433         assert(type_class);
00434 
00435         PyObject *id_obj = PyObject_GetAttrString(type_class, "id"); // A _new_ reference
00436         assert(id_obj);
00437         PyObject *name_obj = PyObject_GetAttrString(type_class, "name"); // A new reference
00438         assert(name_obj);
00439         PyObject *crc_extra_obj = PyObject_GetAttrString(type_class, "crc_extra"); // A new reference
00440         assert(crc_extra_obj);
00441         PyObject *fieldname_list = PyObject_GetAttrString(type_class, "ordered_fieldnames"); // A new reference
00442         assert(fieldname_list);
00443         //PyObject *order_list = PyObject_GetAttrString(type_class, "orders"); // A new reference
00444         //assert(order_list);
00445         PyObject *arrlen_list = PyObject_GetAttrString(type_class, "array_lengths"); // A new reference
00446         assert(arrlen_list);
00447         PyObject *type_format = PyObject_GetAttrString(type_class, "native_format"); // A new reference
00448         assert(type_format);
00449         char *type_str = PyByteArray_AsString(type_format);
00450         assert(type_str);
00451                
00452         Py_ssize_t num_fields = PyList_Size(fieldname_list);
00453 
00454         uint8_t id = (uint8_t) PyInt_AsLong(id_obj);
00455         py_message_info_t *d = &py_message_info[id];
00456 
00457         d->id = id_obj;
00458         d->name = name_obj;
00459         d->num_fields = num_fields;
00460         d->crc_extra = PyInt_AsLong(crc_extra_obj);
00461         d->fieldnames = PyObject_GetAttrString(type_class, "fieldnames"); // A new reference
00462         assert(d->fieldnames);
00463 
00464         int fnum;
00465         unsigned wire_offset = 0;
00466         for(fnum = 0; fnum < num_fields; fnum++) {
00467             PyObject *field_name_obj = PyList_GetItem(fieldname_list, fnum); // returns a _borrowed_ reference
00468             assert(field_name_obj);
00469             Py_INCREF(field_name_obj);
00470 
00471             PyObject *len_obj = PyList_GetItem(arrlen_list, fnum); // returns a _borrowed_ reference
00472             assert(len_obj);                        
00473 
00474             d->fields[fnum].name = field_name_obj;
00475             d->fields[fnum].array_length = PyInt_AsLong(len_obj);
00476             char type_char = type_str[1 + fnum];
00477             d->fields[fnum].wire_offset = wire_offset; // Store the current offset before advancing
00478             d->fields[fnum].type = get_py_typeinfo(type_char, d->fields[fnum].array_length, &wire_offset);            
00479         }
00480         d->len = wire_offset;
00481 
00482         Py_DECREF(crc_extra_obj);
00483         Py_DECREF(arrlen_list);
00484         Py_DECREF(type_format);
00485         //Py_DECREF(order_list);
00486     }
00487 
00488     Py_DECREF(items_list);
00489 }
00490 
00491 static PyObject *createPyNone(void)
00492 {
00493     Py_INCREF(Py_None);
00494     return Py_None;
00495 }
00496 
00497 
00498 
00502 static void set_attribute(PyObject *obj, const char *attrName, PyObject *val) {
00503     assert(val);
00504     PyObject_SetAttrString(obj, attrName, val);
00505     Py_DECREF(val);
00506 }
00507 
00508 
00509 
00515 static PyObject *pyextract_mavlink(const mavlink_message_t *msg, const py_field_info_t *field) {
00516     unsigned offset = field->wire_offset;
00517     int index;
00518 
00519     // For arrays of chars we build the result in a string instead of an array
00520     PyObject *arrayResult =  (field->array_length != 0 && field->type != MAVLINK_TYPE_CHAR) ? PyList_New(field->array_length) : NULL;
00521     PyObject *stringResult = (field->array_length != 0 && field->type == MAVLINK_TYPE_CHAR) ? PyByteString_FromString("") : NULL;
00522     PyObject *result = NULL;
00523 
00524     int numValues = (field->array_length == 0) ? 1 : field->array_length;
00525     unsigned fieldSize = get_field_size(field->type);
00526 
00527     uint8_t string_ended = FALSE;
00528 
00529     if(arrayResult != NULL)
00530         result = arrayResult;
00531 
00532     // Either build a full array of results, or return a single value 
00533     for(index = 0; index < numValues; index++) {
00534         PyObject *val = NULL;
00535 
00536         switch(field->type) {
00537             case MAVLINK_TYPE_CHAR: {
00538                 // If we are building a string, stop writing when we see a null char
00539                 char c = _MAV_RETURN_char(msg, offset);
00540 
00541                 if(stringResult && c == 0) 
00542                     string_ended = TRUE;
00543 
00544                 val = PyByteString_FromStringAndSize(&c, 1);
00545                 break;
00546                 }
00547             case MAVLINK_TYPE_UINT8_T:
00548                 val = PyInt_FromLong(_MAV_RETURN_uint8_t(msg, offset));
00549                 break;
00550             case MAVLINK_TYPE_INT8_T:
00551                 val = PyInt_FromLong(_MAV_RETURN_int8_t(msg, offset));
00552                 break;
00553             case MAVLINK_TYPE_UINT16_T:
00554                 val = PyInt_FromLong(_MAV_RETURN_uint16_t(msg, offset));
00555                 break;
00556             case MAVLINK_TYPE_INT16_T:
00557                 val = PyInt_FromLong(_MAV_RETURN_int16_t(msg, offset));
00558                 break;
00559             case MAVLINK_TYPE_UINT32_T:
00560                 val = PyLong_FromLong(_MAV_RETURN_uint32_t(msg, offset));
00561                 break;
00562             case MAVLINK_TYPE_INT32_T:
00563                 val = PyInt_FromLong(_MAV_RETURN_int32_t(msg, offset));   
00564                 break;
00565             case MAVLINK_TYPE_UINT64_T:
00566                 val = PyLong_FromLongLong(_MAV_RETURN_uint64_t(msg, offset));
00567                 break;
00568             case MAVLINK_TYPE_INT64_T:
00569                 val = PyLong_FromLongLong(_MAV_RETURN_int64_t(msg, offset));
00570                 break;
00571             case MAVLINK_TYPE_FLOAT:
00572                 val = PyFloat_FromDouble(_MAV_RETURN_float(msg, offset));
00573                 break;
00574             case MAVLINK_TYPE_DOUBLE:
00575                 val = PyFloat_FromDouble(_MAV_RETURN_double(msg, offset));
00576                 break;            
00577             default:
00578                 mavdebug("BAD MAV TYPE %d\n", field->type);
00579                 set_pyerror("Unexpected mavlink type");
00580                 return NULL;
00581         }
00582         offset += fieldSize;
00583 
00584         assert(val);
00585         if(arrayResult != NULL)  
00586             PyList_SetItem(arrayResult, index, val);
00587         else if(stringResult != NULL) {
00588             if(!string_ended)
00589                 PyByteString_ConcatAndDel(&stringResult, val);
00590             else
00591                 Py_DECREF(val); // We didn't use this char
00592 
00593             result = stringResult;
00594         }
00595         else // Not building an array
00596             result = val;
00597     }
00598 
00599     assert(result);
00600     return result;
00601 }
00602 
00603 
00611 static PyObject *msg_to_py(PyObject* msgclass, const py_message_t *pymsg) {
00612     const mavlink_message_t *msg = &pymsg->msg;
00613     const py_message_info_t *info = &py_message_info[msg->msgid];
00614 
00615     mavdebug("Found a msg: %s\n", PyString_AS_STRING(info->name));
00616 
00617     /* Call the class object to create our instance */
00618     // PyObject *obj = PyObject_CallObject((PyObject *) &NativeConnectionType, null);
00619     PyObject *argList = PyTuple_Pack(2, info->id, info->name);
00620     PyObject *obj = PyObject_CallObject(msgclass, argList);
00621     uint8_t objValid = TRUE;
00622     assert(obj);
00623     Py_DECREF(argList);
00624 
00625     // Find the header subobject
00626     PyObject *header = PyObject_GetAttrString(obj, "_header");
00627     assert(header);
00628 
00629     // msgid already set in the constructor call
00630     set_attribute(header, "mlen", PyInt_FromLong(msg->len));
00631     set_attribute(header, "seq", PyInt_FromLong(msg->seq));
00632     set_attribute(header, "srcSystem", PyInt_FromLong(msg->sysid));
00633     set_attribute(header, "srcComponent", PyInt_FromLong(msg->compid));
00634     Py_DECREF(header);
00635     header = NULL;
00636 
00637     // FIXME - we should generate this expensive field only as needed (via a getattr override)
00638     set_attribute(obj, "_msgbuf", PyByteArray_FromStringAndSize((const char *) pymsg->bytes, pymsg->numBytes));
00639 
00640     // Now add all the fields - FIXME - do this lazily using getattr overrides
00641     PyObject_SetAttrString(obj, "_fieldnames", info->fieldnames); // Will increment the reference count
00642 
00643     // FIXME - reuse the fieldnames list from python - so it is in the right order
00644 
00645     int fnum;
00646     for(fnum = 0; fnum < info->num_fields && objValid; fnum++) {
00647         const py_field_info_t *f = &info->fields[fnum];
00648         PyObject *val = pyextract_mavlink(msg, f);
00649 
00650         if(val != NULL) {
00651             PyObject_SetAttr(obj, f->name, val);
00652             Py_DECREF(val); // We no longer need val, the attribute will keep a ref
00653         }
00654         else 
00655             objValid = FALSE;
00656     }
00657 
00658     if(objValid)
00659         return obj;
00660     else {
00661         Py_DECREF(obj);
00662         return NULL;
00663     }
00664 }
00665 
00666 
00670 static int get_expectedlength(NativeConnection *self)
00671 {
00672     int desired;
00673 
00674     mavlink_message_t *msg = &self->msg.msg;
00675 
00676     switch(self->mav_status.parse_state) {
00677         case MAVLINK_PARSE_STATE_UNINIT: 
00678         case MAVLINK_PARSE_STATE_IDLE: 
00679             desired = 8;
00680             break;
00681         case MAVLINK_PARSE_STATE_GOT_STX: 
00682             desired = 7;
00683             break;
00684         case MAVLINK_PARSE_STATE_GOT_LENGTH:
00685             desired = msg->len + 6;
00686             break;
00687         case MAVLINK_PARSE_STATE_GOT_SEQ: 
00688             desired = msg->len + 5;
00689             break;
00690         case MAVLINK_PARSE_STATE_GOT_SYSID: 
00691             desired = msg->len + 4;
00692             break;        
00693         case MAVLINK_PARSE_STATE_GOT_COMPID:
00694             desired = msg->len + 3;
00695             break;         
00696         case MAVLINK_PARSE_STATE_GOT_MSGID: 
00697             desired = msg->len - self->mav_status.packet_idx + 2;
00698             break;          
00699         case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
00700             desired = 2;
00701             break;          
00702         case MAVLINK_PARSE_STATE_GOT_CRC1: 
00703             desired = 1;
00704             break;  
00705         default:
00706             // Huh?  Just claim 1
00707             desired = 1;
00708             break;
00709     } 
00710     
00711     mavdebug("in state %d, expected_length=%d\n", (int) self->mav_status.parse_state, desired);
00712     return desired;
00713 }
00714 
00715 
00716 static PyObject *NativeConnection_getexpectedlength(NativeConnection *self, void *closure)
00717 {
00718     return PyInt_FromLong(get_expectedlength(self));
00719 }
00720 
00725 static PyObject *
00726 py_parse_chars(NativeConnection *self, PyObject *args)
00727 {
00728     PYTHON_ENTRY
00729 
00730     PyObject* byteObj;
00731     if (!PyArg_ParseTuple(args, "O", &byteObj)) {
00732         set_pyerror("Invalid arguments");
00733         return NULL;
00734     }
00735     
00736     assert(PyByteArray_Check(byteObj));
00737     Py_ssize_t numBytes = PyByteArray_Size(byteObj);    
00738     mavdebug("numbytes %u\n", (unsigned) numBytes);
00739 
00740     char *start = PyByteArray_AsString(byteObj);
00741     assert(start);
00742     char *bytes = start;
00743     PyObject *result = NULL;
00744 
00745     // Generate a list of messages found 
00746     while(numBytes) {
00747         char c = *bytes++;
00748         numBytes--;
00749         get_expectedlength(self); mavdebug("parse 0x%x\n", (unsigned char) c);
00750 
00751         if (py_mavlink_parse_char(c, &self->msg, &self->mav_status)) {
00752             mavdebug("got packet\n");
00753             result = msg_to_py(self->MAVLinkMessage, &self->msg);
00754             if(result != NULL)
00755                 break;
00756         }
00757     }
00758 
00759     // We didn't process all bytes provided by the caller, so fixup their array
00760     memmove(start, bytes, numBytes);
00761     PyByteArray_Resize(byteObj, numBytes);
00762 
00763     if(result != NULL) 
00764         return result;
00765     else
00766         return createPyNone();
00767 
00768     PYTHON_EXIT
00769 }
00770 
00778 static PyObject *
00779 py_parse_buffer(NativeConnection *self, PyObject *args)
00780 {
00781     PYTHON_ENTRY
00782 
00783     mavdebug("Enter py_parse_buffer\n");
00784 
00785     const char *bytes;
00786     Py_ssize_t numBytes = 0;
00787 
00788     if (!PyArg_ParseTuple(args, "s#", &bytes, &numBytes)) {
00789         set_pyerror("Invalid arguments");
00790         return NULL;
00791     }
00792     
00793     // mavdebug("numbytes %u\n", (unsigned) numBytes);
00794 
00795     PyObject* list = PyList_New(0);
00796 
00797     // Generate a list of messages found 
00798     while(numBytes--) {
00799         char c = *bytes++;
00800         // mavdebug("parse %c\n", c);
00801 
00802         if (py_mavlink_parse_char(c, &self->msg, &self->mav_status)) {
00803             PyObject *obj = msg_to_py(self->MAVLinkMessage, &self->msg);
00804             if(obj != NULL) {
00805                 PyList_Append(list, obj);
00806             
00807                 // Append will have bummped up the ref count on obj, so we need to release our count
00808                 Py_DECREF(obj);
00809             }
00810         }
00811     }
00812 
00813     return list;
00814 
00815     PYTHON_EXIT
00816 }
00817 
00818 static PyObject *
00819 NativeConnection_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
00820 {
00821     NativeConnection *self = (NativeConnection *)type->tp_alloc(type, 0);
00822 
00823     mavdebug("new connection\n");
00824     return (PyObject *)self;
00825 }
00826 
00827 
00828 static int
00829 NativeConnection_init(NativeConnection *self, PyObject *args, PyObject *kwds)
00830 {
00831     PYTHON_ENTRY
00832 
00833     mavdebug("Enter init\n");
00834 
00835     memset(&self->mav_status, 0, sizeof(self->mav_status));
00836 
00837     PyObject* msgclass, *mavlink_map;
00838     if (!PyArg_ParseTuple(args, "OO", &msgclass, &mavlink_map)) {
00839         set_pyerror("Invalid arguments");
00840         return -1;
00841     }
00842 
00843     // keep a ref to our mavlink instance constructor
00844     assert(msgclass);
00845     self->MAVLinkMessage = msgclass;
00846     Py_INCREF(msgclass);
00847 
00848     assert(mavlink_map);
00849     if(!info_inited) {
00850         init_message_info(mavlink_map);
00851         info_inited = TRUE;
00852     }
00853 
00854     mavdebug("inited connection\n");
00855     return 0;
00856 
00857     PYTHON_EXIT_INT
00858 }
00859 
00860 static void NativeConnection_dealloc(NativeConnection* self)
00861 {
00862     Py_XDECREF(self->MAVLinkMessage);
00863     Py_TYPE(self)->tp_free((PyObject*)self);
00864 }
00865 
00866 static PyMemberDef NativeConnection_members[] = {
00867     {NULL}  /* Sentinel */
00868 };
00869 
00870 static PyGetSetDef NativeConnection_getseters[] = {
00871     {"expected_length", 
00872      (getter)NativeConnection_getexpectedlength, NULL,
00873      "How many characters would the state-machine like to read now",
00874      NULL},
00875     {NULL}  /* Sentinel */
00876 };
00877 
00878 static PyMethodDef NativeConnection_methods[] = {
00879     {"parse_chars",  (PyCFunction) py_parse_chars, METH_VARARGS,
00880      "Given a msg class and an array of bytes, Parse chars, returning a message or None"},    
00881     {"parse_buffer",  (PyCFunction) py_parse_buffer, METH_VARARGS,
00882      "Given a msg class and a string like object, Parse chars, returning a (possibly empty) list of messages"},
00883     {NULL,  NULL},
00884 };
00885 
00886 // FIXME - delete this?
00887 static PyTypeObject NativeConnectionType = {
00888 #if PY_MAJOR_VERSION >= 3
00889     PyVarObject_HEAD_INIT(NULL, 0)
00890 #else
00891     PyObject_HEAD_INIT(NULL)
00892     0,                       /* ob_size */
00893 #endif
00894     "mavnative.NativeConnection",      /* tp_name */
00895     sizeof(NativeConnection),          /* tp_basicsize */
00896     0,                       /* tp_itemsize */
00897     (destructor)NativeConnection_dealloc,  /* tp_dealloc */
00898     0,                       /* tp_print */
00899     0,                       /* tp_getattr */
00900     0,                       /* tp_setattr */
00901     0,                       /* tp_compare */
00902     0,                       /* tp_repr */
00903     0,                       /* tp_as_number */
00904     0,                       /* tp_as_sequence */
00905     0,                       /* tp_as_mapping */
00906     0,                       /* tp_hash */
00907     0,                       /* tp_call */
00908     0,                       /* tp_str */
00909     0,                       /* tp_getattro */
00910     0,                       /* tp_setattro */
00911     0,                       /* tp_as_buffer */
00912     Py_TPFLAGS_DEFAULT |
00913       Py_TPFLAGS_BASETYPE,   /* tp_flags */
00914     "NativeConnection objects",  /* tp_doc */
00915     0,                       /* tp_traverse */
00916     0,                       /* tp_clear */
00917     0,                       /* tp_richcompare */
00918     0,                       /* tp_weaklistoffset */
00919     0,                       /* tp_iter */
00920     0,                       /* tp_iternext */
00921     NativeConnection_methods,  /* tp_methods */
00922     NativeConnection_members,  /* tp_members */
00923     NativeConnection_getseters,  /* tp_getset */
00924     0,                       /* tp_base */
00925     0,                       /* tp_dict */
00926     0,                       /* tp_descr_get */
00927     0,                       /* tp_descr_set */
00928     0,                       /* tp_dictoffset */
00929     (initproc)NativeConnection_init,   /* tp_init */
00930     0,                       /* tp_alloc */
00931     NativeConnection_new,    /* tp_new */
00932 };
00933 
00934 #if PY_MAJOR_VERSION >= 3
00935 #define MOD_RETURN(m) return m
00936 #else
00937 #define MOD_RETURN(m) return
00938 #endif
00939 
00940 PyMODINIT_FUNC
00941 #if PY_MAJOR_VERSION >= 3
00942     PyInit_mavnative(void)
00943 #else
00944     initmavnative(void)
00945 #endif
00946 {
00947     if (PyType_Ready(&NativeConnectionType) < 0)
00948         MOD_RETURN(NULL);
00949 
00950 #if PY_MAJOR_VERSION < 3
00951     static PyMethodDef ModuleMethods[] = {
00952         {NULL, NULL, 0, NULL}        /* Sentinel */
00953     };
00954 
00955     PyObject *m = Py_InitModule3("mavnative", ModuleMethods, "Mavnative module");
00956     if (m == NULL)
00957         MOD_RETURN(m);
00958 #else
00959     static PyModuleDef mod_def = {
00960         PyModuleDef_HEAD_INIT,
00961         "mavnative",
00962         "EMavnative module",
00963         -1,
00964         NULL, NULL, NULL, NULL, NULL
00965     };
00966 
00967     PyObject *m = PyModule_Create(&mod_def);
00968 #endif
00969 
00970     MAVNativeError = PyErr_NewException("mavnative.error", NULL, NULL);
00971     Py_INCREF(MAVNativeError);
00972     PyModule_AddObject(m, "error", MAVNativeError);
00973 
00974     Py_INCREF(&NativeConnectionType);
00975     PyModule_AddObject(m, "NativeConnection", (PyObject *) &NativeConnectionType);
00976     MOD_RETURN(m);
00977 }


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57