/**************************************************************************** * drivers/sensors/wtgahrs2.c * Driver for the Wit-Motion WTGAHRS2 accelerometer, gyroscope, magnetic, * angle, barometer, temperature, gps sensors by serial interface with host * * Licensed to the Apache Software Foundation (ASF) under one or more * contributor license agreements. See the NOTICE file distributed with * this work for additional information regarding copyright ownership. The * ASF licenses this file to you under the Apache License, Version 2.0 (the * "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations * under the License. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #define WTGAHRS2_ARRAYSIZE(a) (sizeof((a))/sizeof(a[0])) #define WTGAHRS2_ACCEL_IDX 0 #define WTGAHRS2_GYRO_IDX 1 #define WTGAHRS2_MAG_IDX 2 #define WTGAHRS2_BARO_IDX 3 #define WTGAHRS2_GPS_IDX 4 #define WTGAHRS2_MAX_IDX 5 #define WTGAHRS2_GPS0_MASK (1 << 0) /* Time */ #define WTGAHRS2_GPS1_MASK (1 << 1) /* Longitude, Latitude */ #define WTGAHRS2_GPS2_MASK (1 << 2) /* Ground speed, Height, Yaw */ #define WTGAHRS2_GPS_MASK (7 << 0) #define WTGAHRS2_GPS0_INFO 0x50 #define WTGAHRS2_ACCEL_INFO 0x51 #define WTGAHRS2_GYRO_INFO 0x52 #define WTGAHRS2_MAG_INFO 0x54 #define WTGAHRS2_BARO_INFO 0x56 #define WTGAHRS2_GPS1_INFO 0x57 #define WTGAHRS2_GPS2_INFO 0x58 #define WTGAHRS2_RSP_HEADER 0x55 #define WTGAHRS2_RSP_LENGTH 11 #define WTGAHRS2_CMD_LENGTH 5 /**************************************************************************** * Private Types ****************************************************************************/ struct wtgahrs2_sensor_s { struct sensor_lowerhalf_s lower; unsigned int interval; unsigned int last_update; bool enable; }; struct wtgahrs2_dev_s { struct wtgahrs2_sensor_s dev[WTGAHRS2_MAX_IDX]; struct file file; struct sensor_event_gps gps; unsigned char gps_mask; }; /**************************************************************************** * Private ****************************************************************************/ /**************************************************************************** * Private Function Prototypes ****************************************************************************/ static int wtgahrs2_activate(FAR struct sensor_lowerhalf_s *lower, bool sw); static int wtgahrs2_set_interval(FAR struct sensor_lowerhalf_s *lower, FAR unsigned int *interval); /**************************************************************************** * Private Data ****************************************************************************/ /* in microseconds */ static const unsigned int g_wtgahrs2_interval[] = { 10000000, /* 0.1 hz */ 2000000, /* 0.5 hz */ 1000000, /* 1 hz */ 500000, /* 2 hz */ 200000, /* 5 hz */ 100000, /* 10 hz */ 50000, /* 20 hz */ 20000, /* 50 hz */ 10000, /* 100 hz */ 5000, /* 200 hz */ }; static const uint8_t g_wtgahrs2_unlock[] = { 0xff, 0xaa, 0x69, 0x88, 0xb5 }; static const uint8_t g_wtgahrs2_odr_200hz[] = { 0xff, 0xaa, 0x03, 0x0b, 0x00 }; static const uint8_t g_wtgahrs2_enable_sensor[] = { 0xff, 0xaa, 0x02, 0xd7, 0x05 }; static const struct sensor_ops_s g_wtgahrs2_ops = { .activate = wtgahrs2_activate, .set_interval = wtgahrs2_set_interval, .batch = NULL, }; /**************************************************************************** * Private Functions ****************************************************************************/ static void wtgahrs2_sendcmd(FAR struct wtgahrs2_dev_s *rtdata, const void *cmd) { file_write(&rtdata->file, cmd, WTGAHRS2_CMD_LENGTH); usleep(10000); } static int wtgahrs2_activate(FAR struct sensor_lowerhalf_s *lower, bool sw) { FAR struct wtgahrs2_sensor_s *dev = (FAR struct wtgahrs2_sensor_s *)lower; dev->enable = sw; return 0; } static int wtgahrs2_set_interval(FAR struct sensor_lowerhalf_s *lower, FAR unsigned int *interval) { FAR struct wtgahrs2_sensor_s *dev = (FAR struct wtgahrs2_sensor_s *)lower; int idx = 0; for (; idx < WTGAHRS2_ARRAYSIZE(g_wtgahrs2_interval) - 1; idx++) { if (*interval >= g_wtgahrs2_interval[idx]) { break; } } *interval = g_wtgahrs2_interval[idx]; dev->interval = *interval; return 0; } static void wtgahrs2_accel_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer) { FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_ACCEL_IDX]; FAR struct sensor_lowerhalf_s *lower = &dev->lower; uint64_t now = sensor_get_timestamp(); struct sensor_event_accel accel; if (!dev->enable || now - dev->last_update < dev->interval) { return; } dev->last_update = now; accel.timestamp = now; accel.x = (short)(buffer[1] << 8 | buffer[0]) * (16 * 9.8f / 32768); accel.y = (short)(buffer[3] << 8 | buffer[2]) * (16 * 9.8f / 32768); accel.z = (short)(buffer[5] << 8 | buffer[4]) * (16 * 9.8f / 32768); accel.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f; lower->push_event(lower->priv, &accel, sizeof(accel)); sninfo("Accel: %.3fm/s^2 %.3fm/s^2 %.3fm/s^2, t:%.1f\r\n", accel.x, accel.y, accel.z, accel.temperature); } static void wtgahrs2_gyro_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer) { FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_GYRO_IDX]; FAR struct sensor_lowerhalf_s *lower = &dev->lower; uint64_t now = sensor_get_timestamp(); struct sensor_event_gyro gyro; if (!dev->enable || now - dev->last_update < dev->interval) { return; } dev->last_update = now; gyro.timestamp = now; gyro.x = (short)(buffer[1] << 8 | buffer[0]) * (2000 * M_PI / 180 / 32768); gyro.y = (short)(buffer[3] << 8 | buffer[2]) * (2000 * M_PI / 180 / 32768); gyro.z = (short)(buffer[5] << 8 | buffer[4]) * (2000 * M_PI / 180 / 32768); gyro.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f; lower->push_event(lower->priv, &gyro, sizeof(gyro)); sninfo("Gyro: %.3frad/s %.3frad/s %.3frad/s, t:%.1f\r\n", gyro.x, gyro.y, gyro.z, gyro.temperature); } static void wtgahrs2_mag_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer) { FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_MAG_IDX]; FAR struct sensor_lowerhalf_s *lower = &dev->lower; uint64_t now = sensor_get_timestamp(); struct sensor_event_mag mag; if (!dev->enable || now - dev->last_update < dev->interval) { return; } dev->last_update = now; mag.timestamp = now; mag.x = (short)(buffer[1] << 8 | buffer[0]) * 0.16f; mag.y = (short)(buffer[3] << 8 | buffer[2]) * 0.16f; mag.z = (short)(buffer[5] << 8 | buffer[4]) * 0.16f; mag.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f; lower->push_event(lower->priv, &mag, sizeof(mag)); sninfo("Mag: %.3fuT %.3fuT %.3fuT, t:%.1f\r\n", mag.x, mag.y, mag.z, mag.temperature); } static void wtgahrs2_baro_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer) { FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_BARO_IDX]; FAR struct sensor_lowerhalf_s *lower = &dev->lower; uint64_t now = sensor_get_timestamp(); struct sensor_event_baro baro; if (!dev->enable || now - dev->last_update < dev->interval) { return; } dev->last_update = now; baro.timestamp = now; baro.pressure = (long)(buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]) / 100.0f; baro.temperature = NAN; lower->push_event(lower->priv, &baro, sizeof(baro)); sninfo("Pressure : %.3fhPa\r\n", baro.pressure); } static void wtgahrs2_gps_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer, int info_type) { FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_GPS_IDX]; FAR struct sensor_lowerhalf_s *lower = &dev->lower; uint64_t now = sensor_get_timestamp(); if (!dev->enable || now - dev->last_update < dev->interval) { return; } if (rtdata->gps_mask == 0) { dev->last_update = now; } switch (info_type) { case WTGAHRS2_GPS0_INFO: rtdata->gps_mask |= WTGAHRS2_GPS0_MASK; rtdata->gps.year = 2000 + buffer[0]; rtdata->gps.month = buffer[1]; rtdata->gps.day = buffer[2]; rtdata->gps.hour = buffer[3]; rtdata->gps.min = buffer[4]; rtdata->gps.sec = buffer[5]; rtdata->gps.msec = (buffer[7] << 8) | buffer[6]; break; case WTGAHRS2_GPS1_INFO: rtdata->gps_mask |= WTGAHRS2_GPS1_MASK; rtdata->gps.longitude = (long)(buffer[3] << 8 | buffer[2] << 8 | buffer[1] << 8 | buffer[0]) / 10000000.0f; rtdata->gps.latitude = (long)(buffer[7] << 8 | buffer[6] << 8 | buffer[5] << 8 | buffer[4]) / 10000000.0f; break; case WTGAHRS2_GPS2_INFO: rtdata->gps_mask |= WTGAHRS2_GPS2_MASK; rtdata->gps.height = (short)(buffer[1] << 8 | buffer[0]) / 10.0f; rtdata->gps.yaw = (short)(buffer[3] << 8 | buffer[2]) / 10.0f; rtdata->gps.speed = (long)(buffer[7] << 8 | buffer[6] << 8 | buffer[5] << 8 | buffer[4]) / 3600.0f; break; } if (rtdata->gps_mask == WTGAHRS2_GPS_MASK) { rtdata->gps_mask = 0; lower->push_event(lower->priv, &rtdata->gps, sizeof(rtdata->gps)); sninfo("Time : %d/%d/%d-%d:%d:%d\r\n", rtdata->gps.year, rtdata->gps.month, rtdata->gps.day, rtdata->gps.hour, rtdata->gps.min, rtdata->gps.sec); sninfo("GPS longitude : %fdegree, latitude:%fdegree\r\n", rtdata->gps.longitude, rtdata->gps.latitude); sninfo("GPS speed: %fm/s, yaw:%fdegrees, height:%fm \r\n", rtdata->gps.speed, rtdata->gps.yaw, rtdata->gps.height); } } static bool wtgahrs2_process_data(FAR struct wtgahrs2_dev_s *rtdata, FAR unsigned char *buffer) { unsigned char sum = 0; int i; /* calculate sum and verify checksum */ for (i = 0; i < WTGAHRS2_RSP_LENGTH - 1; i++) { sum += buffer[i]; } if (sum != buffer[WTGAHRS2_RSP_LENGTH - 1]) { return false; } switch (buffer[1]) { case WTGAHRS2_ACCEL_INFO: wtgahrs2_accel_data(rtdata, &buffer[2]); break; case WTGAHRS2_GYRO_INFO: wtgahrs2_gyro_data(rtdata, &buffer[2]); break; case WTGAHRS2_MAG_INFO: wtgahrs2_mag_data(rtdata, &buffer[2]); break; case WTGAHRS2_BARO_INFO: wtgahrs2_baro_data(rtdata, &buffer[2]); break; case WTGAHRS2_GPS0_INFO: case WTGAHRS2_GPS1_INFO: case WTGAHRS2_GPS2_INFO: wtgahrs2_gps_data(rtdata, &buffer[2], buffer[1]); break; } return true; } static int wtgahrs2_thread(int argc, FAR char *argv[]) { FAR struct wtgahrs2_dev_s *rtdata = (FAR struct wtgahrs2_dev_s *) ((uintptr_t)strtoul(argv[1], NULL, 0)); unsigned char buffer[8 * WTGAHRS2_RSP_LENGTH]; ssize_t count = 0; ssize_t pos; while (1) { count += file_read(&rtdata->file, buffer + count, sizeof(buffer) - count); for (pos = 0; pos < count; pos++) { if (buffer[pos] != WTGAHRS2_RSP_HEADER) { continue; } if (count - pos < WTGAHRS2_RSP_LENGTH) { memmove(buffer, buffer + pos, count - pos); break; } if (wtgahrs2_process_data(rtdata, &buffer[pos])) { pos += WTGAHRS2_RSP_LENGTH - 1; } } count -= pos; } return 0; } /**************************************************************************** * Public Functions ****************************************************************************/ int wtgahrs2_initialize(FAR const char *path, int devno) { FAR struct wtgahrs2_dev_s *rtdata; FAR struct wtgahrs2_sensor_s *tmp; #ifdef CONFIG_SERIAL_TERMIOS struct termios opt; #endif FAR char *argv[2]; char arg1[16]; int ret; if (!path) { snerr("Invalid path for serial interface\n"); return -EINVAL; } rtdata = kmm_zalloc(sizeof(struct wtgahrs2_dev_s)); if (!rtdata) { snerr("Memory cannot be allocated for wtgahrs2\n"); return -ENOMEM; } /* Open serial tty port and set baud rate */ ret = file_open(&rtdata->file, path, O_RDWR); if (ret < 0) { snerr("Failed to open wtgahrs2 serial:%s, err:%d", path, ret); goto open_err; } #ifdef CONFIG_SERIAL_TERMIOS file_ioctl(&rtdata->file, TCGETS, &opt); cfmakeraw(&opt); cfsetispeed(&opt, B115200); cfsetospeed(&opt, B115200); file_ioctl(&rtdata->file, TCSETS, &opt); #endif /* Accelerometer register */ tmp = &rtdata->dev[WTGAHRS2_ACCEL_IDX]; tmp->lower.ops = &g_wtgahrs2_ops; tmp->lower.type = SENSOR_TYPE_ACCELEROMETER; tmp->lower.buffer_number = 1; ret = sensor_register(&tmp->lower, devno); if (ret < 0) { goto accel_err; } /* Gyroscope register */ tmp = &rtdata->dev[WTGAHRS2_GYRO_IDX]; tmp->lower.ops = &g_wtgahrs2_ops; tmp->lower.type = SENSOR_TYPE_GYROSCOPE; tmp->lower.buffer_number = 1; ret = sensor_register(&tmp->lower, devno); if (ret < 0) { goto gyro_err; } /* Magnetic register */ tmp = &rtdata->dev[WTGAHRS2_MAG_IDX]; tmp->lower.ops = &g_wtgahrs2_ops; tmp->lower.type = SENSOR_TYPE_MAGNETIC_FIELD; tmp->lower.buffer_number = 1; ret = sensor_register(&tmp->lower, devno); if (ret < 0) { goto mag_err; } /* Barometer register */ tmp = &rtdata->dev[WTGAHRS2_BARO_IDX]; tmp->lower.ops = &g_wtgahrs2_ops; tmp->lower.type = SENSOR_TYPE_BAROMETER; tmp->lower.buffer_number = 1; ret = sensor_register(&tmp->lower, devno); if (ret < 0) { goto baro_err; } /* GPS register */ tmp = &rtdata->dev[WTGAHRS2_GPS_IDX]; tmp->lower.ops = &g_wtgahrs2_ops; tmp->lower.type = SENSOR_TYPE_GPS; tmp->lower.buffer_number = 1; ret = sensor_register(&tmp->lower, devno); if (ret < 0) { goto gps_err; } /* Set sensor default attributes and enter unlock mode */ wtgahrs2_sendcmd(rtdata, g_wtgahrs2_unlock); /* Set sensor default odr 200hz */ wtgahrs2_sendcmd(rtdata, g_wtgahrs2_odr_200hz); /* Enable all sensor */ wtgahrs2_sendcmd(rtdata, g_wtgahrs2_enable_sensor); snprintf(arg1, 16, "0x%" PRIxPTR, (uintptr_t)rtdata); argv[0] = arg1; argv[1] = NULL; ret = kthread_create("wtgahrs2_thread", SCHED_PRIORITY_DEFAULT, CONFIG_DEFAULT_TASK_STACKSIZE, wtgahrs2_thread, argv); if (ret < 0) { goto thr_err; } return ret; thr_err: sensor_unregister(&rtdata->dev[WTGAHRS2_GPS_IDX].lower, devno); gps_err: sensor_unregister(&rtdata->dev[WTGAHRS2_BARO_IDX].lower, devno); baro_err: sensor_unregister(&rtdata->dev[WTGAHRS2_MAG_IDX].lower, devno); mag_err: sensor_unregister(&rtdata->dev[WTGAHRS2_GYRO_IDX].lower, devno); gyro_err: sensor_unregister(&rtdata->dev[WTGAHRS2_ACCEL_IDX].lower, devno); accel_err: file_close(&rtdata->file); open_err: kmm_free(rtdata); return ret; }