static void sol_mavlink_initial_status(struct sol_mavlink *mavlink, mavlink_message_t *msg) { uint8_t base_mode, type; enum sol_mavlink_mode mode; type = mavlink_msg_heartbeat_get_type(msg); if (!sol_mavlink_check_known_vehicle(type)) { SOL_INF("Unknown vehicle type, we'll retry on next heartbeat"); return; } mode = sol_mavlink_convert_mode(type, msg, &base_mode); if (mode == SOL_MAVLINK_MODE_UNKNOWN) { SOL_INF("Could not determine mode, we'll retry on next heartbeat"); return; } mavlink->mode = mode; mavlink->sysid = msg->sysid; mavlink->compid = msg->compid; mavlink->type = type; mavlink->base_mode = base_mode; mavlink->custom_mode_enabled = base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; mavlink->status |= SOL_MAVLINK_STATUS_INITIAL_SETUP; sol_mavlink_request_home_position(mavlink); }
static bool reply_cb(struct sol_coap_server *server, struct sol_coap_packet *req, const struct sol_network_link_addr *cliaddr, void *data) { struct sol_str_slice *path = data; static int count; struct sol_buffer *buf; size_t offset; SOL_BUFFER_DECLARE_STATIC(addr, SOL_INET_ADDR_STRLEN); if (!req || !cliaddr) //timeout return false; sol_network_link_addr_to_str(cliaddr, &addr); SOL_INF("Got response from %.*s\n", SOL_STR_SLICE_PRINT(sol_buffer_get_slice(&addr))); sol_coap_packet_get_payload(req, &buf, &offset); SOL_INF("Payload: %.*s\n", (int)(buf->used - offset), (char *)sol_buffer_at(buf, offset)); if (++count == 10) disable_observing(req, server, path, cliaddr); return true; }
static void on_fork(void *data) { unsigned i; static const char *daemon_possible_paths[] = { "/usr/libexec/bluetooth/bluetoothd", // fedora/yocto-style "/usr/lib/bluetooth/bluetoothd", // arch-style "/usr/sbin/bluetoothd" // debian-style }; const char *argv[] = { NULL, // waiting to be set "--nodetach", NULL }; static const char *envp[] = { "BLUETOOTH_SYSTEM_BUS_ADDRESS=unix:path=/run/dbus/system_bus_socket", NULL }; for (i = 0; i < SOL_UTIL_ARRAY_SIZE(daemon_possible_paths); i++) { argv[0] = daemon_possible_paths[i]; SOL_INF("attempting to exec %s", daemon_possible_paths[i]); execvpe(argv[0], (char *const *)argv, (char *const *)envp); } SOL_INF("bluetooth daemon executable not found, aborting"); sol_platform_linux_fork_run_exit(EXIT_FAILURE); }
static bool sol_mavlink_fd_handler(void *data, int fd, uint32_t cond) { struct sol_mavlink *mavlink = data; mavlink_message_t msg = { 0 }; mavlink_status_t status; uint8_t buf[MAVLINK_MAX_PACKET_LEN] = { 0 }; int i, res; res = recv(mavlink->fd, buf, MAVLINK_MAX_PACKET_LEN, 0); if (res == -1) { if (errno == EINTR) { SOL_INF("Could not read socket, retrying."); return true; } else { SOL_WRN("Could not read socket. %s", sol_util_strerrora(errno)); return false; } } for (i = 0; i < res; ++i) { if (!mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) continue; switch (msg.msgid) { case MAVLINK_MSG_ID_GPS_RAW_INT: sol_mavlink_position_handler(mavlink, &msg); break; case MAVLINK_MSG_ID_HEARTBEAT: sol_mavlink_heartbeat_handler(mavlink, &msg); break; case MAVLINK_MSG_ID_STATUSTEXT: sol_mavlink_statustext_handler(&msg); break; case MAVLINK_MSG_ID_HOME_POSITION: sol_mavlink_home_position_handler(mavlink, &msg); break; case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: sol_mavlink_mission_reached_handler(mavlink); break; default: SOL_INF("Unhandled event, msgid: %d", msg.msgid); } } if (mavlink->status == SOL_MAVLINK_STATUS_FULL_SETUP) { mavlink->status = SOL_MAVLINK_STATUS_READY; if (CHECK_HANDLER(mavlink, connect)) HANDLERS(mavlink)->connect((void *)mavlink->data, mavlink); } return true; }
static bool _load_mux(const char *name) { #ifdef ENABLE_DYNAMIC_MODULES int r; void *handle; char path[PATH_MAX], install_rootdir[PATH_MAX] = { 0 }; const struct sol_pin_mux *p_sym; r = sol_util_get_rootdir(install_rootdir, sizeof(install_rootdir)); SOL_INT_CHECK(r, >= (int)sizeof(install_rootdir), false); r = snprintf(path, sizeof(path), "%s%s/%s.so", install_rootdir, PINMUXDIR, name); SOL_INT_CHECK(r, >= (int)sizeof(path), false); SOL_INT_CHECK(r, < 0, false); handle = dlopen(path, RTLD_LAZY | RTLD_LOCAL | RTLD_NODELETE); if (!handle) { SOL_INF("Could not load platform pin multiplexer '%s': %s", path, dlerror()); return true; // Not find a mux isn't necessarily an error, so we are returning true here } p_sym = dlsym(handle, "SOL_PIN_MUX"); SOL_NULL_CHECK_MSG_GOTO(p_sym, error, "Could not find symbol SOL_PIN_MUX in module '%s': %s", path, dlerror()); #ifndef SOL_NO_API_VERSION if (p_sym->api_version != SOL_PIN_MUX_API_VERSION) { SOL_WRN("Mux '%s' has incorrect api_version: %lu expected %lu", path, p_sym->api_version, SOL_PIN_MUX_API_VERSION); goto error; } #endif if (dl_handle) dlclose(dl_handle); mux = p_sym; dl_handle = handle; SOL_INF("Loaded pin multiplexer '%s' from '%s'", mux->plat_name, path); return true; error: dlclose(handle); return false; #else return true; #endif }
static void disable_observing(struct sol_coap_packet *req, struct sol_coap_server *server, struct sol_str_slice path[], const struct sol_network_link_addr *cliaddr) { struct sol_coap_packet *pkt = sol_coap_packet_new(req); uint8_t observe = 1; int i, r; if (!pkt) return; r = sol_coap_header_set_code(pkt, SOL_COAP_METHOD_GET); SOL_INT_CHECK_GOTO(r, < 0, err); r = sol_coap_header_set_type(pkt, SOL_COAP_TYPE_CON); SOL_INT_CHECK_GOTO(r, < 0, err); r = sol_coap_add_option(pkt, SOL_COAP_OPTION_OBSERVE, &observe, sizeof(observe)); SOL_INT_CHECK_GOTO(r, < 0, err); for (i = 0; path[i].data; i++) sol_coap_add_option(pkt, SOL_COAP_OPTION_URI_PATH, path[i].data, path[i].len); sol_coap_send_packet(server, pkt, cliaddr); SOL_INF("Disabled observing"); return; err: sol_coap_packet_unref(pkt); }
static bool update_light(void *data) { struct light_context *context = data; struct sol_coap_server *server = context->server; struct sol_coap_resource *resource = context->resource; struct sol_coap_packet *pkt; struct sol_buffer *buf; int r; SOL_INF("Emitting notification"); pkt = sol_coap_packet_notification_new(server, resource); SOL_NULL_CHECK(pkt, false); r = sol_coap_header_set_code(pkt, SOL_COAP_RSPCODE_CONTENT); SOL_INT_CHECK_GOTO(r, < 0, err); r = sol_coap_packet_get_payload(pkt, &buf, NULL); SOL_INT_CHECK_GOTO(r, < 0, err); r = light_resource_to_rep(resource, get_scrolllock_led(), buf); SOL_INT_CHECK_GOTO(r, < 0, err); return !sol_coap_packet_send_notification(server, resource, pkt); err: sol_coap_packet_unref(pkt); return false; }
int main(int argc, char *argv[]) { struct sol_mqtt *mqtt; int port; sol_init(); if (argc < 4) { SOL_INF("Usage: %s <ip> <port> <topic>", argv[0]); return 0; } port = atoi(argv[2]); topic = argv[3]; mqtt = sol_mqtt_connect(argv[1], port, &config, NULL); if (!mqtt) { SOL_WRN("Unable to create MQTT session"); return -1; } sol_run(); sol_mqtt_disconnect(mqtt); sol_shutdown(); return 0; }
static void mavlink_connect_cb(void *data, struct sol_mavlink *mavlink) { int err; enum sol_mavlink_mode mode; SOL_INF("mavlink connection stablished"); mode = sol_mavlink_get_mode(mavlink); if (mode != SOL_MAVLINK_MODE_GUIDED) { err = sol_mavlink_set_mode(mavlink, SOL_MAVLINK_MODE_GUIDED); if (err < 0) { SOL_ERR("Could not set mode: %s", sol_util_strerrora(-err)); } return; } if (!sol_mavlink_check_armed(mavlink)) { err = sol_mavlink_set_armed(mavlink, true); if (err < 0) { SOL_ERR("Could not arm vechicle: %s", sol_util_strerrora(-err)); } return; } takeoff(mavlink); }
static void on_message(void *data, struct sol_mqtt *mqtt, const struct sol_mqtt_message *message) { SOL_NULL_CHECK(message); SOL_INF("%.*s", (int)message->payload->used, (char *)message->payload->data); }
/* * do things getty would do to spawn a shell, basically become the * session leader of the given tty, then make stdio/stdout/stderr use * it. */ static void do_shell(const char *tty) { char term_buf[128]; const char *envp[] = { term_buf, "HOME=/", NULL, }; char tty_path[PATH_MAX]; pid_t pid, tsid; int r; SOL_INF("no getty, exec shell: %s", shell); r = snprintf(term_buf, sizeof(term_buf), "TERM=%s", term ? term : get_term_for_tty(tty)); if (r < 0 || r >= (int)sizeof(term_buf)) envp[0] = "TERM=vt102"; pid = setsid(); if (pid < 0) { int fd; SOL_WRN("could not setsid(): %s", sol_util_strerrora(errno)); pid = getpid(); fd = open("/dev/tty", O_RDWR | O_NONBLOCK); if (fd >= 0) { sighandler_t oldsig; /* man:tty(4) * TIOCNOTTY: * Detach the calling process from its controlling terminal. * * If the process is the session leader, then SIGHUP and * SIGCONT signals are sent to the foreground process * group and all processes in the current session lose * their controlling tty. */ oldsig = signal(SIGHUP, SIG_IGN); ioctl(fd, TIOCNOTTY); close(fd); signal(SIGHUP, oldsig); } } r = snprintf(tty_path, sizeof(tty_path), "/dev/%s", tty); SOL_INT_CHECK_GOTO(r, < 0, end); SOL_INT_CHECK_GOTO(r, >= (int)sizeof(tty_path), end); close(STDIN_FILENO); r = open(tty_path, O_RDWR | O_NONBLOCK); SOL_INT_CHECK_GOTO(r, < 0, end); if (r != 0) { r = dup2(r, 0); SOL_INT_CHECK_GOTO(r, < 0, end); }
static void on_disconnect(void *user_data, struct sol_bt_conn *conn) { SOL_BUFFER_DECLARE_STATIC(str, SOL_BLUETOOTH_ADDR_STRLEN); sol_network_link_addr_to_str(sol_bt_conn_get_addr(conn), &str); SOL_INF("Disconnected from device %.*s, trying again", (int)str.used, (char *)str.data); }
static void enabled(void *data, bool powered) { if (!powered) return; SOL_INF("Bluetooth Adapter enabled"); scan = sol_bt_start_scan(SOL_BT_TRANSPORT_ALL, found_device, NULL); SOL_NULL_CHECK(scan); }
static bool on_connect(void *user_data, struct sol_bt_conn *conn) { SOL_BUFFER_DECLARE_STATIC(str, SOL_BLUETOOTH_ADDR_STRLEN); sol_network_link_addr_to_str(sol_bt_conn_get_addr(conn), &str); SOL_INF("Connected to device %.*s", (int)str.used, (char *)str.data); return true; }
static void on_fork(void *data) { const char *argv[] = { "/usr/bin/dbus-daemon", "--config-file=/etc/dbus-1/system.conf", "--nofork", NULL }; if (mkdir("/run/dbus", 0755) < 0 && errno != EEXIST) { SOL_WRN("could not create /run/dbus"); goto error; } if (access("/etc/dbus-1/system.conf", R_OK) < 0) { FILE *fp; SOL_INF("/etc/dbus-1/system.conf does not exist, create one as /run/dbus/system.conf"); fp = fopen("/run/dbus/system.conf", "we"); if (!fp) { SOL_WRN("could not create /run/dbus/system.conf: %s", sol_util_strerrora(errno)); goto error; } fputs("<!DOCTYPE busconfig PUBLIC \"-//freedesktop//DTD D-Bus Bus Configuration 1.0//EN\" \"http://www.freedesktop.org/standards/dbus/1.0/busconfig.dtd\">\n" "<busconfig>\n" "<type>system</type>\n" "<listen>unix:path=/run/dbus/system_bus_socket</listen>\n" "<policy context=\"default\">\n" "<allow user=\"*\"/>\n" "<allow own=\"*\"/>\n" "<allow send_type=\"method_call\"/>\n" "<allow send_type=\"signal\"/>\n" "<allow send_type=\"method_return\"/>\n" "<allow send_type=\"error\"/>\n" "<allow receive_type=\"method_call\"/>\n" "<allow receive_type=\"signal\"/>\n" "<allow receive_type=\"method_return\"/>\n" "<allow receive_type=\"error\"/>\n" "</policy>\n" "</busconfig>\n", fp); fclose(fp); argv[1] = "--config-file=/run/dbus/system.conf"; } execv(argv[0], (char *const *)argv); error: sol_platform_linux_fork_run_exit(EXIT_FAILURE); }
SOL_API int sol_util_iterate_dir(const char *path, enum sol_util_iterate_dir_reason (*iterate_dir_cb)(void *data, const char *dir_path, struct dirent *ent), const void *data) { DIR *dir; struct dirent *ent, *res; int r; long name_max; size_t len; SOL_NULL_CHECK(path, -EINVAL); SOL_NULL_CHECK(iterate_dir_cb, -EINVAL); /* See readdir_r(3) */ name_max = pathconf(path, _PC_NAME_MAX); if (name_max == -1) name_max = 255; len = offsetof(struct dirent, d_name) + name_max + 1; ent = malloc(len); SOL_NULL_CHECK(ent, -ENOMEM); dir = opendir(path); if (!dir) { int aux_errno = errno; SOL_INF("Could not open dir [%s] to iterate: %s", path, sol_util_strerrora(aux_errno)); free(ent); return -aux_errno; } r = readdir_r(dir, ent, &res); SOL_INT_CHECK_GOTO(r, != 0, exit); while (res) { if (!streq(res->d_name, ".") && !streq(res->d_name, "..")) { r = iterate_dir_cb((void *)data, path, res); SOL_INT_CHECK_GOTO(r, < 0, exit); if (r == SOL_UTIL_ITERATE_DIR_STOP) break; } r = readdir_r(dir, ent, &res); SOL_INT_CHECK_GOTO(r, != 0, exit); }
static int light_method_put(struct sol_coap_server *server, const struct sol_coap_resource *resource, struct sol_coap_packet *req, const struct sol_network_link_addr *cliaddr, void *data) { sol_coap_responsecode_t code = SOL_COAP_RSPCODE_CONTENT; struct sol_coap_packet *resp; struct sol_buffer *buf; char *sub = NULL; size_t offset; bool value; int r; sol_coap_packet_get_payload(req, &buf, &offset); if (buf) sub = strstr((char *)sol_buffer_at(buf, offset), "state\":"); if (!sub) { code = SOL_COAP_RSPCODE_BAD_REQUEST; goto done; } value = !memcmp(sub + strlen("state\":"), "true", sizeof("true") - 1); SOL_INF("Changing light state to %s", value ? "on" : "off"); set_scrolllock_led(value); done: resp = sol_coap_packet_new(req); if (!resp) { SOL_WRN("Could not build response packet"); return -1; } r = sol_coap_header_set_type(resp, SOL_COAP_TYPE_ACK); SOL_INT_CHECK_GOTO(r, < 0, err); r = sol_coap_header_set_code(resp, code); SOL_INT_CHECK_GOTO(r, < 0, err); return sol_coap_send_packet(server, resp, cliaddr); err: sol_coap_packet_unref(resp); return r; }
static bool _find_mux(const char *name) { #if (SOL_PIN_MUX_BUILTIN_COUNT > 0) unsigned int i; const struct sol_pin_mux *_mux; for (i = 0; i < SOL_PIN_MUX_BUILTIN_COUNT; i++) { _mux = SOL_PIN_MUX_BUILTINS_ALL[i]; if (streq(_mux->plat_name, name)) { mux = _mux; SOL_INF("Loaded built-in pin multiplexer '%s'", mux->plat_name); return true; } } #endif return false; }
bool sol_util_iterate_dir(const char *path, bool (*iterate_dir_cb)(void *data, const char *dir_path, struct dirent *ent), const void *data) { DIR *dir; struct dirent *ent, *res; int success; long name_max; size_t len; bool result = false; SOL_NULL_CHECK(path, false); SOL_NULL_CHECK(iterate_dir_cb, false); /* See readdir_r(3) */ name_max = pathconf(path, _PC_NAME_MAX); if (name_max == -1) name_max = 255; len = offsetof(struct dirent, d_name) + name_max + 1; ent = malloc(len); SOL_NULL_CHECK(ent, false); dir = opendir(path); if (!dir) { SOL_INF("Could not open dir [%s] to iterate: %s", path, sol_util_strerrora(errno)); free(ent); return false; } success = readdir_r(dir, ent, &res); while (success == 0 && res) { if (iterate_dir_cb((void *)data, path, res)) { result = true; break; } success = readdir_r(dir, ent, &res); } free(ent); closedir(dir); return result; }
SOL_API struct sol_gpio * sol_gpio_open_by_label(const char *label, const struct sol_gpio_config *config) { uint32_t pin; _log_init(); #ifdef USE_PIN_MUX if (!sol_pin_mux_map(label, SOL_IO_GPIO, &pin)) return sol_gpio_open(pin, config); SOL_WRN("Label '%s' couldn't be mapped or can't be used as GPIO", label); #else SOL_INF("Pin Multiplexer support is necessary to open a 'board pin'."); (void)pin; #endif return NULL; }
SOL_API struct sol_aio * sol_aio_open_by_label(const char *label, unsigned int precision) { #ifdef USE_PIN_MUX int device, pin; #endif _log_init(); #ifdef USE_PIN_MUX if (!sol_pin_mux_map(label, SOL_IO_AIO, &device, &pin)) return sol_aio_open(device, pin, precision); SOL_WRN("Label '%s' couldn't be mapped or can't be used as Analog I/O", label); #else SOL_INF("Pin Multiplexer support is necessary to open a 'board pin'."); #endif return NULL; }
SOL_API struct sol_pwm * sol_pwm_open_by_label(const char *label, const struct sol_pwm_config *config) { int device, channel; _log_init(); #ifdef USE_PIN_MUX if (!sol_pin_mux_map(label, SOL_IO_PWM, &device, &channel)) return sol_pwm_open(device, channel, config); SOL_WRN("Label '%s' couldn't be mapped or can't be used as PWM", label); #else SOL_INF("Pin Multiplexer support is necessary to open a 'board pin'."); (void)device; (void)channel; #endif return NULL; }
static int light_method_put(const struct sol_coap_resource *resource, struct sol_coap_packet *req, const struct sol_network_link_addr *cliaddr, void *data) { struct sol_coap_server *server = (void *)data; struct sol_coap_packet *resp; char *sub = NULL; uint8_t *p; uint16_t len; bool value; sol_coap_responsecode_t code = SOL_COAP_RSPCODE_CONTENT; sol_coap_packet_get_payload(req, &p, &len); if (p) sub = strstr((char *)p, "state\":"); if (!sub) { code = SOL_COAP_RSPCODE_BAD_REQUEST; goto done; } value = !memcmp(sub + strlen("state\":"), "true", sizeof("true") - 1); SOL_INF("Changing light state to %s", value ? "on" : "off"); set_scrolllock_led(value); done: resp = sol_coap_packet_new(req); if (!resp) { SOL_WRN("Could not build response packet"); return -1; } sol_coap_header_set_type(resp, SOL_COAP_TYPE_ACK); sol_coap_header_set_code(resp, code); return sol_coap_send_packet(server, resp, cliaddr); }
static bool update_light(void *data) { struct light_context *context = data; struct sol_coap_server *server = context->server; struct sol_coap_resource *resource = context->resource; struct sol_coap_packet *pkt; uint8_t *payload; uint16_t len; SOL_INF("Emitting notification"); pkt = sol_coap_packet_notification_new(server, resource); SOL_NULL_CHECK(pkt, false); sol_coap_header_set_code(pkt, SOL_COAP_RSPCODE_CONTENT); sol_coap_packet_get_payload(pkt, &payload, &len); len = light_resource_to_rep(resource, get_scrolllock_led(), (char *)payload, len); sol_coap_packet_set_payload_used(pkt, len); return !sol_coap_packet_send_notification(server, resource, pkt); }
static void i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S; uint8_t num_samples_available; struct sol_direction_vector val = { .min = -GYRO_RANGE, .max = GYRO_RANGE, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } num_samples_available = status / (sizeof(int16_t) * 3); /* raw readings, with only the sensor-provided filtering */ for (uint8_t i = 0; i < num_samples_available; i++) { mdata->reading[0] = mdata->gyro_data.buffer[i][0] * scale; mdata->reading[1] = -mdata->gyro_data.buffer[i][1] * scale; mdata->reading[2] = -mdata->gyro_data.buffer[i][2] * scale; } sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val); mdata->pending_ticks--; if (mdata->pending_ticks) gyro_tick_do(mdata); } static void i2c_read_fifo_status_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; uint8_t num_samples_available; uint8_t fifo_status = mdata->common.buffer[0]; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) { num_samples_available = 32; } else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) { num_samples_available = 0; } else { num_samples_available = fifo_status & GYRO_REG_FIFO_SRC_ENTRIES_MASK; } if (!num_samples_available) { SOL_INF("No samples available"); return; } SOL_DBG("%d samples available", num_samples_available); /* Read *all* the entries in one go, using AUTO_INCREMENT */ mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT, (uint8_t *)&mdata->gyro_data.buffer[0][0], sizeof(mdata->gyro_data.buffer), i2c_read_data_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro samples"); } static bool set_slave(struct gyroscope_l3g4200d_data *mdata, bool (*cb)(void *data)) { int r; r = sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS); if (r < 0) { if (r == -EBUSY) gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, cb); else { const char errmsg[] = "Failed to set slave at address 0x%02x"; SOL_WRN(errmsg, GYRO_ADDRESS); sol_flow_send_error_packet(mdata->node, r, errmsg, GYRO_ADDRESS); } return false; } return true; }
static void gyro_read(struct gyroscope_l3g4200d_data *mdata) { uint8_t num_samples_available; uint8_t fifo_status = 0; int r; if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return; } fifo_status = 0; r = sol_i2c_read_register(mdata->i2c, GYRO_REG_FIFO_SRC, &fifo_status, 1); if (r <= 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) { num_samples_available = 32; } else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) { num_samples_available = 0; } else { num_samples_available = fifo_status & GYRO_REG_FIFO_SRC_ENTRIES_MASK; } if (!num_samples_available) { SOL_INF("No samples available"); return; } SOL_DBG("%d samples available", num_samples_available); { /* Read *all* the entries in one go, using AUTO_INCREMENT. * int16_t and 3 entries because of x, y and z axis are read, * each consisting of L + H byte parts */ int16_t buffer[num_samples_available][3]; double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S; r = sol_i2c_read_register(mdata->i2c, GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT, (uint8_t *)&buffer[0][0], sizeof(buffer)); if (r <= 0) { SOL_WRN("Failed to read L3G4200D gyro samples"); return; } /* raw readings, with only the sensor-provided filtering */ for (uint8_t i = 0; i < num_samples_available; i++) { mdata->reading[0] = buffer[i][0] * scale; mdata->reading[1] = -buffer[i][1] * scale; mdata->reading[2] = -buffer[i][2] * scale; } } }
static void i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S; uint8_t num_samples_available; struct sol_direction_vector val = { .min = -GYRO_RANGE, .max = GYRO_RANGE, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } num_samples_available = status / (sizeof(int16_t) * 3); /* raw readings, with only the sensor-provided filtering */ for (uint8_t i = 0; i < num_samples_available; i++) { mdata->reading[0] = mdata->gyro_data.buffer[i][0] * scale; mdata->reading[1] = -mdata->gyro_data.buffer[i][1] * scale; mdata->reading[2] = -mdata->gyro_data.buffer[i][2] * scale; } sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val); mdata->pending_ticks--; if (mdata->pending_ticks) gyro_tick_do(mdata); } static void i2c_read_fifo_status_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; uint8_t num_samples_available; uint8_t fifo_status = mdata->common.buffer[0]; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) { num_samples_available = 32; } else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) { num_samples_available = 0; } else { num_samples_available = fifo_status & GYRO_REG_FIFO_SRC_ENTRIES_MASK; } if (!num_samples_available) { SOL_INF("No samples available"); return; } SOL_DBG("%d samples available", num_samples_available); /* Read *all* the entries in one go, using AUTO_INCREMENT */ mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT, (uint8_t *)&mdata->gyro_data.buffer[0][0], sizeof(mdata->gyro_data.buffer), i2c_read_data_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro samples"); } static bool gyro_tick_do(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_tick_do); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = 0; mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_FIFO_SRC, mdata->common.buffer, 1, i2c_read_fifo_status_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro fifo status"); return false; } static bool gyro_ready(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->ready = true; SOL_DBG("gyro is ready for reading"); if (mdata->pending_ticks) gyro_tick_do(mdata); return false; } static void i2c_write_fifo_ctl_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_stream(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* enable FIFO in stream mode */ mdata->common.buffer[0] = GYRO_REG_FIFO_CTL_STREAM; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, mdata->common.buffer, 1, i2c_write_fifo_ctl_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return false; } static void i2c_write_ctrl_reg5_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_fifo(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = GYRO_REG_CTRL_REG5_FIFO_EN; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, mdata->common.buffer, 1, i2c_write_ctrl_reg5_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return false; } static void i2c_write_ctrl_reg4_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_range(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_range); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 2000 degrees/sec */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG4_FS_2000; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, mdata->common.buffer, 1, i2c_write_ctrl_reg4_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return false; } static bool gyro_init_sampling(void *data); static void i2c_write_ctrl_reg1_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return; } mdata->init_sampling_cnt--; if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, mdata->init_sampling_cnt ? gyro_init_sampling : gyro_init_range) < 0) { SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } } /* meant to run 3 times */ static bool gyro_init_sampling(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 800Hz sampling with 110Hz filter */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG1_DRBW_800_110 | GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, mdata->common.buffer, 1, i2c_write_ctrl_reg1_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return false; } static void i2c_read_who_am_i_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read i2c register"); return; } if (mdata->common.buffer[0] != GYRO_REG_WHO_AM_I_VALUE) { SOL_WRN("could not find L3G4200D gyro sensor"); return; } gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); } static bool gyro_init(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_WHO_AM_I, mdata->common.buffer, 1, i2c_read_who_am_i_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read i2c register"); return false; } static int gyroscope_l3g4200d_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct gyroscope_l3g4200d_data *mdata = data; const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts = (const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options; SOL_NULL_CHECK(options, -EINVAL); mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED); SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus"); mdata->use_rad = opts->output_radians; mdata->init_sampling_cnt = 3; mdata->node = node; gyro_init(mdata); return 0; } static void gyroscope_l3g4200d_close(struct sol_flow_node *node, void *data) { struct gyroscope_l3g4200d_data *mdata = data; if (mdata->i2c_pending) sol_i2c_pending_cancel(mdata->i2c, mdata->i2c_pending); if (mdata->i2c) sol_i2c_close(mdata->i2c); if (mdata->timer) sol_timeout_del(mdata->timer); } static int gyroscope_l3g4200d_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct gyroscope_l3g4200d_data *mdata = data; if (!mdata->ready || mdata->pending_ticks) { mdata->pending_ticks++; return 0; } gyro_tick_do(mdata); return 0; }
static void on_disconnect(void *data, struct sol_mqtt *mqtt) { SOL_INF("Reconnecting..."); sol_timeout_add(1000, try_reconnect, mqtt); }
static int sol_mavlink_init_serial(struct sol_mavlink *mavlink) { struct termios tty = { 0 }; char *portname; int baud_rate; baud_rate = mavlink->config->baud_rate; if (!baud_rate) { baud_rate = 115200; SOL_INF("No baud_rate config provided, setting default: 115200"); } portname = sol_str_slice_to_str(*mavlink->address); if (!portname) { SOL_ERR("Could not format portname string - %s", sol_util_strerrora(errno)); return -errno; } mavlink->fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC | O_CLOEXEC); if (mavlink->fd == -1) { SOL_ERR("Could not open serial port: %s - %s", portname, sol_util_strerrora(errno)); goto err; } if (tcgetattr(mavlink->fd, &tty) != 0) { SOL_ERR("Could not read serial attr: %s", sol_util_strerrora(errno)); goto attr_err; } if (cfsetospeed(&tty, baud_rate) == -1) { SOL_ERR("Could not set serial output speed - %s", sol_util_strerrora(errno)); goto attr_err; } if (cfsetispeed(&tty, baud_rate) == -1) { SOL_ERR("Could not set serial input speed - %s", sol_util_strerrora(errno)); goto attr_err; } tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; tty.c_iflag &= ~IGNBRK; tty.c_lflag = 0; tty.c_oflag = 0; tty.c_cc[VMIN] = 0; tty.c_iflag &= ~(IXON | IXOFF | IXANY); tty.c_cflag |= (CLOCAL | CREAD); tty.c_cflag &= ~(PARENB | PARODD); tty.c_cflag |= 0; tty.c_cflag &= ~CSTOPB; tty.c_cflag &= ~CRTSCTS; if (tcsetattr(mavlink->fd, TCSANOW, &tty) != 0) { SOL_ERR("Could not set serial attr: %s", sol_util_strerrora(errno)); goto attr_err; } free(portname); return 0; attr_err: close(mavlink->fd); err: free(portname); return -errno; }