SOL_API struct sol_mavlink * sol_mavlink_connect(const char *addr, const struct sol_mavlink_config *config, const void *data) { struct sol_mavlink *mavlink; struct sol_str_slice address; int port; int (*init) (struct sol_mavlink *mavlink); SOL_NULL_CHECK(addr, NULL); init = sol_mavlink_parse_addr_protocol(addr, &address, &port); SOL_NULL_CHECK(init, NULL); mavlink = calloc(1, sizeof(*mavlink)); SOL_NULL_CHECK(mavlink, NULL); mavlink->address = &address; SOL_NULL_CHECK_GOTO(mavlink->address, err); mavlink->port = port; SOL_NULL_CHECK_GOTO(mavlink->port, err); mavlink->config = config; mavlink->data = data; memset(&mavlink->curr_position, 0, sizeof(mavlink->curr_position)); memset(&mavlink->home_position, 0, sizeof(mavlink->home_position)); if (init(mavlink) < 0) { SOL_ERR("Could not initialize mavlink connection."); goto err; } mavlink->watch = sol_fd_add(mavlink->fd, SOL_FD_FLAGS_IN, sol_mavlink_fd_handler, mavlink); SOL_NULL_CHECK_GOTO(mavlink->watch, err); if (!setup_data_stream(mavlink)) { SOL_ERR("Could not setup data stream"); goto err; } return mavlink; err: sol_mavlink_free(mavlink); return NULL; }
void setup() { // minimal SPI bus config (cant have two devices being addressed at once) //PORTG |= (1<<0); // set AMC7812 CS pin high if connected digitalWrite(AMC7812_CS_ARDUINO_PIN, HIGH); // set AMC CS pin high digitalWrite(SS, HIGH); // set ENCJ CS pin high //pinMode(trigpin, INPUT); frontpanel_setup(); Serial.begin(115200); //Turn on Serial Port for debugging uint8_t ret = setup_DAQ(); frontpanel_set_led( COMM_LED, 1 ); setup_ethernet(); setup_ntp(); register_stream(); frontpanel_set_led( COMM_LED, 0 ); delay(1000); // increase stability frontpanel_set_led( COMM_LED, 1 ); setup_data_stream(); frontpanel_set_led( COMM_LED, 0 ); delay(1000); }
SOL_API struct sol_mavlink * sol_mavlink_connect(const char *addr, const struct sol_mavlink_config *config, const void *data) { struct sol_mavlink *mavlink; struct sol_str_slice address; int port; int (*init) (struct sol_mavlink *mavlink); SOL_NULL_CHECK(addr, NULL); SOL_NULL_CHECK(config, NULL); #ifndef SOL_NO_API_VERSION if (SOL_UNLIKELY(config->api_version != SOL_MAVLINK_CONFIG_API_VERSION)) { SOL_ERR("Unexpected API version (config is %" PRIu16 ", expected %" PRIu16 ")", config->api_version, SOL_MAVLINK_CONFIG_API_VERSION); return NULL; } SOL_NULL_CHECK(config->handlers, NULL); if (SOL_UNLIKELY(config->handlers->api_version != SOL_MAVLINK_HANDLERS_API_VERSION)) { SOL_ERR("Unexpected API version (config is %" PRIu16 ", expected %" PRIu16 ")", config->handlers->api_version, SOL_MAVLINK_HANDLERS_API_VERSION); return NULL; } #else SOL_NULL_CHECK(config->handlers, NULL); #endif init = sol_mavlink_parse_addr_protocol(addr, &address, &port); SOL_NULL_CHECK(init, NULL); mavlink = calloc(1, sizeof(*mavlink)); SOL_NULL_CHECK(mavlink, NULL); mavlink->address = &address; SOL_NULL_CHECK_GOTO(mavlink->address, err); mavlink->port = port; SOL_NULL_CHECK_GOTO(mavlink->port, err); mavlink->config = config; mavlink->data = data; memset(&mavlink->curr_position, 0, sizeof(mavlink->curr_position)); memset(&mavlink->home_position, 0, sizeof(mavlink->home_position)); if (init(mavlink) < 0) { SOL_ERR("Could not initialize mavlink connection."); goto err; } mavlink->watch = sol_fd_add(mavlink->fd, SOL_FD_FLAGS_IN, sol_mavlink_fd_handler, mavlink); SOL_NULL_CHECK_GOTO(mavlink->watch, err); if (!setup_data_stream(mavlink)) { SOL_ERR("Could not setup data stream"); goto err; } return mavlink; err: sol_mavlink_free(mavlink); return NULL; }