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;
}
