static PyObject* _cbson_do_batched_write_command(PyObject* self, PyObject* args) { struct module_state *state = GETSTATE(self); long max_bson_size; long max_cmd_size; long max_write_batch_size; long idx_offset = 0; int idx = 0; int cmd_len_loc; int lst_len_loc; int ns_len; int ordered; char *ns = NULL; PyObject* max_bson_size_obj; PyObject* max_write_batch_size_obj; PyObject* command; PyObject* doc; PyObject* docs; PyObject* ctx; PyObject* iterator; PyObject* result; PyObject* results; PyObject* to_publish = NULL; unsigned char op; unsigned char check_keys; codec_options_t options; unsigned char empty = 1; unsigned char errors = 0; buffer_t buffer; if (!PyArg_ParseTuple(args, "et#bOObO&O", "utf-8", &ns, &ns_len, &op, &command, &docs, &check_keys, convert_codec_options, &options, &ctx)) { return NULL; } max_bson_size_obj = PyObject_GetAttrString(ctx, "max_bson_size"); #if PY_MAJOR_VERSION >= 3 max_bson_size = PyLong_AsLong(max_bson_size_obj); #else max_bson_size = PyInt_AsLong(max_bson_size_obj); #endif Py_XDECREF(max_bson_size_obj); if (max_bson_size == -1) { destroy_codec_options(&options); PyMem_Free(ns); return NULL; } /* * Max BSON object size + 16k - 2 bytes for ending NUL bytes * XXX: This should come from the server - SERVER-10643 */ max_cmd_size = max_bson_size + 16382; max_write_batch_size_obj = PyObject_GetAttrString(ctx, "max_write_batch_size"); #if PY_MAJOR_VERSION >= 3 max_write_batch_size = PyLong_AsLong(max_write_batch_size_obj); #else max_write_batch_size = PyInt_AsLong(max_write_batch_size_obj); #endif Py_XDECREF(max_write_batch_size_obj); if (max_write_batch_size == -1) { destroy_codec_options(&options); PyMem_Free(ns); return NULL; } /* Default to True */ ordered = !((PyDict_GetItemString(command, "ordered")) == Py_False); if (!(results = PyList_New(0))) { destroy_codec_options(&options); PyMem_Free(ns); return NULL; } if (!(to_publish = PyList_New(0))) { destroy_codec_options(&options); PyMem_Free(ns); Py_DECREF(results); return NULL; } if (!(buffer = _command_buffer_new(ns, ns_len))) { destroy_codec_options(&options); PyMem_Free(ns); Py_DECREF(results); Py_DECREF(to_publish); return NULL; } PyMem_Free(ns); /* Position of command document length */ cmd_len_loc = buffer_get_position(buffer); if (!write_dict(state->_cbson, buffer, command, 0, &options, 0)) { goto cmdfail; } /* Write type byte for array */ *(buffer_get_buffer(buffer) + (buffer_get_position(buffer) - 1)) = 0x4; switch (op) { case _INSERT: { if (!buffer_write_bytes(buffer, "documents\x00", 10)) goto cmdfail; break; } case _UPDATE: { /* MongoDB does key validation for update. */ check_keys = 0; if (!buffer_write_bytes(buffer, "updates\x00", 8)) goto cmdfail; break; } case _DELETE: { /* Never check keys in a delete command. */ check_keys = 0; if (!buffer_write_bytes(buffer, "deletes\x00", 8)) goto cmdfail; break; } default: { PyObject* InvalidOperation = _error("InvalidOperation"); if (InvalidOperation) { PyErr_SetString(InvalidOperation, "Unknown command"); Py_DECREF(InvalidOperation); } goto cmdfail; } } /* Save space for list document */ lst_len_loc = buffer_save_space(buffer, 4); if (lst_len_loc == -1) { PyErr_NoMemory(); goto cmdfail; } iterator = PyObject_GetIter(docs); if (iterator == NULL) { PyObject* InvalidOperation = _error("InvalidOperation"); if (InvalidOperation) { PyErr_SetString(InvalidOperation, "input is not iterable"); Py_DECREF(InvalidOperation); } goto cmdfail; } while ((doc = PyIter_Next(iterator)) != NULL) { int sub_doc_begin = buffer_get_position(buffer); int cur_doc_begin; int cur_size; int enough_data = 0; int enough_documents = 0; char key[16]; empty = 0; INT2STRING(key, idx); if (!buffer_write_bytes(buffer, "\x03", 1) || !buffer_write_bytes(buffer, key, (int)strlen(key) + 1)) { goto cmditerfail; } cur_doc_begin = buffer_get_position(buffer); if (!write_dict(state->_cbson, buffer, doc, check_keys, &options, 1)) { goto cmditerfail; } /* We have enough data, maybe send this batch. */ enough_data = (buffer_get_position(buffer) > max_cmd_size); enough_documents = (idx >= max_write_batch_size); if (enough_data || enough_documents) { buffer_t new_buffer; cur_size = buffer_get_position(buffer) - cur_doc_begin; /* This single document is too large for the command. */ if (!idx) { if (op == _INSERT) { _set_document_too_large(cur_size, max_bson_size); } else { PyObject* DocumentTooLarge = _error("DocumentTooLarge"); if (DocumentTooLarge) { /* * There's nothing intelligent we can say * about size for update and remove. */ PyErr_SetString(DocumentTooLarge, "command document too large"); Py_DECREF(DocumentTooLarge); } } goto cmditerfail; } if (!(new_buffer = buffer_new())) { PyErr_NoMemory(); goto cmditerfail; } /* New buffer including the current overflow document */ if (!buffer_write_bytes(new_buffer, (const char*)buffer_get_buffer(buffer), lst_len_loc + 5) || !buffer_write_bytes(new_buffer, "0\x00", 2) || !buffer_write_bytes(new_buffer, (const char*)buffer_get_buffer(buffer) + cur_doc_begin, cur_size)) { buffer_free(new_buffer); goto cmditerfail; } /* * Roll the existing buffer back to the beginning * of the last document encoded. */ buffer_update_position(buffer, sub_doc_begin); if (!buffer_write_bytes(buffer, "\x00\x00", 2)) { buffer_free(new_buffer); goto cmditerfail; } result = _send_write_command(ctx, buffer, lst_len_loc, cmd_len_loc, &errors, to_publish); buffer_free(buffer); buffer = new_buffer; if (!result) goto cmditerfail; #if PY_MAJOR_VERSION >= 3 result = Py_BuildValue("NN", PyLong_FromLong(idx_offset), result); #else result = Py_BuildValue("NN", PyInt_FromLong(idx_offset), result); #endif if (!result) goto cmditerfail; if (PyList_Append(results, result) < 0) { Py_DECREF(result); goto cmditerfail; } Py_DECREF(result); if (errors && ordered) { destroy_codec_options(&options); Py_DECREF(iterator); buffer_free(buffer); return results; } idx_offset += idx; idx = 0; Py_DECREF(to_publish); if (!(to_publish = PyList_New(0))) { goto cmditerfail; } } if (PyList_Append(to_publish, doc) < 0) { goto cmditerfail; } Py_CLEAR(doc); idx += 1; } Py_DECREF(iterator); if (PyErr_Occurred()) { goto cmdfail; } if (empty) { PyObject* InvalidOperation = _error("InvalidOperation"); if (InvalidOperation) { PyErr_SetString(InvalidOperation, "cannot do an empty bulk write"); Py_DECREF(InvalidOperation); } goto cmdfail; } if (!buffer_write_bytes(buffer, "\x00\x00", 2)) goto cmdfail; result = _send_write_command(ctx, buffer, lst_len_loc, cmd_len_loc, &errors, to_publish); if (!result) goto cmdfail; #if PY_MAJOR_VERSION >= 3 result = Py_BuildValue("NN", PyLong_FromLong(idx_offset), result); #else result = Py_BuildValue("NN", PyInt_FromLong(idx_offset), result); #endif if (!result) goto cmdfail; buffer_free(buffer); if (PyList_Append(results, result) < 0) { Py_DECREF(result); goto cmdfail; } Py_DECREF(result); Py_DECREF(to_publish); destroy_codec_options(&options); return results; cmditerfail: Py_XDECREF(doc); Py_DECREF(iterator); cmdfail: destroy_codec_options(&options); Py_DECREF(results); Py_XDECREF(to_publish); buffer_free(buffer); return NULL; }
int InterfaceElem1d :: computeLocalCoordinates(FloatArray &answer, const FloatArray &gcoords) { _error("Not implemented"); return 0; }
// Process bytes available from the stream // // The stream is assumed to contain only our custom message. If it // contains other messages, and those messages contain the preamble bytes, // it is possible for this code to become de-synchronised. Without // buffering the entire message and re-processing it from the top, // this is unavoidable. // // The lack of a standard header length field makes it impossible to skip // unrecognised messages. // bool AP_GPS_MTK::read(void) { uint8_t data; int numc; bool parsed = false; numc = _port->available(); for (int i = 0; i < numc; i++){ // Process bytes received // read the next byte data = _port->read(); restart: switch(_step){ // Message preamble, class, ID detection // // If we fail to match any of the expected bytes, we // reset the state machine and re-consider the failed // byte as the first byte of the preamble. This // improves our chances of recovering from a mismatch // and makes it less likely that we will be fooled by // the preamble appearing as data in some other message. // case 0: if(PREAMBLE1 == data) _step++; break; case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; goto restart; case 2: if (MESSAGE_CLASS == data) { _step++; _ck_b = _ck_a = data; // reset the checksum accumulators } else { _step = 0; // reset and wait for a message of the right class goto restart; } break; case 3: if (MESSAGE_ID == data) { _step++; _ck_b += (_ck_a += data); _payload_counter = 0; } else { _step = 0; goto restart; } break; // Receive message data // case 4: _buffer.bytes[_payload_counter++] = data; _ck_b += (_ck_a += data); if (_payload_counter == sizeof(_buffer)) _step++; break; // Checksum and message processing // case 5: _step++; if (_ck_a != data) { _error("GPS_MTK: checksum error\n"); _step = 0; } break; case 6: _step = 0; if (_ck_b != data) { _error("GPS_MTK: checksum error\n"); break; } fix = (_buffer.msg.fix_type == FIX_3D); latitude = _swapl(&_buffer.msg.latitude) * 10; longitude = _swapl(&_buffer.msg.longitude) * 10; altitude = _swapl(&_buffer.msg.altitude); ground_speed = _swapl(&_buffer.msg.ground_speed); ground_course = _swapl(&_buffer.msg.ground_course) / 10000; num_sats = _buffer.msg.satellites; // XXX docs say this is UTC, but our clients expect msToW time = _swapl(&_buffer.msg.utc_time); parsed = true; } } return parsed; }
/** * @details Initializes both on-chip and off-chip CAN and sets up filters and masks. */ void Init_CAN(void) { // ------------------------------------------------ // MCP2515 Init MCP2515_Init(MCP_CS_GPIO, MCP_CS_PIN, MCP_INT_GPIO, MCP_INT_PIN); uint8_t i = MCP2515_SetBitRate(MCP_BAUD_KHZ, MCP_INP_CLK_MHZ, MCP_SJW); itoa(i, str, 10); if (i) { DEBUG_Print("Baud Error: "); DEBUG_Print(str); DEBUG_Print("\r\n"); _error(ERROR_CAN_BUS, true, true); } Brusa_MakeCTL(&brusa_control, &mcp_msg_obj); MCP2515_LoadBuffer(0, &mcp_msg_obj); MCP2515_BitModify(RXB0CTRL, RXM_MASK, RXM_STD); //Turn RXB0 Mask MCP2515_BitModify(RXB1CTRL, RXM_MASK, RXM_STD); MCP2515_BitModify(RXF0SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF0SIDL, 0xE0, 0x00); // Message 610 MCP2515_BitModify(RXF1SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF1SIDL, 0xE0, 0x80); // Message 614 MCP2515_BitModify(RXM0SIDH, 0xFF, 0xFF); MCP2515_BitModify(RXM0SIDL, 0xE0, 0xE0); // Buffer 0 gets odd MCP2515_BitModify(RXF2SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF2SIDL, 0xE0, 0x20); // Message 611 MCP2515_BitModify(RXF3SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF3SIDL, 0xE0, 0x60); // Message 613 MCP2515_BitModify(RXF4SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF4SIDL, 0xE0, 0x20); // Message 611 MCP2515_BitModify(RXF5SIDH, 0xFF, 0xC2); MCP2515_BitModify(RXF5SIDL, 0xE0, 0x20); // Message 611 MCP2515_BitModify(RXM1SIDH, 0xFF, 0xFF); MCP2515_BitModify(RXM1SIDL, 0xE0, 0xE0); // Buffer 1 gets even MCP2515_Mode(MODE_NORMAL); // ------------------------------------------------ // On-Chip CCAN Init RingBuffer_Init(&rx_buffer, _rx_buffer, sizeof(CCAN_MSG_OBJ_T), 8); RingBuffer_Flush(&rx_buffer); // MBB_STD can_msg_obj.msgobj = MBB_STD_MSG_OBJ; can_msg_obj.mode_id = 0x200; can_msg_obj.mask = 0xF00; LPC_CCAN_API->config_rxmsgobj(&can_msg_obj); // MBB_EXT_1 can_msg_obj.msgobj = MBB_EXT_1_MSG_OBJ; can_msg_obj.mode_id = 0x300; can_msg_obj.mask = 0xF00; LPC_CCAN_API->config_rxmsgobj(&can_msg_obj); // MBB_EXT_2 can_msg_obj.msgobj = MBB_EXT_2_MSG_OBJ; can_msg_obj.mode_id = 0x400; can_msg_obj.mask = 0xF00; LPC_CCAN_API->config_rxmsgobj(&can_msg_obj); // MBB_EXT_3 can_msg_obj.msgobj = MBB_EXT_3_MSG_OBJ; can_msg_obj.mode_id = 0x500; can_msg_obj.mask = 0xF00; LPC_CCAN_API->config_rxmsgobj(&can_msg_obj); // MBB_EXT_4 can_msg_obj.msgobj = MBB_EXT_4_MSG_OBJ; can_msg_obj.mode_id = 0x600; can_msg_obj.mask = 0xF00; LPC_CCAN_API->config_rxmsgobj(&can_msg_obj); }
/* Executes the program */ byte _sbi_run(void) // Runs a SBI program // Returns: // 0: No errors // 1: Reached end (no exit found) // 2: Program exited // 3: Wrong instruction code // 4: Can't understand byte // 5: User error { _exec = 1; rd = (*_getfch)(); switch (rd) { case _istr_assign: var1 = (*_getfch)(); _t[var1] = (*_getfch)(); break; case _istr_move: var1 = (*_getfch)(); _t[var1] = _t[(*_getfch)()]; break; case _istr_add: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); _t[(*_getfch)()] = _getval(var1t, var1) + _getval(var2t, var2); break; case _istr_sub: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); _t[(*_getfch)()] = _getval(var1t, var1) - _getval(var2t, var2); break; case _istr_mul: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); _t[(*_getfch)()] = _getval(var1t, var1) * _getval(var2t, var2); break; case _istr_div: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); _t[(*_getfch)()] = _getval(var1t, var1) / _getval(var2t, var2); break; case _istr_incr: _t[(*_getfch)()]++; break; case _istr_decr: _t[(*_getfch)()]--; break; case _istr_inv: var1 = (*_getfch)(); if (_t[var1]==0) _t[var1]=1; else _t[var1]=0; break; case _istr_tob: var1 = (*_getfch)(); if (_t[var1]>0) _t[var1]=1; else _t[var1]=0; break; case _istr_cmp: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); if (_getval(var1t, var1)==_getval(var2t, var2)) _t[(*_getfch)()]=1; else _t[(*_getfch)()]=0; break; case _istr_high: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); if (_getval(var1t, var1)>_getval(var2t, var2)) _t[(*_getfch)()]=1; else _t[(*_getfch)()]=0; break; case _istr_low: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); if (_getval(var1t, var1)<_getval(var2t, var2)) _t[(*_getfch)()]=1; else _t[(*_getfch)()]=0; break; case _istr_jump: var1t = (*_getfch)(); var1 = (*_getfch)(); if ((*_getfch)() > 0) { for (i=RETURNADDRESSESN-2; i>0; i--) _returnaddresses[i+1] = _returnaddresses[i]; _returnaddresses[1] = _returnaddresses[0]; _returnaddresses[0] = (*_getfpos)(); } (*_setfpos)(_labels[_getval(var1t, var1)]); break; case _istr_cmpjump: var1t = (*_getfch)(); var1 = (*_getfch)(); var2t = (*_getfch)(); var2 = (*_getfch)(); var3t = (*_getfch)(); var3 = (*_getfch)(); if ((*_getfch)() > 0) { for (i=RETURNADDRESSESN-2; i>0; i--) _returnaddresses[i+1] = _returnaddresses[i]; _returnaddresses[1] = _returnaddresses[0]; _returnaddresses[0] = (*_getfpos)(); } if (_getval(var1t, var1)==_getval(var2t, var2)) { (*_setfpos)(_labels[_getval(var3t, var3)]); } break; case _istr_ret: (*_setfpos)(_returnaddresses[0]); for (i=1; i<RETURNADDRESSESN; i++) _returnaddresses[i-1] = _returnaddresses[i]; break; case _istr_debug: var1t = (*_getfch)(); _debug(_getval(var1t, (*_getfch)())); break; case _istr_error: var1t = (*_getfch)(); _error(_getval(var1t, (*_getfch)())); return 5; break; case _istr_sint: var1t = (*_getfch)(); _userfid=_getval(var1t, (*_getfch)()); break; case _istr_int: for (i=0; i<16; i++) b[i] = (*_getfch)(); _sbifuncs[_userfid](b); break; case _istr_exit: return 2; break; case FOOTER_0: if ((*_getfch)()==FOOTER_1) return 1; else return 4; default: _error(0xB1); return 3; break; } _exec = 0; if (_intinqueue==1) _interrupt(_queuedint); // If there are interrupts in // in the queue, do it return 0; }
WndInfo *registerWindow(guint32 windowXid, jeventcallback handler) { // _info("register new window"); WndInfo *wi = (WndInfo *) malloc(sizeof(WndInfo)); memset(wi, 0, sizeof(WndInfo)); wi->xid = (guint32) windowXid; wi->menuPath = malloc(64); sprintf(wi->menuPath, "/com/canonical/menu/0x%x", windowXid); wi->menuroot = dbusmenu_menuitem_new(); if (wi->menuroot == NULL) { _error("can't create menuitem for new root"); _releaseWindow(wi); return NULL; } g_object_set_data(G_OBJECT(wi->menuroot), MENUITEM_JHANDLER_PROPERTY, handler); dbusmenu_menuitem_property_set(wi->menuroot, DBUSMENU_MENUITEM_PROP_LABEL, "DBusMenuRoot"); wi->server = dbusmenu_server_new(wi->menuPath); dbusmenu_server_set_root(wi->server, wi->menuroot); wi->registrar = g_dbus_proxy_new_for_bus_sync(G_BUS_TYPE_SESSION, G_DBUS_PROXY_FLAGS_NONE, NULL, DBUS_NAME, REG_OBJECT, REG_IFACE, NULL, NULL); if (wi->registrar == NULL) { // probably need to watch for registrar on dbus // guint watcher = g_bus_watch_name(G_BUS_TYPE_SESSION, DBUS_NAME, G_BUS_NAME_WATCHER_FLAGS_NONE, on_registrar_available, on_registrar_unavailable); _error("can't obtain registrar"); _releaseWindow(wi); return NULL; } char buf[1024]; _printWndInfo(wi, buf, 1024); _logmsg(LOG_LEVEL_INFO, "new window info: %s", buf); GError *error = NULL; g_dbus_proxy_call_sync( wi->registrar, "RegisterWindow", g_variant_new("(uo)", windowXid, wi->menuPath), G_DBUS_CALL_FLAGS_NONE, -1, NULL, &error); if (error != NULL) { _logmsg(LOG_LEVEL_ERROR, "Unable to register window, error: %s", error->message); g_error_free(error); _releaseWindow(wi); return NULL; } wi->jhandler = handler; g_signal_connect(wi->registrar, "notify::g-name-owner", G_CALLBACK(_onDbusOwnerChange), wi); return wi; }
void RigidArmNode :: computeMasterContribution() { int k, sign; IntArray R_uvw(3), uvw(3); FloatArray xyz(3); DofIDItem id; // decode of masterMask uvw.at(1) = this->findDofWithDofId(R_u); uvw.at(2) = this->findDofWithDofId(R_v); uvw.at(3) = this->findDofWithDofId(R_w); for ( int i = 1; i <= 3; i++ ) { xyz.at(i) = this->giveCoordinate(i) - masterNode->giveCoordinate(i); } if ( hasLocalCS() ) { // LCS is stored as global-to-local, so LCS*xyz_glob = xyz_loc xyz.rotatedWith(* this->localCoordinateSystem, 'n'); } for ( int i = 1; i <= numberOfDofs; i++ ) { id = this->giveDof(i)->giveDofID(); R_uvw.zero(); switch ( masterMask.at(i) ) { case 0: continue; break; case 1: if ( id == D_u ) { if ( uvw.at(2) && masterMask.at( uvw.at(2) ) ) { R_uvw.at(3) = ( ( int ) R_v ); } if ( uvw.at(3) && masterMask.at( uvw.at(3) ) ) { R_uvw.at(2) = -( ( int ) R_w ); } } else if ( id == D_v ) { if ( uvw.at(1) && masterMask.at( uvw.at(1) ) ) { R_uvw.at(3) = -( ( int ) R_u ); } if ( uvw.at(3) && masterMask.at( uvw.at(3) ) ) { R_uvw.at(1) = ( ( int ) R_w ); } } else if ( id == D_w ) { if ( uvw.at(1) && masterMask.at( uvw.at(1) ) ) { R_uvw.at(2) = ( ( int ) R_u ); } if ( uvw.at(2) && masterMask.at( uvw.at(2) ) ) { R_uvw.at(1) = -( ( int ) R_v ); } } break; default: _error("computeMasterContribution: unknown value in masterMask"); } k = ++ this->countOfMasterDofs->at(i); this->masterDofID [ i - 1 ]->at(k) = ( int ) id; this->masterContribution [ i - 1 ]->at(k) = 1.0; for ( int j = 1; j <= 3; j++ ) { if ( R_uvw.at(j) != 0 ) { sign = R_uvw.at(j) < 0 ? -1 : 1; k = ++ this->countOfMasterDofs->at(i); this->masterDofID [ i - 1 ]->at(k) = sign * R_uvw.at(j); this->masterContribution [ i - 1 ]->at(k) = sign * xyz.at(j); } } } }
// Private Methods ////////////////////////////////////////////////////////////// void AP_GPS_NMEA::parse_nmea_gps(void) { uint8_t NMEA_check; long aux_deg; long aux_min; char *parseptr; if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA if (buffer[bufferidx-4]=='*'){ // Check for the "*" character NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters if (GPS_checksum == NMEA_check){ // Checksum validation //Serial.println("buffer"); _setTime(); valid_read = true; new_data = true; // New GPS Data parseptr = strchr(buffer, ',')+1; //parseptr = strchr(parseptr, ',')+1; time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss parseptr = strchr(parseptr, ',')+1; aux_deg = parsedecimal(parseptr, 2); // degrees aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) parseptr = strchr(parseptr, ',')+1; if ( * parseptr == 'S') latitude = -1 * latitude; // South latitudes are negative parseptr = strchr(parseptr, ',')+1; // W longitudes are Negative aux_deg = parsedecimal(parseptr, 3); // degrees aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal) longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) //longitude = -1*longitude; // This Assumes that we are in W longitudes... parseptr = strchr(parseptr, ',')+1; if ( * parseptr == 'W') longitude = -1 * longitude; // West longitudes are negative parseptr = strchr(parseptr, ',')+1; fix = parsedecimal(parseptr, 1); parseptr = strchr(parseptr, ',')+1; num_sats = parsedecimal(parseptr, 2); parseptr = strchr(parseptr, ',')+1; HDOP = parsenumber(parseptr, 1); // HDOP * 10 parseptr = strchr(parseptr, ',')+1; altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters if (fix < 1) quality = 0; // No FIX else if(num_sats < 5) quality = 1; // Bad (Num sats < 5) else if(HDOP > 30) quality = 2; // Poor (HDOP > 30) else if(HDOP > 25) quality = 3; // Medium (HDOP > 25) else quality = 4; // Good (HDOP < 25) } else { _error("GPSERR: Checksum error!!\n"); } } } else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG //Serial.println(buffer); if (buffer[bufferidx-4]=='*'){ // Check for the "*" character NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters if (GPS_checksum == NMEA_check){ // Checksum validation _setTime(); valid_read = true; new_data = true; // New GPS Data parseptr = strchr(buffer, ',')+1; ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100 parseptr = strchr(parseptr, ',')+1; parseptr = strchr(parseptr, ',')+1; parseptr = strchr(parseptr, ',')+1; parseptr = strchr(parseptr, ',')+1; parseptr = strchr(parseptr, ',')+1; parseptr = strchr(parseptr, ',')+1; ground_speed = parsenumber(parseptr, 1) * 100 / 36; // Convert Km / h to m / s ( * 100) //GPS_line = true; } else { _error("GPSERR: Checksum error!!\n"); } } } else { bufferidx = 0; _error("GPSERR: Bad sentence!!\n"); } }
/* Steps the program of one instruction Returns: sbi_error_t */ sbi_error_t _sbi_step_internal(SBITHREAD* thread, sbi_runtime_t* rt) { uint8_t rd, var1t, var2t, var3t; DTYPE var1, var2, var3; int i; rd = _getfch(); _TRACE("Instruction code 0x%02x at pcount: 0x%02x thread %d\n", rd, CUR_PCOUNT-1, thread->threadid ); switch (rd) { case _istr_assign: var1t= _getfch(); var1 = _getfch(); _setval(var1t,var1,_getfval(_getfch()), thread); break; case _istr_move: var1t = _getfch(); var1 = _getfch(); var2t = _getfch(); var2 = _getfval(var2t); _setval ( var1t, var1, _getval(var2t,var2, thread), thread ); break; case _istr_add: case _istr_sub: case _istr_mul: case _istr_div: case _istr_cmp: case _istr_high: case _istr_low: case _istr_lte: case _istr_gte: { DTYPE v1, v2, val; var1t = _getfch(); var1 = _getfval(var1t); var2t = _getfch(); var2 = _getfval(var2t); var3t = _getfch(); var3 = _getfch(); v1=_getval(var1t,var1, thread); v2=_getval(var2t,var2, thread); val = rd == _istr_add ? v1+v2 : rd == _istr_sub ? v1-v2 : rd == _istr_mul ? v1*v2 : rd == _istr_div ? v1/v2 : rd == _istr_cmp ? (v1==v2?1:0) : rd == _istr_high ? (v1>v2?1:0) : rd == _istr_low ? (v1<v2?1:0) : rd == _istr_lte ? (v1<=v2?1:0) : rd == _istr_gte ? (v1>=v2?1:0) : 0; _setval( var3t, var3, val, thread ); } break; case _istr_push: if (thread->stackp>=STACK_SIZE-1) { _error(SBI_STACK_OVERFLOW); // TODO error codes (overflow) return SBI_PROG_ERROR; } var1t=_getfch(); var1 = _getfval(var1t); thread->stack[thread->stackp++] = _getval(var1t,var1,thread); break; case _istr_pop: { uint8_t n; if (thread->stackp==0) { _error(SBI_STACK_UNDERFLOW); // underflow return SBI_PROG_ERROR; } n = _getfch(); --thread->stackp; if (n) { DTYPE val = thread->stack[thread->stackp]; var1t=_getfch(); var1=_getfch(); _setval(var1t,var1,val,thread); } } break; case _istr_incr: case _istr_decr: case _istr_inv: case _istr_tob: { DTYPE val; var1t=_getfch(); var1=_getfch(); val = _getval( var1t, var1, thread); val = rd == _istr_incr ? val+1 : rd == _istr_decr ? val-1 : rd == _istr_inv ? !val : rd == _istr_tob ? (val?1:0) : 0; _setval(var1t,var1,val,thread); } break; case _istr_jump: var1t = _getfch(); var1 = _getfval(var1t); if (_getfch() > 0) { _RETADDRS[thread->raddr_cnt++] = CUR_PCOUNT; } CUR_PCOUNT = _LABELS[_getval(var1t, var1, thread)]; break; case _istr_cmpjump: var1t = _getfch(); var1 = _getfval(var1t); var2t = _getfch(); var2 = _getfval(var2t); var3t = _getfch(); var3 = _getfch(); i=_getfch(); // push ret if (_getval(var1t, var1, thread)==_getval(var2t, var2, thread)) { if (i > 0) { _RETADDRS[thread->raddr_cnt++] = CUR_PCOUNT; } CUR_PCOUNT = _LABELS[_getval(var3t, var3, thread)]; } break; case _istr_ret: if (thread->raddr_cnt>0) { CUR_PCOUNT = _RETADDRS[--thread->raddr_cnt]; } else { // thread exit return SBI_THREAD_EXIT; } break; case _istr_debug: var1t = _getfch(); var1 = _getfval(var1t); _debug(_getval(var1t, var1, thread)); break; case _istr_error: var1t = _getfch(); var1 = _getfval(var1t); _error(_getval(var1t, var1, thread)); return SBI_PROG_ERROR; break; case _istr_print: { uint16_t strLoc = _getfch(); strLoc |= _getfch()<<8; if (rt->ctx->print) { int slen, tmplen; char *tmp, cur; PCOUNT curp = CUR_PCOUNT; CUR_PCOUNT = strLoc; slen = 0; tmplen=0; tmp = NULL; cur; do { cur = _getfch(); if (tmplen==slen) { tmp = (char*) realloc ( tmp, slen+20 ); slen += 20; if (!tmp) return SBI_ALLOC_ERROR; } tmp[tmplen++] = cur; } while (cur != 0); _print(tmp); free(tmp); CUR_PCOUNT = curp; } } break; case _istr_sint: var1t = _getfch(); var1 = _getfval(var1t); thread->_userfid=_getval(var1t, var1, thread); break; case _istr_int: case _istr_intr: { uint8_t argc; DTYPE *pvals=NULL, r; // NOTE should parameters be pushed on the stack instead? if (rd==_istr_intr) { var1t=_getfch(); var1=_getfch(); } argc = _getfch(); if (argc>0) { pvals = malloc(sizeof(DTYPE)*(argc)); if (!pvals) return SBI_ALLOC_ERROR; } for (i=0;i<argc;++i) { var2t = _getfch(); var2 = _getfval(var2t); pvals[i] = _getval(var2t,var2,thread); } r = RT->ctx->sbi_user_funcs[thread->_userfid](argc,pvals); if (rd==_istr_intr) { _setval(var1t,var1,r,thread); } if (pvals) free(pvals); } break; case _istr_thread: { int ret; uint8_t tId; var1t = _getfch(); var1 = _getfch(); var2t = _getfch(); var2 = _getfch(); ret = _sbi_new_thread_at(_LABELS[_getval(var1t,var1,thread)], rt); tId = !ret ? rt->new_threadid : 0; _setval( var2t, var2, tId, thread ); if (ret) _error(ret); return ret; } break; case _istr_wait: { DTYPE tId; var1t = _getfch(); var1 = _getfch(); tId = _getval(var1t,var1,thread); for (i=0;i<rt->thread_cnt;++i) { if ( rt->_sbi_threads[i]->threadid == tId && rt->_sbi_threads[i]->status == RUNNING ) { CUR_PCOUNT-=3; // rerun wait break; } } } break; case _istr_alive: { DTYPE tId, val; var1t = _getfch(); var1 = _getfch(); var2t = _getfch(); var2 = _getfch(); tId = _getval(var1t,var1,thread); val = 0; // not running for (i=0;i<rt->thread_cnt;++i) { if (rt->_sbi_threads[i]->threadid == tId && rt->_sbi_threads[i]->status == RUNNING) { val=1; break; } } _setval(var2t,var2,val,thread); } break; case _istr_stop: { DTYPE tId; var1t = _getfch(); var1 = _getfch(); tId = _getval(var1t,var1,thread); for (i=0;i<rt->thread_cnt;++i) { if (rt->_sbi_threads[i]->threadid == tId) { rt->_sbi_threads[i]->status = STOPPED; break; } } } break; case _istr_exit: return SBI_PROG_EXIT; break; case FOOTER_0: if (_getfch()==FOOTER_1) return SBI_PROG_EOF; else return SBI_INSTR_ERROR; default: _error(SBI_INSTR_ERROR); _error(rd); _error(CUR_PCOUNT-1); _ERR("Instruction error 0x%02x at pcount: 0x%02x thread %d\n", rd, CUR_PCOUNT-1, thread->threadid ); return SBI_PROG_ERROR; break; } return SBI_NOERROR; }
// Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // void AP_GPS_UBLOX::update(void) { byte data; int numc; numc = _port->available(); for (int i = 0; i < numc; i++){ // Process bytes received // read the next byte data = _port->read(); switch(_step){ // Message preamble detection // // If we fail to match any of the expected bytes, we reset // the state machine and re-consider the failed byte as // the first byte of the preamble. This improves our // chances of recovering from a mismatch and makes it less // likely that we will be fooled by the preamble appearing // as data in some other message. // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; // FALLTHROUGH case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // // We sniff the class and message ID to decide whether we // are going to gather the message bytes or just discard // them. // // We always collect the length so that we can avoid being // fooled by preamble bytes in messages. // case 2: _step++; if (CLASS_NAV == data) { _gather = true; // class is interesting, maybe gather _ck_b = _ck_a = data; // reset the checksum accumulators } else { _error("ignoring class 0x%x\n", (int)data); _gather = false; // class is not interesting, discard } break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; if (_gather) { // if class was interesting switch(data) { case MSG_POSLLH: // message is interesting _expect = sizeof(ubx_nav_posllh); break; case MSG_STATUS: _expect = sizeof(ubx_nav_status); break; case MSG_SOL: _expect = sizeof(ubx_nav_solution); break; case MSG_VELNED: _expect = sizeof(ubx_nav_velned); break; default: _error("ignoring message 0x%x\n", (int)data); _gather = false; // message is not interesting } } break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)data; // payload length high byte _payload_counter = 0; // prepare to receive payload if (_payload_length != _expect) { _error("payload %d expected %d\n", _payload_length, _expect); _gather = false; } break; // Receive message data // case 6: _ck_b += (_ck_a += data); // checksum byte if (_gather) // gather data if requested _buffer.bytes[_payload_counter] = data; if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 7: _step++; if (_ck_a != data) { _error("GPS_UBLOX: checksum error\n"); _step = 0; } break; case 8: _step = 0; if (_ck_b != data) { _error("GPS_UBLOX: checksum error\n"); break; } if (_gather) _parse_gps(); // Parse the new GPS packet } } }
void OpcodeComparison::_run() { Logger::debug("SCRIPT") << "[8033-8038] [*] " << _cmpOpcodeName() << std::endl; auto bValue = _script->dataStack()->pop(); auto aValue = _script->dataStack()->pop(); int result = 0; switch (aValue.type()) { case StackValue::Type::INTEGER: { int arg1 = aValue.integerValue(); switch (bValue.type()) { case StackValue::Type::INTEGER: { result = _compare(arg1, bValue.integerValue()); // INTEGER op INTEGER break; } case StackValue::Type::FLOAT: { result = _compare(arg1, bValue.floatValue()); // INTEGER op FLOAT break; } case StackValue::Type::STRING: { result = _compare(arg1, bValue.toInteger()); // INTEGER op STRING (parsed as int) break; } default: { _error(std::string() + _cmpOpcodeName() + ": invalid right argument type: " + bValue.typeName()); } } break; } case StackValue::Type::FLOAT: { float arg1 = aValue.floatValue(); switch (bValue.type()) { case StackValue::Type::INTEGER: { result = _compare(arg1, bValue.integerValue()); // FLOAT op INTEGER break; } case StackValue::Type::FLOAT: { result = _compare(arg1, bValue.floatValue()); // FLOAT op FLOAT break; } case StackValue::Type::STRING: { float arg2 = 0.0; try { arg2 = std::stof(bValue.stringValue()); } catch (std::invalid_argument ex) { } catch (std::out_of_range ex) { } result = _compare(arg1, arg2); // FLOAT op STRING (parsed as float) break; } default: { _error(std::string() + _cmpOpcodeName() + ": invalid right argument type: " + bValue.typeName()); } } break; } case StackValue::Type::STRING: { switch (bValue.type()) { case StackValue::Type::INTEGER: { result = _compare(aValue.toInteger(), bValue.integerValue()); // STRING (as integer) op INTEGER break; } case StackValue::Type::FLOAT: { float arg1 = 0.0; try { arg1 = std::stof(aValue.stringValue()); } catch (std::invalid_argument ex) { } catch (std::out_of_range ex) { } result = _compare(arg1, bValue.floatValue()); // STRING (as float) op FLOAT break; } case StackValue::Type::STRING: { result = _compare(aValue.stringValue(), bValue.stringValue()); // STRING op STRING break; } default: { _error(std::string() + _cmpOpcodeName() + ": invalid right argument type: " + bValue.typeName()); } } break; } case StackValue::Type::OBJECT: { switch (bValue.type()) { case StackValue::Type::INTEGER: { result = _compare((int)aValue.toBoolean(), bValue.integerValue()); // OBJECT op INTEGER break; } case StackValue::Type::FLOAT: { result = _compare((float)aValue.toBoolean(), bValue.floatValue()); // OBJECT op FLOAT break; } case StackValue::Type::STRING: { result = _compare(aValue.toString(), bValue.stringValue()); // OBJECT op STRING - compare object name break; } case StackValue::Type::OBJECT: { result = _compare(aValue.objectValue(), bValue.objectValue()); // OBJECT op OBJECT break; } default: { _error(std::string() + _cmpOpcodeName() + ": invalid right argument type: " + bValue.typeName()); } } break; } default: { _error(std::string() + _cmpOpcodeName() + ": invalid left argument type: " + aValue.typeName()); } } _script->dataStack()->push(result); }
void DEIDynamic :: solveYourselfAt(TimeStep *tStep) { // // creates system of governing eq's and solves them at given time step // // this is an explicit problem: we assemble governing equating at time t // and solution is obtained for time t+dt // // first assemble problem at current time step to obtain results in following // time step. // and then print results for this step also. // for first time step we need special start code Domain *domain = this->giveDomain(1); int nelem = domain->giveNumberOfElements(); int nman = domain->giveNumberOfDofManagers(); IntArray loc; Element *element; DofManager *node; Dof *iDof; int nDofs, neq; int i, k, n, j, jj, kk, init = 0; double coeff, maxDt, maxOmi, maxOm = 0., maxOmEl, c1, c2, c3; FloatMatrix charMtrx, charMtrx2; FloatArray previousDisplacementVector; neq = this->giveNumberOfDomainEquations(1, EModelDefaultEquationNumbering()); if ( tStep->giveNumber() == giveNumberOfFirstStep() ) { init = 1; #ifdef VERBOSE OOFEM_LOG_INFO("Assembling mass matrix\n"); #endif // // first step assemble mass Matrix // massMatrix.resize(neq); massMatrix.zero(); EModelDefaultEquationNumbering dn; for ( i = 1; i <= nelem; i++ ) { element = domain->giveElement(i); element->giveLocationArray(loc, EID_MomentumBalance, dn); element->giveCharacteristicMatrix(charMtrx, LumpedMassMatrix, tStep); // charMtrx.beLumpedOf(fullCharMtrx); element->giveCharacteristicMatrix(charMtrx2, StiffnessMatrix, tStep); // // assemble it manually // #ifdef DEBUG if ( ( n = loc.giveSize() ) != charMtrx.giveNumberOfRows() ) { _error("solveYourselfAt : dimension mismatch"); } #endif n = loc.giveSize(); maxOmEl = 0.; for ( j = 1; j <= n; j++ ) { if ( charMtrx.at(j, j) > ZERO_MASS ) { maxOmi = charMtrx2.at(j, j) / charMtrx.at(j, j); if ( init ) { maxOmEl = ( maxOmEl > maxOmi ) ? ( maxOmEl ) : ( maxOmi ); } } } maxOm = ( maxOm > maxOmEl ) ? ( maxOm ) : ( maxOmEl ); for ( j = 1; j <= n; j++ ) { jj = loc.at(j); if ( ( jj ) && ( charMtrx.at(j, j) <= ZERO_MASS ) ) { charMtrx.at(j, j) = charMtrx2.at(j, j) / maxOmEl; } } for ( j = 1; j <= n; j++ ) { jj = loc.at(j); if ( jj ) { massMatrix.at(jj) += charMtrx.at(j, j); } } } // if init - try to determine the best deltaT if ( init ) { maxDt = 2 / sqrt(maxOm); if ( deltaT > maxDt ) { OOFEM_LOG_RELEVANT("DEIDynamic: deltaT reduced to %e\n", maxDt); deltaT = maxDt; tStep->setTimeIncrement(deltaT); } } // // special init step - compute displacements at tstep 0 // displacementVector.resize(neq); displacementVector.zero(); nextDisplacementVector.resize(neq); nextDisplacementVector.zero(); velocityVector.resize(neq); velocityVector.zero(); accelerationVector.resize(neq); accelerationVector.zero(); for ( j = 1; j <= nman; j++ ) { node = domain->giveDofManager(j); nDofs = node->giveNumberOfDofs(); for ( k = 1; k <= nDofs; k++ ) { // ask for initial values obtained from // bc (boundary conditions) and ic (initial conditions) // now we are setting initial cond. for step -1. iDof = node->giveDof(k); if ( !iDof->isPrimaryDof() ) { continue; } jj = iDof->__giveEquationNumber(); if ( jj ) { nextDisplacementVector.at(jj) = iDof->giveUnknown(VM_Total, tStep); // become displacementVector after init velocityVector.at(jj) = iDof->giveUnknown(VM_Velocity, tStep); // accelerationVector = iDof->giveUnknown(AccelerartionVector,tStep) ; } } } for ( j = 1; j <= neq; j++ ) { nextDisplacementVector.at(j) -= velocityVector.at(j) * ( deltaT ); } return; } // end of init step #ifdef VERBOSE OOFEM_LOG_INFO("Assembling right hand side\n"); #endif c1 = ( 1. / ( deltaT * deltaT ) ); c2 = ( 1. / ( 2. * deltaT ) ); c3 = ( 2. / ( deltaT * deltaT ) ); previousDisplacementVector = displacementVector; displacementVector = nextDisplacementVector; // // assembling the element part of load vector // loadVector.resize( this->giveNumberOfDomainEquations(1, EModelDefaultEquationNumbering()) ); loadVector.zero(); this->assembleVector(loadVector, tStep, EID_MomentumBalance, ExternalForcesVector, VM_Total, EModelDefaultEquationNumbering(), domain); // // assembling additional parts of right hand side // EModelDefaultEquationNumbering dn; for ( i = 1; i <= nelem; i++ ) { element = domain->giveElement(i); element->giveLocationArray(loc, EID_MomentumBalance, dn); element->giveCharacteristicMatrix(charMtrx, StiffnessMatrix, tStep); n = loc.giveSize(); for ( j = 1; j <= n; j++ ) { jj = loc.at(j); if ( jj ) { for ( k = 1; k <= n; k++ ) { kk = loc.at(k); if ( kk ) { loadVector.at(jj) -= charMtrx.at(j, k) * displacementVector.at(kk); } } // // if init step - find minimum period of vibration in order to // determine maximal admissible time step // //maxOmi = charMtrx.at(j,j)/massMatrix.at(jj) ; //if (init) maxOm = (maxOm > maxOmi) ? (maxOm) : (maxOmi) ; } } } for ( j = 1; j <= neq; j++ ) { coeff = massMatrix.at(j); loadVector.at(j) += coeff * c3 * displacementVector.at(j) - coeff * ( c1 - dumpingCoef * c2 ) * previousDisplacementVector.at(j); } // // set-up numerical model // /* it is not necessary to call numerical method * approach used here is not good, but effective enough * inverse of diagonal mass matrix is done here */ // // call numerical model to solve arose problem - done locally here // #ifdef VERBOSE OOFEM_LOG_RELEVANT( "Solving [step number %8d, time %15e]\n", tStep->giveNumber(), tStep->giveTargetTime() ); #endif double prevD; for ( i = 1; i <= neq; i++ ) { prevD = previousDisplacementVector.at(i); nextDisplacementVector.at(i) = loadVector.at(i) / ( massMatrix.at(i) * ( c1 + dumpingCoef * c2 ) ); velocityVector.at(i) = nextDisplacementVector.at(i) - prevD; accelerationVector.at(i) = nextDisplacementVector.at(i) - 2. * displacementVector.at(i) + prevD; } accelerationVector.times(c1); velocityVector.times(c2); }
void NlDEIDynamic :: computeMassMtrx(FloatArray &massMatrix, double &maxOm, TimeStep *tStep) { Domain *domain = this->giveDomain(1); int nelem = domain->giveNumberOfElements(); int neq = this->giveNumberOfEquations(EID_MomentumBalance); int i, j, jj, n; double maxOmi, maxOmEl; FloatMatrix charMtrx, charMtrx2; IntArray loc; Element *element; EModelDefaultEquationNumbering en; #ifdef __PARALLEL_MODE int result; #endif #ifndef LOCAL_ZERO_MASS_REPLACEMENT FloatArray diagonalStiffMtrx; #endif maxOm = 0.; massMatrix.resize(neq); massMatrix.zero(); for ( i = 1; i <= nelem; i++ ) { element = domain->giveElement(i); #ifdef __PARALLEL_MODE // skip remote elements (these are used as mirrors of remote elements on other domains // when nonlocal constitutive models are used. They introduction is necessary to // allow local averaging on domains without fine grain communication between domains). if ( element->giveParallelMode() == Element_remote ) { continue; } #endif element->giveLocationArray(loc, EID_MomentumBalance, en); element->giveCharacteristicMatrix(charMtrx, LumpedMassMatrix, tStep); #ifdef LOCAL_ZERO_MASS_REPLACEMENT element->giveCharacteristicMatrix(charMtrx2, StiffnessMatrix, tStep); #endif #ifdef DEBUG if ( ( n = loc.giveSize() ) != charMtrx.giveNumberOfRows() ) { _error("solveYourselfAt : dimension mismatch"); } #endif n = loc.giveSize(); #ifdef LOCAL_ZERO_MASS_REPLACEMENT maxOmEl = 0.; double maxElmass = -1.0; for ( j = 1; j <= n; j++ ) { maxElmass = max( maxElmass, charMtrx.at(j, j) ); } if ( maxElmass <= 0.0 ) { _warning2("solveYourselfAt: Element (%d) with zero (or negative) lumped mass encountered\n", i); } for ( j = 1; j <= n; j++ ) { if ( charMtrx.at(j, j) > maxElmass * ZERO_REL_MASS ) { maxOmi = charMtrx2.at(j, j) / charMtrx.at(j, j); maxOmEl = ( maxOmEl > maxOmi ) ? ( maxOmEl ) : ( maxOmi ); } } maxOm = ( maxOm > maxOmEl ) ? ( maxOm ) : ( maxOmEl ); for ( j = 1; j <= n; j++ ) { jj = loc.at(j); if ( ( jj ) && ( charMtrx.at(j, j) <= maxElmass * ZERO_REL_MASS ) ) { charMtrx.at(j, j) = charMtrx2.at(j, j) / maxOmEl; } } #endif for ( j = 1; j <= n; j++ ) { jj = loc.at(j); if ( jj ) { massMatrix.at(jj) += charMtrx.at(j, j); } } } #ifndef LOCAL_ZERO_MASS_REPLACEMENT // If init step - find minimun period of vibration in order to // determine maximal admisible time step // global variant for (i=1; i<=nelem; i++) { element = domain -> giveElement(i); element->giveLocationArray(loc, EID_MomentumBalance, en); element->giveCharacteristicMatrix( charMtrx, StiffnessMatrix, tStep ); n = loc.giveSize() ; for (j=1; j<=n; j++) { jj = loc.at(j); if (jj) { diagonalStiffMtrx.at(jj) += charMtrx.at(j,j); } } } // Find find global minimun period of vibration double maxElmass = -1.0; for (j=1 ; j<=n; j++) { maxElmass = max(maxElmass,charMtrx.at(j,j)); } if ( maxElmass <= 0.0 ) { _error("solveYourselfAt: Element with zero (or negative) lumped mass encountered\n"); } for (j=1; j<= neq; j++) { if (massMatrix.at(j) > maxElmass * ZERO_REL_MASS ) { maxOmi = diagonalStiffMtrx.at(j) / massMatrix.at(j); maxOm = (maxOm > maxOmi) ? (maxOm) : (maxOmi); } } // Set ZERO MASS members in massMatrix to value which corresponds to global maxOm. for (i=1; i<= neq; i++) { if (massMatrix.at(i) <= maxElmass*ZERO_REL_MASS) { massMatrix.at(i) = diagonalStiffMtrx.at(i) / maxOm; } } #endif #ifdef __PARALLEL_MODE this->updateSharedDofManagers( massMatrix, MassExchangeTag ); // Determine maxOm over all processes. #ifdef __USE_MPI double globalMaxOm; #ifdef __VERBOSE_PARALLEL VERBOSEPARALLEL_PRINT( "NlDEIDynamic :: computeMassMtrx", "Reduce of maxOm started", this->giveRank() ); #endif result = MPI_Allreduce(& maxOm, & globalMaxOm, 1, MPI_DOUBLE, MPI_MAX, MPI_COMM_WORLD); #ifdef __VERBOSE_PARALLEL VERBOSEPARALLEL_PRINT( "NlDEIDynamic :: computeMassMtrx", "Reduce of maxOm finished", this->giveRank() ); #endif if ( result != MPI_SUCCESS ) { _error("setUpCommunicationMaps: MPI_Allreduce failed"); } maxOm = globalMaxOm; #else WARNING: NOT SUPPORTED MESSAGE PARSING LIBRARY #endif #endif }
/* FRONT END functions */ int ldcs_audit_server_fe_md_open ( char **hostlist, int hostlistsize, unsigned int port, void **data ) { int rc=0; int num_ports = 20; int num_hosts = -1; int* portlist = malloc(num_ports * sizeof(int)); int i, sersize, connid; char *serdata; ldcs_message_t* msg; ldcs_message_t *in_msg; ldcs_msocket_hostinfo_t hostinfo; int myerank, esize; int *eportlist; char **ehostlist; ldcs_msocket_fe_data_t *fe_data; fe_data=(ldcs_msocket_fe_data_t *) malloc(sizeof(ldcs_msocket_fe_data_t)); if(!fe_data) _error("could not allocate fe_data"); /* get info about server over external fabric, if available */ if(ldcs_msocket_external_fabric_CB_registered) { char buffer[MAX_PATH_LEN]; rc=gethostname(buffer, MAX_PATH_LEN); if(!strcmp(buffer,"zam371guest")) { strcpy(buffer,"localhost"); } ldcs_msocket_external_fabric_CB(buffer, -1, &myerank, &esize, &ehostlist, &eportlist, ldcs_msocket_external_fabric_CB_data); num_hosts=esize; } else { num_hosts=hostlistsize; /* build our portlist */ for (i=0; i<num_ports; i++) { portlist[i] = 5000 + i; } } /* connect to first host */ if(ldcs_msocket_external_fabric_CB_registered) { connid=ldcs_audit_server_md_msocket_connect(ehostlist[0], eportlist, 1); debug_printf3("after connect connid=%d (E: %s,%d)\n",connid,ehostlist[0], eportlist[0]); } else { connid=ldcs_audit_server_md_msocket_connect(hostlist[0], portlist, num_ports); debug_printf3("after connect connid=%d (D: %s,%d)\n",connid,hostlist[0], portlist[0]); } /* message for hostinfo */ debug_printf3("send msg hostinfo\n"); msg=ldcs_msg_new(); hostinfo.rank=0; hostinfo.size=num_hosts; hostinfo.depth=0; hostinfo.cinfo_from=hostinfo.cinfo_to=-1; /* hostinfo.cinfo_dir=-1; */ msg->header.type=LDCS_MSG_MD_HOSTINFO; msg->header.mtype=LDCS_MSG_MTYPE_P2P; msg->header.source=-1; msg->header.from=-1; msg->header.dest=0; msg->header.len=sizeof(hostinfo); msg->alloclen=sizeof(hostinfo); msg->data=(char *) &hostinfo; ldcs_send_msg_socket(connid,msg); if(!ldcs_msocket_external_fabric_CB_registered) { debug_printf3("processing hostlist of size %d\n",hostlistsize); ldcs_audit_server_md_msocket_serialize_hostlist(hostlist, hostlistsize, &serdata, &sersize); debug_printf3("serialized hostlist has size %d\n",sersize); /* message for hostlist */ msg->header.type=LDCS_MSG_MD_HOSTLIST; msg->header.mtype=LDCS_MSG_MTYPE_P2P; msg->header.len=sersize; msg->alloclen=sersize; msg->data=serdata; /* sent message to first host */ debug_printf3("send msg hostlist\n"); ldcs_send_msg_socket(connid,msg); } /* message for start of bootstrap */ msg->header.type=LDCS_MSG_MD_BOOTSTRAP; msg->header.mtype=LDCS_MSG_MTYPE_P2P; msg->header.len=0; msg->alloclen=0; msg->data=NULL; /* sent message to first host */ debug_printf3("send msg bootstrap\n"); ldcs_send_msg_socket(connid,msg); /* message for ending bootstrap */ msg->header.type=LDCS_MSG_MD_BOOTSTRAP_END; msg->header.mtype=LDCS_MSG_MTYPE_BCAST; msg->header.source=-1; msg->header.from=-1; msg->header.dest=-1; msg->header.len=0; msg->alloclen=0; msg->data=NULL; /* sent message to first host */ debug_printf3("send msg bootstrap end\n"); ldcs_send_msg_socket(connid,msg); /* receive msg from root node whether bootstrap ended */ in_msg=ldcs_recv_msg_socket(connid, LDCS_READ_BLOCK); if(in_msg->header.type==LDCS_MSG_MD_BOOTSTRAP_END_OK) { fprintf(stderr, "SERVERFE: bootstrap --> successful\n"); } else { fprintf(stderr, "SERVERFE: bootstrap --> not successful\n"); } ldcs_msg_free(&in_msg); fe_data->connid=connid; *data=fe_data; return(rc); }
//is_leaf int is_leaf(Node* n){ if(n == NULL) _error(ONULL); return (n->children == NULL); }
/* Step all threads of one instruction Returns the error code for current thread that was stepped */ sbi_error_t sbi_step(void *rt) { unsigned int ret=0; SBITHREAD *thread; if (!rt) return SBI_INVALID_RT; if (!RT->thread_cnt) return SBI_NOT_RUNNING; #if !_SBI_MULTITHREADING_EQUALTIME // TODO fix //int c = 0; //int i; //for (i=0; i<THREADMAXNUM; i++) // if (RT->_sbi_threads[i]) // { // if (RT->_sbi_threads[i]->status == RUNNING) // { // unsigned int ret = _sbi_step_internal(_sbi_threads[i], RT); // c++; // if (ret) // { // RT->_sbi_threads[i]->_lasterror = ret; // if (ret > 2) // RT->_sbi_threads[i]->status = ERROR; // else // RT->_sbi_threads[i]->status = STOPPED; // } // } // } //return c; #else thread = RT->_sbi_threads[RT->_sbi_currentthreadn]; if (thread) { if (thread->status == RUNNING) { if (RT->int_cnt>0) { INTERRUPT id; _RETADDRS[thread->raddr_cnt++] = CUR_PCOUNT; id = RT->_intinqueue[--RT->int_cnt]; CUR_PCOUNT = RT->_interrupts[id]; // Set the program counter to interrupt's address } ret = _sbi_step_internal(thread,rt); } else if (thread->status == STOPPED) { ret = SBI_THREAD_EXIT; } if (ret) { _TRACE ( "Thread (%d) exit: %d\n", thread->threadid, ret ); _sbi_removethread(thread,RT); thread=NULL; if (ret >= SBI_PROG_EXIT) { // program exit int i; for ( i=0;i<RT->thread_cnt;++i ) RT->_sbi_threads[i]->status = STOPPED; } else { // don't skip the next thread // set it to the previous threadn so the next thread // is not starved. RT->_sbi_currentthreadn = RT->_sbi_currentthreadn == 0 ? RT->thread_cnt-1 : RT->_sbi_currentthreadn-1; } } } else { // should not get this _error(SBI_INTERNAL_ERROR); } RT->_sbi_currentthreadn++; if (RT->_sbi_currentthreadn > (RT->thread_cnt - 1)) RT->_sbi_currentthreadn = 0; #endif return ret; }
//child Node* child(Node* n, int i){ if(n == NULL) _error(ONULL); return listget(&n->children, i); }
void QDeclarativeFeedbackEffect::setFeedbackEffect(QFeedbackEffect* effect) { m_effect = effect; QObject::connect(m_effect, SIGNAL(stateChanged()), this, SLOT(updateState())); QObject::connect(m_effect, SIGNAL(error(QFeedbackEffect::ErrorType)), this, SLOT(_error(QFeedbackEffect::ErrorType))); }
size_t _parseGDBMessage(struct GDBStub* stub, const char* message) { uint8_t checksum = 0; int parsed = 1; switch (*message) { case '+': stub->lineAck = GDB_ACK_RECEIVED; return parsed; case '-': stub->lineAck = GDB_NAK_RECEIVED; return parsed; case '$': ++message; break; case '\x03': mDebuggerEnter(&stub->d, DEBUGGER_ENTER_MANUAL, 0); return parsed; default: _nak(stub); return parsed; } int i; char messageType = message[0]; for (i = 0; message[i] != '#'; ++i, ++parsed) { checksum += message[i]; } if (!message[i]) { _nak(stub); return parsed; } ++i; ++parsed; if (!message[i]) { _nak(stub); return parsed; } else if (!message[i + 1]) { ++parsed; _nak(stub); return parsed; } parsed += 2; int networkChecksum = _hex2int(&message[i], 2); if (networkChecksum != checksum) { mLOG(DEBUGGER, WARN, "Checksum error: expected %02x, got %02x", checksum, networkChecksum); _nak(stub); return parsed; } _ack(stub); ++message; switch (messageType) { case '?': snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGINT); _sendMessage(stub); break; case 'c': _continue(stub, message); break; case 'G': _writeGPRs(stub, message); break; case 'g': _readGPRs(stub, message); break; case 'H': // This is faked because we only have one thread strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4); _sendMessage(stub); break; case 'M': _writeMemory(stub, message); break; case 'm': _readMemory(stub, message); break; case 'P': _writeRegister(stub, message); break; case 'p': _readRegister(stub, message); break; case 'Q': _processQWriteCommand(stub, message); break; case 'q': _processQReadCommand(stub, message); break; case 's': _step(stub, message); break; case 'V': _processVWriteCommand(stub, message); break; case 'v': _processVReadCommand(stub, message); break; case 'X': _writeMemoryBinary(stub, message); break; case 'Z': _setBreakpoint(stub, message); break; case 'z': _clearBreakpoint(stub, message); break; default: _error(stub, GDB_UNSUPPORTED_COMMAND); break; } return parsed; }
bool CommonSolverSparseLib::_solve(Matrix *mat, double *res) { printf("SparseLib++ solver\n"); CSCMatrix *Acsc = NULL; if (CooMatrix *mcoo = dynamic_cast<CooMatrix*>(mat)) Acsc = new CSCMatrix(mcoo); else if (DenseMatrix *mden = dynamic_cast<DenseMatrix *>(mat)) Acsc = new CSCMatrix(mden); else if (CSCMatrix *mcsc = dynamic_cast<CSCMatrix*>(mat)) Acsc = mcsc; else if (CSRMatrix *mcsr = dynamic_cast<CSRMatrix*>(mat)) Acsc = new CSCMatrix(mcsr); else _error("Matrix type not supported."); int nnz = Acsc->get_nnz(); int size = Acsc->get_size(); CompCol_Mat_double Acc = CompCol_Mat_double(size, size, nnz, Acsc->get_Ax(), Acsc->get_Ai(), Acsc->get_Ap()); // rhs VECTOR_double rhs(res, size); // preconditioner CompCol_ILUPreconditioner_double ILU(Acc); VECTOR_double xv = ILU.solve(rhs); // method int result = -1; switch (method) { case HERMES_CommonSolverSparseLibSolver_ConjugateGradientSquared: result = CGS(Acc, xv, rhs, ILU, maxiter, tolerance); break; case CommonSolverSparseLibSolver_RichardsonIterativeRefinement: result = IR(Acc, xv, rhs, ILU, maxiter, tolerance); break; default: _error("SparseLib++ error. Method is not defined."); } if (result == 0) printf("SparseLib++ solver: maxiter: %i, tol: %e\n", maxiter, tolerance); else _error("SparseLib++ error."); double *x; x = (double*) malloc(size * sizeof(double)); for (int i = 0 ; i < xv.size() ; i++) x[i] = xv(i); memcpy(res, x, size*sizeof(double)); delete[] x; if (!dynamic_cast<CSCMatrix*>(mat)) delete Acsc; }
NM_Status GJacobi :: solve(FloatMatrix *a, FloatMatrix *b, FloatArray *eigv, FloatMatrix *x) //void GJacobi :: solveYourselfAt (TimeStep* tNow) // // this function solve the generalized eigenproblem using the Generalized // jacobi iteration // // { int i, j, k, nsweep, nr, jj; double eps, eptola, eptolb, akk, ajj, ab, check, sqch, d1, d2, den, ca, cg, ak, bk, xj, xk; double aj, bj, tol, dif, epsa, epsb, bb; int jm1, kp1, km1, jp1; // first check whether Amatrix is defined if ( !a ) { _error("solveYourselfAt: unknown A matrix"); } // and whether Bmatrix if ( !b ) { _error("solveYourselfAt: unknown Bmatrx"); } if ( ( a->giveNumberOfRows() != b->giveNumberOfRows() ) || ( !a->isSquare() ) || ( !b->isSquare() ) ) { _error("solveYourselfAt: A matrix, B mtrix -> size mismatch"); } int n = a->giveNumberOfRows(); // // Check output arrays // if ( !eigv ) { _error("solveYourselfAt: unknown eigv array"); } if ( !x ) { _error("solveYourselfAt: unknown x mtrx "); } if ( eigv->giveSize() != n ) { _error("solveYourselfAt: eigv size mismatch"); } if ( ( !x->isSquare() ) || ( x->giveNumberOfRows() != n ) ) { _error("solveYourselfAt: x size mismatch"); } // // Create temporary arrays // FloatArray *d = new FloatArray(n); // // Initialize EigenValue and EigenVector Matrices // for ( i = 1; i <= n; i++ ) { // if((a->at(i,i) <= 0. ) && (b->at(i,i) <= 0.)) // _error ("solveYourselfAt: Matrices are not positive definite"); d->at(i) = a->at(i, i) / b->at(i, i); eigv->at(i) = d->at(i); } for ( i = 1; i <= n; i++ ) { for ( j = 1; j <= n; j++ ) { x->at(i, j) = 0.0; } x->at(i, i) = 1.0; } if ( n == 1 ) { return NM_Success; } // // Initialize sweep counter and begin iteration // nsweep = 0; nr = n - 1; do { nsweep++; # ifdef DETAILED_REPORT OOFEM_LOG_DEBUG("*GJacobi*: sweep number %4d\n", nsweep); #endif // // check if present off-diagonal element is large enough to require zeroing // eps = pow(0.01, ( double ) nsweep); eps *= eps; for ( j = 1; j <= nr; j++ ) { jj = j + 1; for ( k = jj; k <= n; k++ ) { eptola = ( a->at(j, k) * a->at(j, k) ) / ( a->at(j, j) * a->at(k, k) ); eptolb = ( b->at(j, k) * b->at(j, k) ) / ( b->at(j, j) * b->at(k, k) ); if ( ( eptola < eps ) && ( eptolb < eps ) ) { continue; } // // if zeroing is required, calculate the rotation matrix elements ca and cg // akk = a->at(k, k) * b->at(j, k) - b->at(k, k) * a->at(j, k); ajj = a->at(j, j) * b->at(j, k) - b->at(j, j) * a->at(j, k); ab = a->at(j, j) * b->at(k, k) - a->at(k, k) * b->at(j, j); check = ( ab * ab + 4.0 * akk * ajj ) / 4.0; if ( fabs(check) < GJacobi_ZERO_CHECK_TOL ) { check = fabs(check); } else if ( check < 0.0 ) { _error("solveYourselfAt: Matrices are not positive definite"); } sqch = sqrt(check); d1 = ab / 2. + sqch; d2 = ab / 2. - sqch; den = d1; if ( fabs(d2) > fabs(d1) ) { den = d2; } if ( den != 0.0 ) { // strange ! ca = akk / den; cg = -ajj / den; } else { ca = 0.0; cg = -a->at(j, k) / a->at(k, k); } // // perform the generalized rotation to zero // if ( ( n - 2 ) != 0 ) { jp1 = j + 1; jm1 = j - 1; kp1 = k + 1; km1 = k - 1; if ( ( jm1 - 1 ) >= 0 ) { for ( i = 1; i <= jm1; i++ ) { aj = a->at(i, j); bj = b->at(i, j); ak = a->at(i, k); bk = b->at(i, k); a->at(i, j) = aj + cg * ak; b->at(i, j) = bj + cg * bk; a->at(i, k) = ak + ca * aj; b->at(i, k) = bk + ca * bj; } } if ( ( kp1 - n ) <= 0 ) { for ( i = kp1; i <= n; i++ ) { // label 140 aj = a->at(j, i); bj = b->at(j, i); ak = a->at(k, i); bk = b->at(k, i); a->at(j, i) = aj + cg * ak; b->at(j, i) = bj + cg * bk; a->at(k, i) = ak + ca * aj; b->at(k, i) = bk + ca * bj; } } if ( ( jp1 - km1 ) <= 0 ) { // label 160 for ( i = jp1; i <= km1; i++ ) { aj = a->at(j, i); bj = b->at(j, i); ak = a->at(i, k); bk = b->at(i, k); a->at(j, i) = aj + cg * ak; b->at(j, i) = bj + cg * bk; a->at(i, k) = ak + ca * aj; b->at(i, k) = bk + ca * bj; } } } // label 190 ak = a->at(k, k); bk = b->at(k, k); a->at(k, k) = ak + 2.0 *ca *a->at(j, k) + ca *ca *a->at(j, j); b->at(k, k) = bk + 2.0 *ca *b->at(j, k) + ca *ca *b->at(j, j); a->at(j, j) = a->at(j, j) + 2.0 *cg *a->at(j, k) + cg * cg * ak; b->at(j, j) = b->at(j, j) + 2.0 *cg *b->at(j, k) + cg * cg * bk; a->at(j, k) = 0.0; b->at(j, k) = 0.0; // // update the eigenvector matrix after each rotation // for ( i = 1; i <= n; i++ ) { xj = x->at(i, j); xk = x->at(i, k); x->at(i, j) = xj + cg * xk; x->at(i, k) = xk + ca * xj; } // label 200 } } // label 210 // // update the eigenvalues after each sweep // #ifdef DETAILED_REPORT OOFEM_LOG_DEBUG("GJacobi: a,b after sweep\n"); a->printYourself(); b->printYourself(); #endif for ( i = 1; i <= n; i++ ) { // in original uncommented // if ((a->at(i,i) <= 0.) || (b->at(i,i) <= 0.)) // error ("solveYourselfAt: Matrices are not positive definite"); eigv->at(i) = a->at(i, i) / b->at(i, i); } // label 220 # ifdef DETAILED_REPORT OOFEM_LOG_DEBUG("GJacobi: current eigenvalues are:\n"); eigv->printYourself(); OOFEM_LOG_DEBUG("GJacobi: current eigenvectors are:\n"); x->printYourself(); # endif // // check for convergence // for ( i = 1; i <= n; i++ ) { // label 230 tol = rtol * d->at(i); dif = ( eigv->at(i) - d->at(i) ); if ( fabs(dif) > tol ) { goto label280; } } // label 240 // // check all off-diagonal elements to see if another sweep is required // eps = rtol * rtol; for ( j = 1; j <= nr; j++ ) { jj = j + 1; for ( k = jj; k <= n; k++ ) { epsa = ( a->at(j, k) * a->at(j, k) ) / ( a->at(j, j) * a->at(k, k) ); epsb = ( b->at(j, k) * b->at(j, k) ) / ( b->at(j, j) * b->at(k, k) ); if ( ( epsa < eps ) && ( epsb < eps ) ) { continue; } goto label280; } } // label 250 // // fill out bottom triangle of resultant matrices and scale eigenvectors // break; // goto label255 ; // // update d matrix and start new sweep, if allowed // label280: for ( i = 1; i <= n; i++ ) { d->at(i) = eigv->at(i); } } while ( nsweep < nsmax ); // label255: for ( i = 1; i <= n; i++ ) { for ( j = 1; j <= n; j++ ) { a->at(j, i) = a->at(i, j); b->at(j, i) = b->at(i, j); } // label 260 } for ( j = 1; j <= n; j++ ) { bb = sqrt( fabs( b->at(j, j) ) ); for ( k = 1; k <= n; k++ ) { x->at(k, j) /= bb; } } // label 270 solved = 1; delete d; return NM_Success; }
bool CommonSolverSparseLib::_solve(Matrix *mat, cplx *res) { _error("CommonSolverSparseLib::solve(Matrix *mat, cplx *res) not implemented."); }
/** * @details Where all the magic happens * @return Shouldn't return */ int main(void) { Init_Core(); Init_SM(); Init_Board(); Init_Globals(); Init_CAN(); Init_Timers(); // ------------------------------------------------ // Begin DEBUG_Print("Started Up\r\n"); while(1) { uint8_t count; if ((count = Chip_UART_Read(LPC_USART, Rx_Buf, UART_RX_BUF_SIZE)) != 0) { switch (Rx_Buf[0]) { case 'a': // Print out Brusa Mains Info DEBUG_Print("Actual Mains Voltage: "); itoa(brusa_actual_1.mains_mVolts, str, 10); DEBUG_Print(str); DEBUG_Print("\r\n"); DEBUG_Print("Mains type: "); itoa(brusa_actual_1.mains_cAmps, str, 10); DEBUG_Print(str); DEBUG_Print("\r\n"); DEBUG_Print("Temp: "); itoa((brusa_temp.power_temp / 10) - 40 , str, 10); DEBUG_Print(str); DEBUG_Print("\r\n"); DEBUG_Print("Temp: 0x"); itoa(brusa_temp.power_temp , str, 16); DEBUG_Print(str); DEBUG_Print("\r\n"); break; case 'b': // Print out Actual Brusa Output DEBUG_Print("Actual Out Voltage: "); itoa(brusa_actual_1.output_mVolts, str, 10); DEBUG_Println(str); DEBUG_Print("Actual Out Current: "); itoa(brusa_actual_1.output_cAmps, str, 10); DEBUG_Println(str); break; case 'f': // Print out Pack State itoa(pack_state.pack_min_mVolts, str, 10); DEBUG_Print("Pack Min Voltage: "); DEBUG_Print(str); DEBUG_Print("\r\n"); itoa(pack_state.pack_max_mVolts, str, 10); DEBUG_Print("Pack Max Voltage: "); DEBUG_Print(str); DEBUG_Print("\r\n"); break; case 'y': // Print out Module Balance State itoa(PackManager_GetExtModId(0), str, 16); DEBUG_Print("Mod 0x"); DEBUG_Print(str); itoa(PackManager_GetExtBal(0), str, 2); DEBUG_Print(": 0b"); DEBUG_Println(str); itoa(PackManager_GetExtModId(1), str, 16); DEBUG_Print("Mod 0x"); DEBUG_Print(str); itoa(PackManager_GetExtBal(1), str, 2); DEBUG_Print(": 0b"); DEBUG_Println(str); break; case 'e': itoa(brusa_error,str, 2); DEBUG_Println(str); break; case 'm': // Print out charge mode and brusa error DEBUG_Print("Charge Mode: "); itoa(Charge_GetMode(), str, 10); DEBUG_Println(str); DEBUG_Print("Error Messages: "); itoa((uint64_t)brusa_error, str, 2); DEBUG_Println(str); break; default: DEBUG_Print("Unknown Command\r\n"); } } //----------------------------- // Detect Input Changes (Default to IDLE) MODE_INPUT_T inp = INP_IDLE; if (!Board_Switch_Read()) { inp = INP_CHRG; } else { inp = INP_IDLE; } //----------------------------- // Update pack_state pack_state.contactors_closed = Board_Contactors_Closed(); pack_state.msTicks = msTicks; pack_state.brusa_error = brusa_error; pack_state.pack_cAmps_in = brusa_actual_1.output_cAmps; //----------------------------- // SSM Step ERROR_T result = SSM_Step(&pack_state, inp, &out_state); if (result != ERROR_NONE) { _error(result, true, false); } //----------------------------- // Check if SSM has Changed State // Currently only changes Poll Frequency // [TODO] Set a status LED!! if (SSM_GetMode() != mode) { mode = SSM_GetMode(); switch (SSM_GetMode()) { case IDLE: Chip_TIMER_SetMatch(LPC_TIMER32_1, 0, Hertz2Ticks(BCM_POLL_IDLE_FREQ)); Chip_TIMER_Reset(LPC_TIMER32_1); // Otherwise shit gets F****D break; case CHARGING: Chip_TIMER_SetMatch(LPC_TIMER32_1, 0, Hertz2Ticks(BCM_POLL_CHARGING_FREQ)); Chip_TIMER_Reset(LPC_TIMER32_1); break; case DRAINING: Chip_TIMER_SetMatch(LPC_TIMER32_1, 0, Hertz2Ticks(BCM_POLL_DRAINING_FREQ)); Chip_TIMER_Reset(LPC_TIMER32_1); break; } } //----------------------------- // Carry out out_state if (out_state.close_contactors && !Board_Contactors_Closed()) { Board_Close_Contactors(true); } else if (!out_state.close_contactors && Board_Contactors_Closed()) { Board_Close_Contactors(false); } if (out_state.brusa_output) { brusa_control.clear_error = out_state.brusa_clear_latch; brusa_control.output_mVolts = out_state.brusa_mVolts; brusa_control.output_cAmps = out_state.brusa_cAmps; Chip_TIMER_Enable(LPC_TIMER32_0); } else { brusa_control.output_mVolts = 0; brusa_control.output_cAmps = 0; Chip_TIMER_Disable(LPC_TIMER32_0); } //----------------------------- // Retrieve available brusa messages int8_t tmp = MCP2515_GetFullReceiveBuffer(); int8_t res = 0; if (tmp == 2) { MCP2515_ReadBuffer(&mcp_msg_obj, 0); res = Brusa_Decode(&brusa_messages, &mcp_msg_obj); if (res == -1) { DEBUG_Println("Brusa Decode Error"); res = 0; } MCP2515_ReadBuffer(&mcp_msg_obj, 1); res = Brusa_Decode(&brusa_messages, &mcp_msg_obj); } else if (tmp == 0) { // Receive Buffer 0 Full MCP2515_ReadBuffer(&mcp_msg_obj, tmp); res = Brusa_Decode(&brusa_messages, &mcp_msg_obj); } else if (tmp == 1) { //Receive buffer 1 full MCP2515_ReadBuffer(&mcp_msg_obj, tmp); res = Brusa_Decode(&brusa_messages, &mcp_msg_obj); } if (res == -1) { DEBUG_Println("Brusa Decode Error"); res = 0; } //----------------------------- // Send brusa message if its time if (brusa_message_send) { brusa_message_send = false; Brusa_MakeCTL(&brusa_control, &mcp_msg_obj); MCP2515_LoadBuffer(0, &mcp_msg_obj); MCP2515_SendBuffer(0); } //----------------------------- // Check for and decode A123 Messages if (!RingBuffer_IsEmpty(&rx_buffer)) { CCAN_MSG_OBJ_T temp_msg; RingBuffer_Pop(&rx_buffer, &temp_msg); res = PackManager_Update(&temp_msg); if (new_std_msg_sent) { PackManager_Commit(&pack_state); new_std_msg_sent = false; } } if (res == -1) { DEBUG_Println("A123 Decode Error"); } //----------------------------- // Timed output if (msTicks - last_debug_message > TIMED_MESSAGE_DELAY) { message_count++; last_debug_message = msTicks; switch (message_count % 7) { case 0: if (out_state.balance) { itoa(mbb_cmd.balance_target_mVolts, str, 10); DEBUG_Print("Balancing to: "); DEBUG_Println(str); } else { DEBUG_Println("Not balancing"); } break; case 1: itoa(brusa_control.output_mVolts, str, 10); DEBUG_Print("Brusa out V: "); DEBUG_Println(str); break; case 2: itoa(brusa_control.output_cAmps, str, 10); DEBUG_Print("Brusa out C: "); DEBUG_Println(str); break; case 3: DEBUG_Print("Actual Out Voltage: "); itoa(brusa_actual_1.output_mVolts, str, 10); DEBUG_Println(str); break; case 4: DEBUG_Print("Actual Out Current: "); itoa(brusa_actual_1.output_cAmps, str, 10); DEBUG_Println(str); break; case 5: DEBUG_Print("Mode: "); DEBUG_Println((SSM_GetMode() == CHARGING) ? "Chrg":"Idle"); break; case 6: DEBUG_Print("Brusa Output: "); itoa(out_state.brusa_output, str, 2); DEBUG_Println(str); DEBUG_Print("\r\n"); break; } } } return 0; }
void AsyncTask::onError() { if (!syncEvent(errorID, 0, 0)) _error(); }
void SIPAccount::serialize (Conf::YamlEmitter *emitter) { if(emitter == NULL) { _error("SIPAccount: Error: emitter is NULL in serialize"); return; } Conf::MappingNode accountmap (NULL); Conf::MappingNode srtpmap (NULL); Conf::MappingNode zrtpmap (NULL); Conf::MappingNode tlsmap (NULL); Conf::ScalarNode id (Account::_accountID); Conf::ScalarNode username (Account::_username); Conf::ScalarNode alias (Account::_alias); Conf::ScalarNode hostname (Account::_hostname); Conf::ScalarNode enable (_enabled); Conf::ScalarNode type (Account::_type); Conf::ScalarNode expire (_registrationExpire); Conf::ScalarNode interface (_interface); std::stringstream portstr; portstr << _localPort; Conf::ScalarNode port (portstr.str()); Conf::ScalarNode serviceRoute (_serviceRoute); Conf::ScalarNode mailbox (_mailBox); Conf::ScalarNode publishAddr (_publishedIpAddress); std::stringstream publicportstr; publicportstr << _publishedPort; Conf::ScalarNode publishPort (publicportstr.str()); Conf::ScalarNode sameasLocal (_publishedSameasLocal); Conf::ScalarNode resolveOnce (_resolveOnce); Conf::ScalarNode codecs (_codecStr); Conf::ScalarNode ringtonePath (_ringtonePath); Conf::ScalarNode ringtoneEnabled (_ringtoneEnabled); Conf::ScalarNode stunServer (_stunServer); Conf::ScalarNode stunEnabled (_stunEnabled); Conf::ScalarNode displayName (_displayName); Conf::ScalarNode dtmfType (_dtmfType==OVERRTP ? "overrtp" : "sipinfo"); std::stringstream countstr; countstr << 0; Conf::ScalarNode count (countstr.str()); Conf::ScalarNode srtpenabled (_srtpEnabled); Conf::ScalarNode keyExchange (_srtpKeyExchange); Conf::ScalarNode rtpFallback (_srtpFallback); Conf::ScalarNode displaySas (_zrtpDisplaySas); Conf::ScalarNode displaySasOnce (_zrtpDisplaySasOnce); Conf::ScalarNode helloHashEnabled (_zrtpHelloHash); Conf::ScalarNode notSuppWarning (_zrtpNotSuppWarning); Conf::ScalarNode tlsport (_tlsPortStr); Conf::ScalarNode certificate (_tlsCertificateFile); Conf::ScalarNode calist (_tlsCaListFile); Conf::ScalarNode ciphers (_tlsCiphers); Conf::ScalarNode tlsenabled (_tlsEnable); Conf::ScalarNode tlsmethod (_tlsMethod); Conf::ScalarNode timeout (_tlsNegotiationTimeoutSec); Conf::ScalarNode tlspassword (_tlsPassword); Conf::ScalarNode privatekey (_tlsPrivateKeyFile); Conf::ScalarNode requirecertif (_tlsRequireClientCertificate); Conf::ScalarNode server (_tlsServerName); Conf::ScalarNode verifyclient (_tlsVerifyServer); Conf::ScalarNode verifyserver (_tlsVerifyClient); accountmap.setKeyValue (aliasKey, &alias); accountmap.setKeyValue (typeKey, &type); accountmap.setKeyValue (idKey, &id); accountmap.setKeyValue (usernameKey, &username); accountmap.setKeyValue (hostnameKey, &hostname); accountmap.setKeyValue (accountEnableKey, &enable); accountmap.setKeyValue (mailboxKey, &mailbox); accountmap.setKeyValue (expireKey, &expire); accountmap.setKeyValue (interfaceKey, &interface); accountmap.setKeyValue (portKey, &port); accountmap.setKeyValue (stunServerKey, &stunServer); accountmap.setKeyValue (stunEnabledKey, &stunEnabled); accountmap.setKeyValue (publishAddrKey, &publishAddr); accountmap.setKeyValue (publishPortKey, &publishPort); accountmap.setKeyValue (sameasLocalKey, &sameasLocal); accountmap.setKeyValue (resolveOnceKey, &resolveOnce); accountmap.setKeyValue (serviceRouteKey, &serviceRoute); accountmap.setKeyValue (dtmfTypeKey, &dtmfType); accountmap.setKeyValue (displayNameKey, &displayName); accountmap.setKeyValue (codecsKey, &codecs); accountmap.setKeyValue (ringtonePathKey, &ringtonePath); accountmap.setKeyValue (ringtoneEnabledKey, &ringtoneEnabled); accountmap.setKeyValue (srtpKey, &srtpmap); srtpmap.setKeyValue (srtpEnableKey, &srtpenabled); srtpmap.setKeyValue (keyExchangeKey, &keyExchange); srtpmap.setKeyValue (rtpFallbackKey, &rtpFallback); accountmap.setKeyValue (zrtpKey, &zrtpmap); zrtpmap.setKeyValue (displaySasKey, &displaySas); zrtpmap.setKeyValue (displaySasOnceKey, &displaySasOnce); zrtpmap.setKeyValue (helloHashEnabledKey, &helloHashEnabled); zrtpmap.setKeyValue (notSuppWarningKey, ¬SuppWarning); Conf::SequenceNode credentialseq (NULL); accountmap.setKeyValue (credKey, &credentialseq); std::vector<std::map<std::string, std::string> >::const_iterator it; for (it = credentials_.begin(); it != credentials_.end(); ++it) { std::map<std::string, std::string> cred = *it; Conf::MappingNode *map = new Conf::MappingNode(NULL); map->setKeyValue(USERNAME, new Conf::ScalarNode(cred[USERNAME])); map->setKeyValue(PASSWORD, new Conf::ScalarNode(cred[PASSWORD])); map->setKeyValue(REALM, new Conf::ScalarNode(cred[REALM])); credentialseq.addNode(map); } accountmap.setKeyValue (tlsKey, &tlsmap); tlsmap.setKeyValue (tlsPortKey, &tlsport); tlsmap.setKeyValue (certificateKey, &certificate); tlsmap.setKeyValue (calistKey, &calist); tlsmap.setKeyValue (ciphersKey, &ciphers); tlsmap.setKeyValue (tlsEnableKey, &tlsenabled); tlsmap.setKeyValue (methodKey, &tlsmethod); tlsmap.setKeyValue (timeoutKey, &timeout); tlsmap.setKeyValue (tlsPasswordKey, &tlspassword); tlsmap.setKeyValue (privateKeyKey, &privatekey); tlsmap.setKeyValue (requireCertifKey, &requirecertif); tlsmap.setKeyValue (serverKey, &server); tlsmap.setKeyValue (verifyClientKey, &verifyclient); tlsmap.setKeyValue (verifyServerKey, &verifyserver); try { emitter->serializeAccount (&accountmap); } catch (Conf::YamlEmitterException &e) { _error ("ConfigTree: %s", e.what()); } Conf::Sequence *seq = credentialseq.getSequence(); Conf::Sequence::iterator seqit; for (seqit = seq->begin(); seqit != seq->end(); ++seqit) { Conf::MappingNode *node = (Conf::MappingNode*)*seqit; delete node->getValue(USERNAME); delete node->getValue(PASSWORD); delete node->getValue(REALM); delete node; } }
int RigidArmNode :: checkConsistency() // Checks internal data consistency in node. // Current implementation checks (when receiver has slave dofs) if receiver has the same // coordinate system as master dofManager of slave dof. // If requested, computes natural coordinates on element-like masternodes { int result = 1; int i, ndofs; Node *master; result = result && Node :: checkConsistency(); // finds master node master = dynamic_cast< Node * >( this->domain->giveDofManager(masterDofMngr) ); if ( !master ) { _warning2("checkConsistency: master dofManager is not compatible", 1); result = 0; } // check if receiver has the same coordinate system as master dofManager if ( !this->hasSameLCS(master) ) { _warning2("checkConsistency: different lcs for master/slave nodes", 1); result = 0; } // check if created DOFs (dofType) compatible with mastermask ndofs = master->giveNumberOfDofs(); for ( i = 1; i <= numberOfDofs; i++ ) { if ( masterMask->at(i) && ( dofArray [ i - 1 ]->giveClassID() == MasterDofClass ) ) { _error("checkConsistency: incompatible mastermask and doftype data"); } } // allocate for ( i = 1; i <= numberOfDofs; i++ ) { if ( masterDofID [ i - 1 ] ) { countOfMasterDofs->at(i) = 0; masterDofID [ i - 1 ]->resize(ndofs); masterContribution [ i - 1 ]->resize(ndofs); } } IntArray masterNodes(ndofs); masterNode = master; for ( i = 1; i <= ndofs; i++ ) { masterNodes.at(i) = master->giveNumber(); } result = result && computeMasterContribution(); // initialize slave dofs (inside check of consistency of receiver and master dof) for ( i = 1; i <= numberOfDofs; i++ ) { if ( dofArray [ i - 1 ]->giveClassID() == SlaveDofClass ) { ( ( SlaveDof * ) dofArray [ i - 1 ] )->initialize(countOfMasterDofs->at(i), masterNodes, masterDofID [ i - 1 ], *masterContribution [ i - 1 ]); } } /* * #ifdef __PARALLEL_MODE * // check if master in same mode * if ( parallel_mode != DofManager_local ) { * if ( ( * masterNode )->giveParallelMode() != parallel_mode ) { * _warning2("checkConsistency: mismatch in parallel mode of RigidArmNode and master", 1); * result = 0; * } * } * * #endif */ // deallocate auxiliary arrays deallocAuxArrays(); return result; }
void LuaEnvironment::Call(const char *func, const char *sig, ...) { va_list vl; int narg, nres; /* number of arguments and results */ va_start(vl, sig); lua_getglobal(L_, func); /* get function */ /* push arguments */ narg = 0; while (*sig) { /* push arguments */ switch (*sig++) { case 'd': /* double argument */ lua_pushnumber(L_, va_arg(vl, double)); break; case 'i': /* int argument */ lua_pushnumber(L_, va_arg(vl, int)); break; case 's': /* string argument */ lua_pushstring(L_, va_arg(vl, char *)); break; case '>': goto endwhile; default: _error(L_, "invalid option (%c)", *(sig - 1)); } narg++; luaL_checkstack(L_, 1, "too many arguments"); } endwhile: /* do the call */ nres = strlen(sig); /* number of expected results */ if (lua_pcall(L_, narg, nres, 0) != 0) /* do the call */ _error(L_, "error running function `%s': %s", func, lua_tostring(L_, -1)); /* retrieve results */ nres = -nres; /* stack index of first result */ while (*sig) { /* get results */ switch (*sig++) { case 'd': /* double result */ if (!lua_isnumber(L_, nres)) _error(L_, "wrong result type"); *va_arg(vl, double *) = lua_tonumber(L_, nres); break; case 'i': /* int result */ if (!lua_isnumber(L_, nres)) _error(L_, "wrong result type"); *va_arg(vl, int *) = (int)lua_tonumber(L_, nres); break; case 's': /* string result */ if (!lua_isstring(L_, nres)) _error(L_, "wrong result type"); *va_arg(vl, const char **) = lua_tostring(L_, nres); break; default: _error(L_, "invalid option (%c)", *(sig - 1)); } nres++; } va_end(vl); }
//nb_of_children int nb_of_children(Node* n) { if(n == NULL) _error(ONULL); return listlenght(&n->children); }
bool CommonSolverCG::_solve(Matrix* A, cplx *x) { _error("CommonSolverCG::solve(Matrix *mat, cplx *res) not implemented."); }
static PyObject* _cbson_do_batched_insert(PyObject* self, PyObject* args) { struct module_state *state = GETSTATE(self); /* NOTE just using a random number as the request_id */ int request_id = rand(); int send_safe, flags = 0; int length_location, message_length; int collection_name_length; char* collection_name = NULL; PyObject* docs; PyObject* doc; PyObject* iterator; PyObject* ctx; PyObject* last_error_args; PyObject* result; PyObject* max_bson_size_obj; PyObject* max_message_size_obj; PyObject* to_publish = NULL; unsigned char check_keys; unsigned char safe; unsigned char continue_on_error; codec_options_t options; unsigned char empty = 1; long max_bson_size; long max_message_size; buffer_t buffer; PyObject *exc_type = NULL, *exc_value = NULL, *exc_trace = NULL; if (!PyArg_ParseTuple(args, "et#ObbObO&O", "utf-8", &collection_name, &collection_name_length, &docs, &check_keys, &safe, &last_error_args, &continue_on_error, convert_codec_options, &options, &ctx)) { return NULL; } if (continue_on_error) { flags += 1; } /* * If we are doing unacknowledged writes *and* continue_on_error * is True it's pointless (and slower) to send GLE. */ send_safe = (safe || !continue_on_error); max_bson_size_obj = PyObject_GetAttrString(ctx, "max_bson_size"); #if PY_MAJOR_VERSION >= 3 max_bson_size = PyLong_AsLong(max_bson_size_obj); #else max_bson_size = PyInt_AsLong(max_bson_size_obj); #endif Py_XDECREF(max_bson_size_obj); if (max_bson_size == -1) { destroy_codec_options(&options); PyMem_Free(collection_name); return NULL; } max_message_size_obj = PyObject_GetAttrString(ctx, "max_message_size"); #if PY_MAJOR_VERSION >= 3 max_message_size = PyLong_AsLong(max_message_size_obj); #else max_message_size = PyInt_AsLong(max_message_size_obj); #endif Py_XDECREF(max_message_size_obj); if (max_message_size == -1) { destroy_codec_options(&options); PyMem_Free(collection_name); return NULL; } buffer = buffer_new(); if (!buffer) { destroy_codec_options(&options); PyErr_NoMemory(); PyMem_Free(collection_name); return NULL; } length_location = init_insert_buffer(buffer, request_id, flags, collection_name, collection_name_length); if (length_location == -1) { goto insertfail; } if (!(to_publish = PyList_New(0))) { goto insertfail; } iterator = PyObject_GetIter(docs); if (iterator == NULL) { PyObject* InvalidOperation = _error("InvalidOperation"); if (InvalidOperation) { PyErr_SetString(InvalidOperation, "input is not iterable"); Py_DECREF(InvalidOperation); } goto insertfail; } while ((doc = PyIter_Next(iterator)) != NULL) { int before = buffer_get_position(buffer); int cur_size; if (!write_dict(state->_cbson, buffer, doc, check_keys, &options, 1)) { goto iterfail; } cur_size = buffer_get_position(buffer) - before; if (cur_size > max_bson_size) { /* If we've encoded anything send it before raising. */ if (!empty) { buffer_update_position(buffer, before); message_length = buffer_get_position(buffer) - length_location; memcpy(buffer_get_buffer(buffer) + length_location, &message_length, 4); result = _send_insert(self, ctx, last_error_args, buffer, collection_name, collection_name_length, request_id, send_safe, &options, to_publish); if (!result) goto iterfail; Py_DECREF(result); } _set_document_too_large(cur_size, max_bson_size); goto iterfail; } empty = 0; /* We have enough data, send this batch. */ if (buffer_get_position(buffer) > max_message_size) { int new_request_id = rand(); int message_start; buffer_t new_buffer = buffer_new(); if (!new_buffer) { PyErr_NoMemory(); goto iterfail; } message_start = init_insert_buffer(new_buffer, new_request_id, flags, collection_name, collection_name_length); if (message_start == -1) { buffer_free(new_buffer); goto iterfail; } /* Copy the overflow encoded document into the new buffer. */ if (!buffer_write_bytes(new_buffer, (const char*)buffer_get_buffer(buffer) + before, cur_size)) { buffer_free(new_buffer); goto iterfail; } /* Roll back to the beginning of this document. */ buffer_update_position(buffer, before); message_length = buffer_get_position(buffer) - length_location; memcpy(buffer_get_buffer(buffer) + length_location, &message_length, 4); result = _send_insert(self, ctx, last_error_args, buffer, collection_name, collection_name_length, request_id, send_safe, &options, to_publish); buffer_free(buffer); buffer = new_buffer; request_id = new_request_id; length_location = message_start; Py_DECREF(to_publish); if (!(to_publish = PyList_New(0))) { goto insertfail; } if (!result) { PyObject *etype = NULL, *evalue = NULL, *etrace = NULL; PyObject* OperationFailure; PyErr_Fetch(&etype, &evalue, &etrace); OperationFailure = _error("OperationFailure"); if (OperationFailure) { if (PyErr_GivenExceptionMatches(etype, OperationFailure)) { if (!safe || continue_on_error) { Py_DECREF(OperationFailure); if (!safe) { /* We're doing unacknowledged writes and * continue_on_error is False. Just return. */ Py_DECREF(etype); Py_XDECREF(evalue); Py_XDECREF(etrace); Py_DECREF(to_publish); Py_DECREF(iterator); Py_DECREF(doc); buffer_free(buffer); PyMem_Free(collection_name); Py_RETURN_NONE; } /* continue_on_error is True, store the error * details to re-raise after the final batch */ Py_XDECREF(exc_type); Py_XDECREF(exc_value); Py_XDECREF(exc_trace); exc_type = etype; exc_value = evalue; exc_trace = etrace; if (PyList_Append(to_publish, doc) < 0) { goto iterfail; } Py_CLEAR(doc); continue; } } Py_DECREF(OperationFailure); } /* This isn't OperationFailure, we couldn't * import OperationFailure, or we are doing * acknowledged writes. Re-raise immediately. */ PyErr_Restore(etype, evalue, etrace); goto iterfail; } else { Py_DECREF(result); } } if (PyList_Append(to_publish, doc) < 0) { goto iterfail; } Py_CLEAR(doc); } Py_DECREF(iterator); if (PyErr_Occurred()) { goto insertfail; } if (empty) { PyObject* InvalidOperation = _error("InvalidOperation"); if (InvalidOperation) { PyErr_SetString(InvalidOperation, "cannot do an empty bulk insert"); Py_DECREF(InvalidOperation); } goto insertfail; } message_length = buffer_get_position(buffer) - length_location; memcpy(buffer_get_buffer(buffer) + length_location, &message_length, 4); /* Send the last (or only) batch */ result = _send_insert(self, ctx, last_error_args, buffer, collection_name, collection_name_length, request_id, safe, &options, to_publish); Py_DECREF(to_publish); PyMem_Free(collection_name); buffer_free(buffer); if (!result) { Py_XDECREF(exc_type); Py_XDECREF(exc_value); Py_XDECREF(exc_trace); return NULL; } else { Py_DECREF(result); } if (exc_type) { /* Re-raise any previously stored exception * due to continue_on_error being True */ PyErr_Restore(exc_type, exc_value, exc_trace); return NULL; } Py_RETURN_NONE; iterfail: Py_XDECREF(doc); Py_DECREF(iterator); insertfail: Py_XDECREF(exc_type); Py_XDECREF(exc_value); Py_XDECREF(exc_trace); Py_XDECREF(to_publish); buffer_free(buffer); PyMem_Free(collection_name); return NULL; }