123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980 |
- #undef NDEBUG
- #include <Python.h>
- #include <structmember.h>
- #include <stdio.h>
- #include <stdlib.h>
- #include <stdint.h>
- #include <assert.h>
- #include <stddef.h>
- #include <setjmp.h>
- #if PY_MAJOR_VERSION >= 3
- #define PyInt_AsLong PyLong_AsLong
- #define PyInt_FromLong PyLong_FromLong
- #define PyByteString_FromString PyBytes_FromString
- #define PyByteString_FromStringAndSize PyBytes_FromStringAndSize
- #define PyByteString_ConcatAndDel PyBytes_ConcatAndDel
- #else
- #define PyByteString_FromString PyString_FromString
- #define PyByteString_FromStringAndSize PyString_FromStringAndSize
- #define PyByteString_ConcatAndDel PyString_ConcatAndDel
- #endif
- #include "mavlink_defaults.h"
- #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
- #include <mavlink_types.h>
- static mavlink_system_t mavlink_system = {42,11,};
- static void comm_send_ch(mavlink_channel_t chan, uint8_t c) {
-
- assert(0);
- }
- #define MAVLINK_ASSERT(x) assert(x)
- #define MAVLINK_MESSAGE_CRC(msgid) py_message_info[msgid].crc_extra
- #define MAVLINK_MESSAGE_LENGTH(msgid) py_message_info[msgid].len
- #define TRUE 1
- #define FALSE 0
- typedef struct {
- PyObject *name;
- mavlink_message_type_t type;
- unsigned int array_length;
- unsigned int wire_offset;
- } py_field_info_t;
- typedef struct {
- PyObject *id;
- PyObject *name;
- unsigned len;
- uint8_t crc_extra;
- unsigned num_fields;
- PyObject *fieldnames;
- py_field_info_t fields[MAVLINK_MAX_FIELDS];
- } py_message_info_t;
- static py_message_info_t py_message_info[256];
- static uint8_t info_inited = FALSE;
- #include <protocol.h>
- typedef struct {
- mavlink_message_t msg;
- int numBytes;
- uint8_t bytes[MAVLINK_MAX_PACKET_LEN];
- } py_message_t;
- typedef struct {
- PyObject_HEAD
- PyObject *MAVLinkMessage;
- mavlink_status_t mav_status;
- py_message_t msg;
- } NativeConnection;
- #ifdef MAVNATIVE_DEBUG
- # define mavdebug printf
- #else
- # define mavdebug(x...)
- #endif
- static PyObject *MAVNativeError;
- static jmp_buf python_entry;
- #define PYTHON_ENTRY if(!setjmp(python_entry)) {
- #define PYTHON_EXIT } else { return NULL; }
- #define PYTHON_EXIT_INT } else { return -1; }
- MAVLINK_HELPER uint8_t py_mavlink_parse_char(uint8_t c, py_message_t* pymsg, mavlink_status_t* status)
- {
- mavlink_message_t *rxmsg = &pymsg->msg;
- int bufferIndex = 0;
- status->msg_received = 0;
- switch (status->parse_state)
- {
- case MAVLINK_PARSE_STATE_UNINIT:
- case MAVLINK_PARSE_STATE_IDLE:
- if (c == MAVLINK_STX)
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
- rxmsg->len = 0;
- pymsg->numBytes = 0;
- rxmsg->magic = c;
- mavlink_start_checksum(rxmsg);
- pymsg->bytes[pymsg->numBytes++] = c;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_STX:
- if (status->msg_received
- #if (MAVLINK_MAX_PAYLOAD_LEN < 255)
- || c > MAVLINK_MAX_PAYLOAD_LEN
- #endif
- )
- {
- status->buffer_overrun++;
- status->parse_error++;
- status->msg_received = 0;
- status->parse_state = MAVLINK_PARSE_STATE_IDLE;
- }
- else
- {
-
- rxmsg->len = c;
- status->packet_idx = 0;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_LENGTH:
- rxmsg->seq = c;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
- break;
- case MAVLINK_PARSE_STATE_GOT_SEQ:
- rxmsg->sysid = c;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
- break;
- case MAVLINK_PARSE_STATE_GOT_SYSID:
- rxmsg->compid = c;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
- break;
- case MAVLINK_PARSE_STATE_GOT_COMPID:
- #ifdef MAVLINK_CHECK_MESSAGE_LENGTH
- if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
- {
- status->parse_error++;
- status->parse_state = MAVLINK_PARSE_STATE_IDLE;
- break;
- }
- #endif
- rxmsg->msgid = c;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- if (rxmsg->len == 0)
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
- }
- else
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_MSGID:
- _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
- mavlink_update_checksum(rxmsg, c);
- pymsg->bytes[pymsg->numBytes++] = c;
- if (status->packet_idx == rxmsg->len)
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
- #if MAVLINK_CRC_EXTRA
- mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
- #endif
- pymsg->bytes[pymsg->numBytes++] = c;
- if (c != (rxmsg->checksum & 0xFF)) {
-
- status->parse_error++;
- status->msg_received = 0;
- status->parse_state = MAVLINK_PARSE_STATE_IDLE;
- if (c == MAVLINK_STX)
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
- rxmsg->len = 0;
- mavlink_start_checksum(rxmsg);
- }
- }
- else
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
- _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_CRC1:
- pymsg->bytes[pymsg->numBytes++] = c;
- if (c != (rxmsg->checksum >> 8)) {
-
- status->parse_error++;
- status->msg_received = 0;
- status->parse_state = MAVLINK_PARSE_STATE_IDLE;
- if (c == MAVLINK_STX)
- {
- status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
- rxmsg->len = 0;
- mavlink_start_checksum(rxmsg);
- }
- }
- else
- {
-
- status->msg_received = 1;
- status->parse_state = MAVLINK_PARSE_STATE_IDLE;
- _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
- }
- break;
- case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
- break;
- }
- bufferIndex++;
-
- if (status->msg_received == 1)
- {
-
-
-
-
-
- status->current_rx_seq = rxmsg->seq;
-
- if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
-
- status->packet_rx_success_count++;
- }
- return status->msg_received;
- }
- static void set_pyerror(const char *msg) {
- PyErr_SetString(MAVNativeError, msg);
- }
- extern void __assert_fail(const char *__assertion, const char *__file, unsigned int __line, const char *__function)
- {
- char msg[256];
- sprintf(msg, "Assertion failed: %s, %s:%d", __assertion, __file, __line);
- set_pyerror(msg);
- longjmp(python_entry, 1);
- }
- static unsigned get_field_size(int field_type) {
- unsigned fieldSize;
- switch(field_type)
- {
- case MAVLINK_TYPE_CHAR:
- fieldSize = 1;
- break;
- case MAVLINK_TYPE_UINT8_T:
- fieldSize = 1;
- break;
- case MAVLINK_TYPE_INT8_T:
- fieldSize = 1;
- break;
- case MAVLINK_TYPE_UINT16_T:
- fieldSize = 2;
- break;
- case MAVLINK_TYPE_INT16_T:
- fieldSize = 2;
- break;
- case MAVLINK_TYPE_UINT32_T:
- fieldSize = 4;
- break;
- case MAVLINK_TYPE_INT32_T:
- fieldSize = 4;
- break;
- case MAVLINK_TYPE_UINT64_T:
- fieldSize = 8;
- break;
- case MAVLINK_TYPE_INT64_T:
- fieldSize = 8;
- break;
- case MAVLINK_TYPE_FLOAT:
- fieldSize = 4;
- break;
- case MAVLINK_TYPE_DOUBLE:
- fieldSize = 8;
- break;
- default:
- mavdebug("BAD MAV TYPE %d\n", field_type);
- set_pyerror("Unexpected mavlink type");
- fieldSize = 1;
- }
- return fieldSize;
- }
- static int get_py_typeinfo(char type_char, int array_size, unsigned *wire_offset)
- {
- int type_code;
- switch(type_char)
- {
- case 'f': type_code = MAVLINK_TYPE_FLOAT; break;
- case 'd': type_code = MAVLINK_TYPE_DOUBLE; break;
- case 'c': type_code = MAVLINK_TYPE_CHAR; break;
- case 'v': type_code = MAVLINK_TYPE_UINT8_T; break;
- case 'b': type_code = MAVLINK_TYPE_INT8_T; break;
- case 'B': type_code = MAVLINK_TYPE_UINT8_T; break;
- case 'h': type_code = MAVLINK_TYPE_INT16_T; break;
- case 'H': type_code = MAVLINK_TYPE_UINT16_T; break;
- case 'i': type_code = MAVLINK_TYPE_INT32_T; break;
- case 'I': type_code = MAVLINK_TYPE_UINT32_T; break;
- case 'q': type_code = MAVLINK_TYPE_INT64_T; break;
- case 'Q': type_code = MAVLINK_TYPE_UINT64_T; break;
- default:
- assert(0);
- }
- int total_len = get_field_size(type_code) * (array_size == 0 ? 1 : array_size);
- *wire_offset += total_len;
- return type_code;
- }
- static void init_message_info(PyObject *mavlink_map) {
-
-
- PyObject *items_list = PyDict_Values(mavlink_map);
- assert(items_list);
- Py_ssize_t numMsgs = PyList_Size(items_list);
- int i;
- for(i = 0; i < numMsgs; i++) {
- PyObject *type_class = PyList_GetItem(items_list, i);
- assert(type_class);
- PyObject *id_obj = PyObject_GetAttrString(type_class, "id");
- assert(id_obj);
- PyObject *name_obj = PyObject_GetAttrString(type_class, "name");
- assert(name_obj);
- PyObject *crc_extra_obj = PyObject_GetAttrString(type_class, "crc_extra");
- assert(crc_extra_obj);
- PyObject *fieldname_list = PyObject_GetAttrString(type_class, "ordered_fieldnames");
- assert(fieldname_list);
-
-
- PyObject *arrlen_list = PyObject_GetAttrString(type_class, "array_lengths");
- assert(arrlen_list);
- PyObject *type_format = PyObject_GetAttrString(type_class, "native_format");
- assert(type_format);
- char *type_str = PyByteArray_AsString(type_format);
- assert(type_str);
-
- Py_ssize_t num_fields = PyList_Size(fieldname_list);
- uint8_t id = (uint8_t) PyInt_AsLong(id_obj);
- py_message_info_t *d = &py_message_info[id];
- d->id = id_obj;
- d->name = name_obj;
- d->num_fields = num_fields;
- d->crc_extra = PyInt_AsLong(crc_extra_obj);
- d->fieldnames = PyObject_GetAttrString(type_class, "fieldnames");
- assert(d->fieldnames);
- int fnum;
- unsigned wire_offset = 0;
- for(fnum = 0; fnum < num_fields; fnum++) {
- PyObject *field_name_obj = PyList_GetItem(fieldname_list, fnum);
- assert(field_name_obj);
- Py_INCREF(field_name_obj);
- PyObject *len_obj = PyList_GetItem(arrlen_list, fnum);
- assert(len_obj);
- d->fields[fnum].name = field_name_obj;
- d->fields[fnum].array_length = PyInt_AsLong(len_obj);
- char type_char = type_str[1 + fnum];
- d->fields[fnum].wire_offset = wire_offset;
- d->fields[fnum].type = get_py_typeinfo(type_char, d->fields[fnum].array_length, &wire_offset);
- }
- d->len = wire_offset;
- Py_DECREF(crc_extra_obj);
- Py_DECREF(arrlen_list);
- Py_DECREF(type_format);
-
- }
- Py_DECREF(items_list);
- }
- static PyObject *createPyNone(void)
- {
- Py_INCREF(Py_None);
- return Py_None;
- }
- static void set_attribute(PyObject *obj, const char *attrName, PyObject *val) {
- assert(val);
- PyObject_SetAttrString(obj, attrName, val);
- Py_DECREF(val);
- }
- static PyObject *pyextract_mavlink(const mavlink_message_t *msg, const py_field_info_t *field) {
- unsigned offset = field->wire_offset;
- int index;
-
- PyObject *arrayResult = (field->array_length != 0 && field->type != MAVLINK_TYPE_CHAR) ? PyList_New(field->array_length) : NULL;
- PyObject *stringResult = (field->array_length != 0 && field->type == MAVLINK_TYPE_CHAR) ? PyByteString_FromString("") : NULL;
- PyObject *result = NULL;
- int numValues = (field->array_length == 0) ? 1 : field->array_length;
- unsigned fieldSize = get_field_size(field->type);
- uint8_t string_ended = FALSE;
- if(arrayResult != NULL)
- result = arrayResult;
-
- for(index = 0; index < numValues; index++) {
- PyObject *val = NULL;
- switch(field->type) {
- case MAVLINK_TYPE_CHAR: {
-
- char c = _MAV_RETURN_char(msg, offset);
- if(stringResult && c == 0)
- string_ended = TRUE;
- val = PyByteString_FromStringAndSize(&c, 1);
- break;
- }
- case MAVLINK_TYPE_UINT8_T:
- val = PyInt_FromLong(_MAV_RETURN_uint8_t(msg, offset));
- break;
- case MAVLINK_TYPE_INT8_T:
- val = PyInt_FromLong(_MAV_RETURN_int8_t(msg, offset));
- break;
- case MAVLINK_TYPE_UINT16_T:
- val = PyInt_FromLong(_MAV_RETURN_uint16_t(msg, offset));
- break;
- case MAVLINK_TYPE_INT16_T:
- val = PyInt_FromLong(_MAV_RETURN_int16_t(msg, offset));
- break;
- case MAVLINK_TYPE_UINT32_T:
- val = PyLong_FromLong(_MAV_RETURN_uint32_t(msg, offset));
- break;
- case MAVLINK_TYPE_INT32_T:
- val = PyInt_FromLong(_MAV_RETURN_int32_t(msg, offset));
- break;
- case MAVLINK_TYPE_UINT64_T:
- val = PyLong_FromLongLong(_MAV_RETURN_uint64_t(msg, offset));
- break;
- case MAVLINK_TYPE_INT64_T:
- val = PyLong_FromLongLong(_MAV_RETURN_int64_t(msg, offset));
- break;
- case MAVLINK_TYPE_FLOAT:
- val = PyFloat_FromDouble(_MAV_RETURN_float(msg, offset));
- break;
- case MAVLINK_TYPE_DOUBLE:
- val = PyFloat_FromDouble(_MAV_RETURN_double(msg, offset));
- break;
- default:
- mavdebug("BAD MAV TYPE %d\n", field->type);
- set_pyerror("Unexpected mavlink type");
- return NULL;
- }
- offset += fieldSize;
- assert(val);
- if(arrayResult != NULL)
- PyList_SetItem(arrayResult, index, val);
- else if(stringResult != NULL) {
- if(!string_ended)
- PyByteString_ConcatAndDel(&stringResult, val);
- else
- Py_DECREF(val);
- result = stringResult;
- }
- else
- result = val;
- }
- assert(result);
- return result;
- }
- static PyObject *msg_to_py(PyObject* msgclass, const py_message_t *pymsg) {
- const mavlink_message_t *msg = &pymsg->msg;
- const py_message_info_t *info = &py_message_info[msg->msgid];
- mavdebug("Found a msg: %s\n", PyString_AS_STRING(info->name));
-
-
- PyObject *argList = PyTuple_Pack(2, info->id, info->name);
- PyObject *obj = PyObject_CallObject(msgclass, argList);
- uint8_t objValid = TRUE;
- assert(obj);
- Py_DECREF(argList);
-
- PyObject *header = PyObject_GetAttrString(obj, "_header");
- assert(header);
-
- set_attribute(header, "mlen", PyInt_FromLong(msg->len));
- set_attribute(header, "seq", PyInt_FromLong(msg->seq));
- set_attribute(header, "srcSystem", PyInt_FromLong(msg->sysid));
- set_attribute(header, "srcComponent", PyInt_FromLong(msg->compid));
- Py_DECREF(header);
- header = NULL;
-
- set_attribute(obj, "_msgbuf", PyByteArray_FromStringAndSize((const char *) pymsg->bytes, pymsg->numBytes));
-
- PyObject_SetAttrString(obj, "_fieldnames", info->fieldnames);
-
- int fnum;
- for(fnum = 0; fnum < info->num_fields && objValid; fnum++) {
- const py_field_info_t *f = &info->fields[fnum];
- PyObject *val = pyextract_mavlink(msg, f);
- if(val != NULL) {
- PyObject_SetAttr(obj, f->name, val);
- Py_DECREF(val);
- }
- else
- objValid = FALSE;
- }
- if(objValid)
- return obj;
- else {
- Py_DECREF(obj);
- return NULL;
- }
- }
- static int get_expectedlength(NativeConnection *self)
- {
- int desired;
- mavlink_message_t *msg = &self->msg.msg;
- switch(self->mav_status.parse_state) {
- case MAVLINK_PARSE_STATE_UNINIT:
- case MAVLINK_PARSE_STATE_IDLE:
- desired = 8;
- break;
- case MAVLINK_PARSE_STATE_GOT_STX:
- desired = 7;
- break;
- case MAVLINK_PARSE_STATE_GOT_LENGTH:
- desired = msg->len + 6;
- break;
- case MAVLINK_PARSE_STATE_GOT_SEQ:
- desired = msg->len + 5;
- break;
- case MAVLINK_PARSE_STATE_GOT_SYSID:
- desired = msg->len + 4;
- break;
- case MAVLINK_PARSE_STATE_GOT_COMPID:
- desired = msg->len + 3;
- break;
- case MAVLINK_PARSE_STATE_GOT_MSGID:
- desired = msg->len - self->mav_status.packet_idx + 2;
- break;
- case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
- desired = 2;
- break;
- case MAVLINK_PARSE_STATE_GOT_CRC1:
- desired = 1;
- break;
- default:
-
- desired = 1;
- break;
- }
-
- mavdebug("in state %d, expected_length=%d\n", (int) self->mav_status.parse_state, desired);
- return desired;
- }
- static PyObject *NativeConnection_getexpectedlength(NativeConnection *self, void *closure)
- {
- return PyInt_FromLong(get_expectedlength(self));
- }
- static PyObject *
- py_parse_chars(NativeConnection *self, PyObject *args)
- {
- PYTHON_ENTRY
- PyObject* byteObj;
- if (!PyArg_ParseTuple(args, "O", &byteObj)) {
- set_pyerror("Invalid arguments");
- return NULL;
- }
-
- assert(PyByteArray_Check(byteObj));
- Py_ssize_t numBytes = PyByteArray_Size(byteObj);
- mavdebug("numbytes %u\n", (unsigned) numBytes);
- char *start = PyByteArray_AsString(byteObj);
- assert(start);
- char *bytes = start;
- PyObject *result = NULL;
-
- while(numBytes) {
- char c = *bytes++;
- numBytes--;
- get_expectedlength(self); mavdebug("parse 0x%x\n", (unsigned char) c);
- if (py_mavlink_parse_char(c, &self->msg, &self->mav_status)) {
- mavdebug("got packet\n");
- result = msg_to_py(self->MAVLinkMessage, &self->msg);
- if(result != NULL)
- break;
- }
- }
-
- memmove(start, bytes, numBytes);
- PyByteArray_Resize(byteObj, numBytes);
- if(result != NULL)
- return result;
- else
- return createPyNone();
- PYTHON_EXIT
- }
- static PyObject *
- py_parse_buffer(NativeConnection *self, PyObject *args)
- {
- PYTHON_ENTRY
- mavdebug("Enter py_parse_buffer\n");
- const char *bytes;
- Py_ssize_t numBytes = 0;
- if (!PyArg_ParseTuple(args, "s#", &bytes, &numBytes)) {
- set_pyerror("Invalid arguments");
- return NULL;
- }
-
-
- PyObject* list = PyList_New(0);
-
- while(numBytes--) {
- char c = *bytes++;
-
- if (py_mavlink_parse_char(c, &self->msg, &self->mav_status)) {
- PyObject *obj = msg_to_py(self->MAVLinkMessage, &self->msg);
- if(obj != NULL) {
- PyList_Append(list, obj);
-
-
- Py_DECREF(obj);
- }
- }
- }
- return list;
- PYTHON_EXIT
- }
- static PyObject *
- NativeConnection_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
- {
- NativeConnection *self = (NativeConnection *)type->tp_alloc(type, 0);
- mavdebug("new connection\n");
- return (PyObject *)self;
- }
- static int
- NativeConnection_init(NativeConnection *self, PyObject *args, PyObject *kwds)
- {
- PYTHON_ENTRY
- mavdebug("Enter init\n");
- memset(&self->mav_status, 0, sizeof(self->mav_status));
- PyObject* msgclass, *mavlink_map;
- if (!PyArg_ParseTuple(args, "OO", &msgclass, &mavlink_map)) {
- set_pyerror("Invalid arguments");
- return -1;
- }
-
- assert(msgclass);
- self->MAVLinkMessage = msgclass;
- Py_INCREF(msgclass);
- assert(mavlink_map);
- if(!info_inited) {
- init_message_info(mavlink_map);
- info_inited = TRUE;
- }
- mavdebug("inited connection\n");
- return 0;
- PYTHON_EXIT_INT
- }
- static void NativeConnection_dealloc(NativeConnection* self)
- {
- Py_XDECREF(self->MAVLinkMessage);
- Py_TYPE(self)->tp_free((PyObject*)self);
- }
- static PyMemberDef NativeConnection_members[] = {
- {NULL}
- };
- static PyGetSetDef NativeConnection_getseters[] = {
- {"expected_length",
- (getter)NativeConnection_getexpectedlength, NULL,
- "How many characters would the state-machine like to read now",
- NULL},
- {NULL}
- };
- static PyMethodDef NativeConnection_methods[] = {
- {"parse_chars", (PyCFunction) py_parse_chars, METH_VARARGS,
- "Given a msg class and an array of bytes, Parse chars, returning a message or None"},
- {"parse_buffer", (PyCFunction) py_parse_buffer, METH_VARARGS,
- "Given a msg class and a string like object, Parse chars, returning a (possibly empty) list of messages"},
- {NULL, NULL},
- };
- static PyTypeObject NativeConnectionType = {
- #if PY_MAJOR_VERSION >= 3
- PyVarObject_HEAD_INIT(NULL, 0)
- #else
- PyObject_HEAD_INIT(NULL)
- 0,
- #endif
- "mavnative.NativeConnection",
- sizeof(NativeConnection),
- 0,
- (destructor)NativeConnection_dealloc,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- Py_TPFLAGS_DEFAULT |
- Py_TPFLAGS_BASETYPE,
- "NativeConnection objects",
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- NativeConnection_methods,
- NativeConnection_members,
- NativeConnection_getseters,
- 0,
- 0,
- 0,
- 0,
- 0,
- (initproc)NativeConnection_init,
- 0,
- NativeConnection_new,
- };
- #if PY_MAJOR_VERSION >= 3
- #define MOD_RETURN(m) return m
- #else
- #define MOD_RETURN(m) return
- #endif
- PyMODINIT_FUNC
- #if PY_MAJOR_VERSION >= 3
- PyInit_mavnative(void)
- #else
- initmavnative(void)
- #endif
- {
- if (PyType_Ready(&NativeConnectionType) < 0)
- MOD_RETURN(NULL);
- #if PY_MAJOR_VERSION < 3
- static PyMethodDef ModuleMethods[] = {
- {NULL, NULL, 0, NULL}
- };
- PyObject *m = Py_InitModule3("mavnative", ModuleMethods, "Mavnative module");
- if (m == NULL)
- MOD_RETURN(m);
- #else
- static PyModuleDef mod_def = {
- PyModuleDef_HEAD_INIT,
- "mavnative",
- "EMavnative module",
- -1,
- NULL, NULL, NULL, NULL, NULL
- };
- PyObject *m = PyModule_Create(&mod_def);
- #endif
- MAVNativeError = PyErr_NewException("mavnative.error", NULL, NULL);
- Py_INCREF(MAVNativeError);
- PyModule_AddObject(m, "error", MAVNativeError);
- Py_INCREF(&NativeConnectionType);
- PyModule_AddObject(m, "NativeConnection", (PyObject *) &NativeConnectionType);
- MOD_RETURN(m);
- }
|