From 5974c37abba562c1372beb04490014db274fd175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 14:06:23 +0200 Subject: Moved the bulk of sensor drivers to the new world --- apps/drivers/blinkm/Makefile | 42 - apps/drivers/blinkm/blinkm.cpp | 919 -------------- apps/drivers/bma180/Makefile | 42 - apps/drivers/bma180/bma180.cpp | 929 -------------- apps/drivers/gps/Makefile | 42 - apps/drivers/gps/gps.cpp | 536 -------- apps/drivers/gps/gps_helper.cpp | 92 -- apps/drivers/gps/gps_helper.h | 52 - apps/drivers/gps/mtk.cpp | 274 ---- apps/drivers/gps/mtk.h | 124 -- apps/drivers/gps/ubx.cpp | 755 ----------- apps/drivers/gps/ubx.h | 395 ------ apps/drivers/hil/Makefile | 42 - apps/drivers/hil/hil.cpp | 851 ------------- apps/drivers/hmc5883/Makefile | 42 - apps/drivers/hmc5883/hmc5883.cpp | 1447 ---------------------- apps/drivers/mb12xx/Makefile | 42 - apps/drivers/mb12xx/mb12xx.cpp | 840 ------------- apps/drivers/mpu6000/Makefile | 42 - apps/drivers/mpu6000/mpu6000.cpp | 1219 ------------------ apps/drivers/ms5611/Makefile | 42 - apps/drivers/ms5611/ms5611.cpp | 1214 ------------------ apps/hott_telemetry/Makefile | 45 - apps/hott_telemetry/hott_telemetry_main.c | 312 ----- apps/hott_telemetry/messages.c | 87 -- apps/hott_telemetry/messages.h | 123 -- makefiles/config_px4fmu_default.mk | 17 +- src/drivers/blinkm/blinkm.cpp | 919 ++++++++++++++ src/drivers/blinkm/module.mk | 40 + src/drivers/bma180/bma180.cpp | 929 ++++++++++++++ src/drivers/bma180/module.mk | 40 + src/drivers/gps/gps.cpp | 536 ++++++++ src/drivers/gps/gps_helper.cpp | 92 ++ src/drivers/gps/gps_helper.h | 52 + src/drivers/gps/module.mk | 43 + src/drivers/gps/mtk.cpp | 274 ++++ src/drivers/gps/mtk.h | 124 ++ src/drivers/gps/ubx.cpp | 755 +++++++++++ src/drivers/gps/ubx.h | 395 ++++++ src/drivers/hil/hil.cpp | 851 +++++++++++++ src/drivers/hil/module.mk | 40 + src/drivers/hmc5883/hmc5883.cpp | 1447 ++++++++++++++++++++++ src/drivers/hmc5883/module.mk | 43 + src/drivers/hott_telemetry/hott_telemetry_main.c | 306 +++++ src/drivers/hott_telemetry/messages.c | 87 ++ src/drivers/hott_telemetry/messages.h | 124 ++ src/drivers/hott_telemetry/module.mk | 41 + src/drivers/mb12xx/mb12xx.cpp | 840 +++++++++++++ src/drivers/mb12xx/module.mk | 40 + src/drivers/mpu6000/module.mk | 43 + src/drivers/mpu6000/mpu6000.cpp | 1219 ++++++++++++++++++ src/drivers/ms5611/module.mk | 40 + src/drivers/ms5611/ms5611.cpp | 1214 ++++++++++++++++++ 53 files changed, 10543 insertions(+), 10558 deletions(-) delete mode 100644 apps/drivers/blinkm/Makefile delete mode 100644 apps/drivers/blinkm/blinkm.cpp delete mode 100644 apps/drivers/bma180/Makefile delete mode 100644 apps/drivers/bma180/bma180.cpp delete mode 100644 apps/drivers/gps/Makefile delete mode 100644 apps/drivers/gps/gps.cpp delete mode 100644 apps/drivers/gps/gps_helper.cpp delete mode 100644 apps/drivers/gps/gps_helper.h delete mode 100644 apps/drivers/gps/mtk.cpp delete mode 100644 apps/drivers/gps/mtk.h delete mode 100644 apps/drivers/gps/ubx.cpp delete mode 100644 apps/drivers/gps/ubx.h delete mode 100644 apps/drivers/hil/Makefile delete mode 100644 apps/drivers/hil/hil.cpp delete mode 100644 apps/drivers/hmc5883/Makefile delete mode 100644 apps/drivers/hmc5883/hmc5883.cpp delete mode 100644 apps/drivers/mb12xx/Makefile delete mode 100644 apps/drivers/mb12xx/mb12xx.cpp delete mode 100644 apps/drivers/mpu6000/Makefile delete mode 100644 apps/drivers/mpu6000/mpu6000.cpp delete mode 100644 apps/drivers/ms5611/Makefile delete mode 100644 apps/drivers/ms5611/ms5611.cpp delete mode 100644 apps/hott_telemetry/Makefile delete mode 100644 apps/hott_telemetry/hott_telemetry_main.c delete mode 100644 apps/hott_telemetry/messages.c delete mode 100644 apps/hott_telemetry/messages.h create mode 100644 src/drivers/blinkm/blinkm.cpp create mode 100644 src/drivers/blinkm/module.mk create mode 100644 src/drivers/bma180/bma180.cpp create mode 100644 src/drivers/bma180/module.mk create mode 100644 src/drivers/gps/gps.cpp create mode 100644 src/drivers/gps/gps_helper.cpp create mode 100644 src/drivers/gps/gps_helper.h create mode 100644 src/drivers/gps/module.mk create mode 100644 src/drivers/gps/mtk.cpp create mode 100644 src/drivers/gps/mtk.h create mode 100644 src/drivers/gps/ubx.cpp create mode 100644 src/drivers/gps/ubx.h create mode 100644 src/drivers/hil/hil.cpp create mode 100644 src/drivers/hil/module.mk create mode 100644 src/drivers/hmc5883/hmc5883.cpp create mode 100644 src/drivers/hmc5883/module.mk create mode 100644 src/drivers/hott_telemetry/hott_telemetry_main.c create mode 100644 src/drivers/hott_telemetry/messages.c create mode 100644 src/drivers/hott_telemetry/messages.h create mode 100644 src/drivers/hott_telemetry/module.mk create mode 100644 src/drivers/mb12xx/mb12xx.cpp create mode 100644 src/drivers/mb12xx/module.mk create mode 100644 src/drivers/mpu6000/module.mk create mode 100644 src/drivers/mpu6000/mpu6000.cpp create mode 100644 src/drivers/ms5611/module.mk create mode 100644 src/drivers/ms5611/ms5611.cpp diff --git a/apps/drivers/blinkm/Makefile b/apps/drivers/blinkm/Makefile deleted file mode 100644 index 5a623693d..000000000 --- a/apps/drivers/blinkm/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# BlinkM I2C LED driver -# - -APPNAME = blinkm -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp deleted file mode 100644 index 3fabfd9a5..000000000 --- a/apps/drivers/blinkm/blinkm.cpp +++ /dev/null @@ -1,919 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file blinkm.cpp - * - * Driver for the BlinkM LED controller connected via I2C. - * - * Connect the BlinkM to I2C3 and put the following line to the rc startup-script: - * blinkm start - * - * To start the system monitor put in the next line after the blinkm start: - * blinkm systemmonitor - * - * - * Description: - * After startup, the Application checked how many lipo cells are connected to the System. - * The recognized number off cells, will be blinked 5 times in purple color. - * 2 Cells = 2 blinks - * ... - * 5 Cells = 5 blinks - * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. - * - * System disarmed: - * The BlinkM should lit solid red. - * - * System armed: - * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. - * - * X-X-X-X-_-_-_-_-_-_- - * ------------------------- - * G G G M - * P P P O - * S S S D - * E - * - * (X = on, _=off) - * - * The first 3 blinks indicates the status of the GPS-Signal (red): - * 0-4 satellites = X-X-X-X-_-_-_-_-_-_- - * 5 satellites = X-X-_-X-_-_-_-_-_-_- - * 6 satellites = X-_-_-X-_-_-_-_-_-_- - * >=7 satellites = _-_-_-X-_-_-_-_-_-_- - * If no GPS is found the first 3 blinks are white - * - * The fourth Blink indicates the Flightmode: - * MANUAL : amber - * STABILIZED : yellow - * HOLD : blue - * AUTO : green - * - * Battery Warning (low Battery Level): - * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X - * - * Battery Alert (critical Battery Level) - * Continuously blinking in red X-X-X-X-X-X-X-X-X-X - * - * General Error (no uOrb Data) - * Continuously blinking in white X-X-X-X-X-X-X-X-X-X - * - */ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include - -static const float MAX_CELL_VOLTAGE = 4.3f; -static const int LED_ONTIME = 120; -static const int LED_OFFTIME = 120; -static const int LED_BLINK = 1; -static const int LED_NOBLINK = 0; - -class BlinkM : public device::I2C -{ -public: - BlinkM(int bus, int blinkm); - virtual ~BlinkM(); - - - virtual int init(); - virtual int probe(); - virtual int setMode(int mode); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - static const char *const script_names[]; - -private: - enum ScriptID { - USER = 0, - RGB, - WHITE_FLASH, - RED_FLASH, - GREEN_FLASH, - BLUE_FLASH, - CYAN_FLASH, - MAGENTA_FLASH, - YELLOW_FLASH, - BLACK, - HUE_CYCLE, - MOOD_LIGHT, - VIRTUAL_CANDLE, - WATER_REFLECTIONS, - OLD_NEON, - THE_SEASONS, - THUNDERSTORM, - STOP_LIGHT, - MORSE_CODE - }; - - enum ledColors { - LED_OFF, - LED_RED, - LED_YELLOW, - LED_PURPLE, - LED_GREEN, - LED_BLUE, - LED_WHITE, - LED_AMBER - }; - - work_s _work; - - int led_color_1; - int led_color_2; - int led_color_3; - int led_color_4; - int led_color_5; - int led_color_6; - int led_color_7; - int led_color_8; - int led_blink; - - bool systemstate_run; - - void setLEDColor(int ledcolor); - static void led_trampoline(void *arg); - void led(); - - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - - int fade_rgb(uint8_t r, uint8_t g, uint8_t b); - int fade_hsb(uint8_t h, uint8_t s, uint8_t b); - - int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); - int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); - - int set_fade_speed(uint8_t s); - - int play_script(uint8_t script_id); - int play_script(const char *script_name); - int stop_script(); - - int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); - int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); - int set_script(uint8_t length, uint8_t repeats); - - int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); - - int get_firmware_version(uint8_t version[2]); -}; - -/* for now, we only support one BlinkM */ -namespace -{ - BlinkM *g_blinkm; -} - -/* list of script names, must match script ID numbers */ -const char *const BlinkM::script_names[] = { - "USER", - "RGB", - "WHITE_FLASH", - "RED_FLASH", - "GREEN_FLASH", - "BLUE_FLASH", - "CYAN_FLASH", - "MAGENTA_FLASH", - "YELLOW_FLASH", - "BLACK", - "HUE_CYCLE", - "MOOD_LIGHT", - "VIRTUAL_CANDLE", - "WATER_REFLECTIONS", - "OLD_NEON", - "THE_SEASONS", - "THUNDERSTORM", - "STOP_LIGHT", - "MORSE_CODE", - nullptr -}; - - -extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); - -BlinkM::BlinkM(int bus, int blinkm) : - I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000), - led_color_1(LED_OFF), - led_color_2(LED_OFF), - led_color_3(LED_OFF), - led_color_4(LED_OFF), - led_color_5(LED_OFF), - led_color_6(LED_OFF), - led_color_7(LED_OFF), - led_color_8(LED_OFF), - led_blink(LED_NOBLINK), - systemstate_run(false) -{ - memset(&_work, 0, sizeof(_work)); -} - -BlinkM::~BlinkM() -{ -} - -int -BlinkM::init() -{ - int ret; - ret = I2C::init(); - - if (ret != OK) { - warnx("I2C init failed"); - return ret; - } - - stop_script(); - set_rgb(0,0,0); - - return OK; -} - -int -BlinkM::setMode(int mode) -{ - if(mode == 1) { - if(systemstate_run == false) { - stop_script(); - set_rgb(0,0,0); - systemstate_run = true; - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); - } - } else { - systemstate_run = false; - } - - return OK; -} - -int -BlinkM::probe() -{ - uint8_t version[2]; - int ret; - - ret = get_firmware_version(version); - - if (ret == OK) - log("found BlinkM firmware version %c%c", version[1], version[0]); - - return ret; -} - -int -BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = ENOTTY; - - switch (cmd) { - case BLINKM_PLAY_SCRIPT_NAMED: - if (arg == 0) { - ret = EINVAL; - break; - } - ret = play_script((const char *)arg); - break; - - case BLINKM_PLAY_SCRIPT: - ret = play_script(arg); - break; - - case BLINKM_SET_USER_SCRIPT: { - if (arg == 0) { - ret = EINVAL; - break; - } - - unsigned lines = 0; - const uint8_t *script = (const uint8_t *)arg; - - while ((lines < 50) && (script[1] != 0)) { - ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); - if (ret != OK) - break; - script += 5; - } - if (ret == OK) - ret = set_script(lines, 0); - break; - } - - default: - break; - } - - return ret; -} - - -void -BlinkM::led_trampoline(void *arg) -{ - BlinkM *bm = (BlinkM *)arg; - - bm->led(); -} - - - -void -BlinkM::led() -{ - - static int vehicle_status_sub_fd; - static int vehicle_gps_position_sub_fd; - - static int num_of_cells = 0; - static int detected_cells_runcount = 0; - - static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; - static int t_led_blink = 0; - static int led_thread_runcount=0; - static int led_interval = 1000; - - static int no_data_vehicle_status = 0; - static int no_data_vehicle_gps_position = 0; - - static bool topic_initialized = false; - static bool detected_cells_blinked = false; - static bool led_thread_ready = true; - - int num_of_used_sats = 0; - - if(!topic_initialized) { - vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(vehicle_status_sub_fd, 1000); - - vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(vehicle_gps_position_sub_fd, 1000); - - topic_initialized = true; - } - - if(led_thread_ready == true) { - if(!detected_cells_blinked) { - if(num_of_cells > 0) { - t_led_color[0] = LED_PURPLE; - } - if(num_of_cells > 1) { - t_led_color[1] = LED_PURPLE; - } - if(num_of_cells > 2) { - t_led_color[2] = LED_PURPLE; - } - if(num_of_cells > 3) { - t_led_color[3] = LED_PURPLE; - } - if(num_of_cells > 4) { - t_led_color[4] = LED_PURPLE; - } - t_led_color[5] = LED_OFF; - t_led_color[6] = LED_OFF; - t_led_color[7] = LED_OFF; - t_led_blink = LED_BLINK; - } else { - t_led_color[0] = led_color_1; - t_led_color[1] = led_color_2; - t_led_color[2] = led_color_3; - t_led_color[3] = led_color_4; - t_led_color[4] = led_color_5; - t_led_color[5] = led_color_6; - t_led_color[6] = led_color_7; - t_led_color[7] = led_color_8; - t_led_blink = led_blink; - } - led_thread_ready = false; - } - - if (led_thread_runcount & 1) { - if (t_led_blink) - setLEDColor(LED_OFF); - led_interval = LED_OFFTIME; - } else { - setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); - //led_interval = (led_thread_runcount & 1) : LED_ONTIME; - led_interval = LED_ONTIME; - } - - if (led_thread_runcount == 15) { - /* obtained data for the first file descriptor */ - struct vehicle_status_s vehicle_status_raw; - struct vehicle_gps_position_s vehicle_gps_position_raw; - - memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); - memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); - - bool new_data_vehicle_status; - bool new_data_vehicle_gps_position; - - orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); - - if (new_data_vehicle_status) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); - no_data_vehicle_status = 0; - } else { - no_data_vehicle_status++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; - } - - orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); - - if (new_data_vehicle_gps_position) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); - no_data_vehicle_gps_position = 0; - } else { - no_data_vehicle_gps_position++; - if(no_data_vehicle_gps_position >= 3) - no_data_vehicle_gps_position = 3; - } - - - - /* get number of used satellites in navigation */ - num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; - } - printf(" cells found:%d\n", num_of_cells); - - } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - /* LED Pattern for battery low warning */ - led_color_1 = LED_YELLOW; - led_color_2 = LED_YELLOW; - led_color_3 = LED_YELLOW; - led_color_4 = LED_YELLOW; - led_color_5 = LED_YELLOW; - led_color_6 = LED_YELLOW; - led_color_7 = LED_YELLOW; - led_color_8 = LED_YELLOW; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - /* LED Pattern for battery critical alerting */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_BLINK; - - } else { - /* no battery warnings here */ - - if(vehicle_status_raw.flag_system_armed == false) { - /* system not armed */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_NOBLINK; - - } else { - /* armed system - initial led pattern */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_OFF; - led_color_5 = LED_OFF; - led_color_6 = LED_OFF; - led_color_7 = LED_OFF; - led_color_8 = LED_OFF; - led_blink = LED_BLINK; - - /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } - - if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used satīs */ - if(num_of_used_sats >= 7) { - led_color_1 = LED_OFF; - led_color_2 = LED_OFF; - led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { - led_color_2 = LED_OFF; - led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { - led_color_3 = LED_OFF; - } - - } else { - /* no vehicle_gps_position data */ - led_color_1 = LED_WHITE; - led_color_2 = LED_WHITE; - led_color_3 = LED_WHITE; - - } - - } - } - } - } else { - /* LED Pattern for general Error - no vehicle_status can retrieved */ - led_color_1 = LED_WHITE; - led_color_2 = LED_WHITE; - led_color_3 = LED_WHITE; - led_color_4 = LED_WHITE; - led_color_5 = LED_WHITE; - led_color_6 = LED_WHITE; - led_color_7 = LED_WHITE; - led_color_8 = LED_WHITE; - led_blink = LED_BLINK; - - } - - /* - printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", - vehicle_status_raw.voltage_battery, - vehicle_status_raw.flag_system_armed, - vehicle_status_raw.flight_mode, - num_of_cells, - no_data_vehicle_status, - no_data_vehicle_gps_position, - num_of_used_sats, - vehicle_gps_position_raw.fix_type, - vehicle_gps_position_raw.satellites_visible); - */ - - led_thread_runcount=0; - led_thread_ready = true; - led_interval = LED_OFFTIME; - - if(detected_cells_runcount < 4){ - detected_cells_runcount++; - } else { - detected_cells_blinked = true; - } - - } else { - led_thread_runcount++; - } - - if(systemstate_run == true) { - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); - } else { - stop_script(); - set_rgb(0,0,0); - } -} - -void BlinkM::setLEDColor(int ledcolor) { - switch (ledcolor) { - case LED_OFF: // off - set_rgb(0,0,0); - break; - case LED_RED: // red - set_rgb(255,0,0); - break; - case LED_YELLOW: // yellow - set_rgb(255,70,0); - break; - case LED_PURPLE: // purple - set_rgb(255,0,255); - break; - case LED_GREEN: // green - set_rgb(0,255,0); - break; - case LED_BLUE: // blue - set_rgb(0,0,255); - break; - case LED_WHITE: // white - set_rgb(255,255,255); - break; - case LED_AMBER: // amber - set_rgb(255,20,0); - break; - } -} - -int -BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'n', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'c', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) -{ - const uint8_t msg[4] = { 'h', h, s, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'C', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) -{ - const uint8_t msg[4] = { 'H', h, s, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::set_fade_speed(uint8_t s) -{ - const uint8_t msg[2] = { 'f', s }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::play_script(uint8_t script_id) -{ - const uint8_t msg[4] = { 'p', script_id, 0, 0 }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::play_script(const char *script_name) -{ - /* handle HTML colour encoding */ - if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { - char code[3]; - uint8_t r, g, b; - - code[2] = '\0'; - - code[0] = script_name[1]; - code[1] = script_name[2]; - r = strtol(code, 0, 16); - code[0] = script_name[3]; - code[1] = script_name[4]; - g = strtol(code, 0, 16); - code[0] = script_name[5]; - code[1] = script_name[6]; - b = strtol(code, 0, 16); - - stop_script(); - return set_rgb(r, g, b); - } - - for (unsigned i = 0; script_names[i] != nullptr; i++) - if (!strcasecmp(script_name, script_names[i])) - return play_script(i); - - return -1; -} - -int -BlinkM::stop_script() -{ - const uint8_t msg[1] = { 'o' }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) -{ - const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) -{ - const uint8_t msg[3] = { 'R', 0, line }; - uint8_t result[5]; - - int ret = transfer(msg, sizeof(msg), result, sizeof(result)); - - if (ret == OK) { - ticks = result[0]; - cmd[0] = result[1]; - cmd[1] = result[2]; - cmd[2] = result[3]; - cmd[3] = result[4]; - } - - return ret; -} - -int -BlinkM::set_script(uint8_t len, uint8_t repeats) -{ - const uint8_t msg[4] = { 'L', 0, len, repeats }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) -{ - const uint8_t msg = 'g'; - uint8_t result[3]; - - int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); - - if (ret == OK) { - r = result[0]; - g = result[1]; - b = result[2]; - } - - return ret; -} - -int -BlinkM::get_firmware_version(uint8_t version[2]) -{ - const uint8_t msg = 'Z'; - - return transfer(&msg, sizeof(msg), version, 2); -} - -void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); -} - -int -blinkm_main(int argc, char *argv[]) -{ - - int i2cdevice = PX4_I2C_BUS_EXPANSION; - int blinkmadr = 9; - - int x; - - for (x = 1; x < argc; x++) { - if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { - if (argc > x + 1) { - i2cdevice = atoi(argv[x + 1]); - } - } - - if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) { - if (argc > x + 1) { - blinkmadr = atoi(argv[x + 1]); - } - } - - } - - if (!strcmp(argv[1], "start")) { - if (g_blinkm != nullptr) - errx(1, "already started"); - - g_blinkm = new BlinkM(i2cdevice, blinkmadr); - - if (g_blinkm == nullptr) - errx(1, "new failed"); - - if (OK != g_blinkm->init()) { - delete g_blinkm; - g_blinkm = nullptr; - errx(1, "init failed"); - } - - exit(0); - } - - - if (g_blinkm == nullptr) { - fprintf(stderr, "not started\n"); - blinkm_usage(); - exit(0); - } - - if (!strcmp(argv[1], "systemstate")) { - g_blinkm->setMode(1); - exit(0); - } - - if (!strcmp(argv[1], "ledoff")) { - g_blinkm->setMode(0); - exit(0); - } - - - if (!strcmp(argv[1], "list")) { - for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) - fprintf(stderr, " %s\n", BlinkM::script_names[i]); - fprintf(stderr, " \n"); - exit(0); - } - - /* things that require access to the device */ - int fd = open(BLINKM_DEVICE_PATH, 0); - if (fd < 0) - err(1, "can't open BlinkM device"); - - g_blinkm->setMode(0); - if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) - exit(0); - - blinkm_usage(); - exit(0); -} diff --git a/apps/drivers/bma180/Makefile b/apps/drivers/bma180/Makefile deleted file mode 100644 index cc01b629e..000000000 --- a/apps/drivers/bma180/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the BMA180 driver. -# - -APPNAME = bma180 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp deleted file mode 100644 index 4409a8a9c..000000000 --- a/apps/drivers/bma180/bma180.cpp +++ /dev/null @@ -1,929 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file bma180.cpp - * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) - -#define ADDR_CHIP_ID 0x00 -#define CHIP_ID 0x03 - -#define ADDR_ACC_X_LSB 0x02 -#define ADDR_ACC_Y_LSB 0x04 -#define ADDR_ACC_Z_LSB 0x06 -#define ADDR_TEMPERATURE 0x08 - -#define ADDR_CTRL_REG0 0x0D -#define REG0_WRITE_ENABLE 0x10 - -#define ADDR_RESET 0x10 -#define SOFT_RESET 0xB6 - -#define ADDR_BW_TCS 0x20 -#define BW_TCS_BW_MASK (0xf<<4) -#define BW_TCS_BW_10HZ (0<<4) -#define BW_TCS_BW_20HZ (1<<4) -#define BW_TCS_BW_40HZ (2<<4) -#define BW_TCS_BW_75HZ (3<<4) -#define BW_TCS_BW_150HZ (4<<4) -#define BW_TCS_BW_300HZ (5<<4) -#define BW_TCS_BW_600HZ (6<<4) -#define BW_TCS_BW_1200HZ (7<<4) - -#define ADDR_HIGH_DUR 0x27 -#define HIGH_DUR_DIS_I2C (1<<0) - -#define ADDR_TCO_Z 0x30 -#define TCO_Z_MODE_MASK 0x3 - -#define ADDR_GAIN_Y 0x33 -#define GAIN_Y_SHADOW_DIS (1<<0) - -#define ADDR_OFFSET_LSB1 0x35 -#define OFFSET_LSB1_RANGE_MASK (7<<1) -#define OFFSET_LSB1_RANGE_1G (0<<1) -#define OFFSET_LSB1_RANGE_2G (2<<1) -#define OFFSET_LSB1_RANGE_3G (3<<1) -#define OFFSET_LSB1_RANGE_4G (4<<1) -#define OFFSET_LSB1_RANGE_8G (5<<1) -#define OFFSET_LSB1_RANGE_16G (6<<1) - -#define ADDR_OFFSET_T 0x37 -#define OFFSET_T_READOUT_12BIT (1<<0) - -extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); } - -class BMA180 : public device::SPI -{ -public: - BMA180(int bus, spi_dev_e device); - virtual ~BMA180(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - - struct hrt_call _call; - unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; - - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _current_lowpass; - unsigned _current_range; - - perf_counter_t _sample_perf; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Read a register from the BMA180 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the BMA180 - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the BMA180 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the BMA180 measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_g); - - /** - * Set the BMA180 internal lowpass filter frequency. - * - * @param frequency The internal lowpass filter frequency is set to a value - * equal or greater to this. - * Zero selects the highest frequency supported. - * @return OK if the value can be supported. - */ - int set_lowpass(unsigned frequency); -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - - -BMA180::BMA180(int bus, spi_dev_e device) : - SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), - _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _current_lowpass(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) -{ - // enable debug() calls - _debug_enabled = true; - - // default scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; -} - -BMA180::~BMA180() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; - - /* delete the perf counter */ - perf_free(_sample_perf); -} - -int -BMA180::init() -{ - int ret = ERROR; - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); - - /* perform soft reset (p48) */ - write_reg(ADDR_RESET, SOFT_RESET); - - /* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */ - usleep(10000); - - /* enable writing to chip config */ - modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); - - /* disable I2C interface */ - modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0); - - /* switch to low-noise mode */ - modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0); - - /* disable 12-bit mode */ - modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0); - - /* disable shadow-disable mode */ - modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0); - - /* disable writing to chip config */ - modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - - if (set_range(4)) warnx("Failed setting range"); - - if (set_lowpass(75)) warnx("Failed setting lowpass"); - - if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { - ret = OK; - - } else { - ret = ERROR; - } - -out: - return ret; -} - -int -BMA180::probe() -{ - /* dummy read to ensure SPI state machine is sane */ - read_reg(ADDR_CHIP_ID); - - if (read_reg(ADDR_CHIP_ID) == CHIP_ID) - return OK; - - return -EIO; -} - -ssize_t -BMA180::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct accel_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_report = _next_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - return ret; -} - -int -BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_interval; - - case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; - - case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ - return -EINVAL; - - case ACCELIOCGSAMPLERATE: - return 1200; /* always operating in low-noise mode */ - - case ACCELIOCSLOWPASS: - return set_lowpass(arg); - - case ACCELIOCGLOWPASS: - return _current_lowpass; - - case ACCELIOCSSCALE: - /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); - return OK; - - case ACCELIOCGSCALE: - /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); - return OK; - - case ACCELIOCSRANGE: - return set_range(arg); - - case ACCELIOCGRANGE: - return _current_range; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -BMA180::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -BMA180::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -BMA180::set_range(unsigned max_g) -{ - uint8_t rangebits; - - if (max_g == 0) - max_g = 16; - - if (max_g > 16) - return -ERANGE; - - if (max_g <= 2) { - _current_range = 2; - rangebits = OFFSET_LSB1_RANGE_2G; - - } else if (max_g <= 3) { - _current_range = 3; - rangebits = OFFSET_LSB1_RANGE_3G; - - } else if (max_g <= 4) { - _current_range = 4; - rangebits = OFFSET_LSB1_RANGE_4G; - - } else if (max_g <= 8) { - _current_range = 8; - rangebits = OFFSET_LSB1_RANGE_8G; - - } else if (max_g <= 16) { - _current_range = 16; - rangebits = OFFSET_LSB1_RANGE_16G; - - } else { - return -EINVAL; - } - - /* set new range scaling factor */ - _accel_range_m_s2 = _current_range * 9.80665f; - _accel_range_scale = _accel_range_m_s2 / 8192.0f; - - /* enable writing to chip config */ - modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); - - /* adjust sensor configuration */ - modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); - - /* block writing to chip config */ - modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - - /* check if wanted value is now in register */ - return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) == - (OFFSET_LSB1_RANGE_MASK & rangebits)); -} - -int -BMA180::set_lowpass(unsigned frequency) -{ - uint8_t bwbits; - - if (frequency > 1200) { - return -ERANGE; - - } else if (frequency > 600) { - bwbits = BW_TCS_BW_1200HZ; - - } else if (frequency > 300) { - bwbits = BW_TCS_BW_600HZ; - - } else if (frequency > 150) { - bwbits = BW_TCS_BW_300HZ; - - } else if (frequency > 75) { - bwbits = BW_TCS_BW_150HZ; - - } else if (frequency > 40) { - bwbits = BW_TCS_BW_75HZ; - - } else if (frequency > 20) { - bwbits = BW_TCS_BW_40HZ; - - } else if (frequency > 10) { - bwbits = BW_TCS_BW_20HZ; - - } else { - bwbits = BW_TCS_BW_10HZ; - } - - /* enable writing to chip config */ - modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); - - /* adjust sensor configuration */ - modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits); - - /* block writing to chip config */ - modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - - /* check if wanted value is now in register */ - return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == - (BW_TCS_BW_MASK & bwbits)); -} - -void -BMA180::start() -{ - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _oldest_report = _next_report = 0; - - /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); -} - -void -BMA180::stop() -{ - hrt_cancel(&_call); -} - -void -BMA180::measure_trampoline(void *arg) -{ - BMA180 *dev = (BMA180 *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -BMA180::measure() -{ - /* BMA180 measurement registers */ -// #pragma pack(push, 1) -// struct { -// uint8_t cmd; -// int16_t x; -// int16_t y; -// int16_t z; -// } raw_report; -// #pragma pack(pop) - - accel_report *report = &_reports[_next_report]; - - /* start the performance counter */ - perf_begin(_sample_perf); - - /* - * Fetch the full set of measurements from the BMA180 in one pass; - * starting from the X LSB. - */ - //raw_report.cmd = ADDR_ACC_X_LSB; - // XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); - - /* - * Adjust and scale results to SI units. - * - * Note that we ignore the "new data" bits. At any time we read, each - * of the axis measurements are the "most recent", even if we've seen - * them before. There is no good way to synchronise with the internal - * measurement flow without using the external interrupt. - */ - _reports[_next_report].timestamp = hrt_absolute_time(); - /* - * y of board is x of sensor and x of board is -y of sensor - * perform only the axis assignment here. - * Two non-value bits are discarded directly - */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; - - /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); - - /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; - - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); - - /* stop the perf counter */ - perf_end(_sample_perf); -} - -void -BMA180::print_info() -{ - perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); -} - -/** - * Local functions in support of the shell command. - */ -namespace bma180 -{ - -BMA180 *g_dev; - -void start(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd = -1; - struct accel_report a_report; - ssize_t sz; - - /* get the driver */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'bma180 start' if the driver is not running)", - ACCEL_DEVICE_PATH); - - /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) - err(1, "reset to manual polling"); - - /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) - err(1, "immediate acc read failed"); - - warnx("single read"); - warnx("time: %lld", a_report.timestamp); - warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); - warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); - warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); - warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); - warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); - warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); - warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); - - /* XXX add poll-rate tests here too */ - - reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "BMA180: driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - - -} // namespace - -int -bma180_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - - */ - if (!strcmp(argv[1], "start")) - bma180::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - bma180::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - bma180::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - bma180::info(); - - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile deleted file mode 100644 index 3859a88a5..000000000 --- a/apps/drivers/gps/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# GPS driver -# - -APPNAME = gps -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp deleted file mode 100644 index e35bdb944..000000000 --- a/apps/drivers/gps/gps.cpp +++ /dev/null @@ -1,536 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file gps.cpp - * Driver for the GPS on a serial port - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ubx.h" -#include "mtk.h" - - -#define TIMEOUT_5HZ 500 -#define RATE_MEASUREMENT_PERIOD 5000000 - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - - - -class GPS : public device::CDev -{ -public: - GPS(const char* uart_path); - virtual ~GPS(); - - virtual int init(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -private: - - bool _task_should_exit; ///< flag to make the main worker task exit - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; ///< current baudrate - char _port[20]; ///< device / serial port path - volatile int _task; //< worker task - bool _healthy; ///< flag to signal if the GPS is ok - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed - bool _mode_changed; ///< flag that the GPS mode has changed - gps_driver_mode_t _mode; ///< current mode - GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position - float _rate; ///< position update rate - - - /** - * Try to configure the GPS, handle outgoing communication to the GPS - */ - void config(); - - /** - * Trampoline to the worker task - */ - static void task_main_trampoline(void *arg); - - - /** - * Worker task: main GPS thread that configures the GPS and parses incoming data, always running - */ - void task_main(void); - - /** - * Set the baudrate of the UART to the GPS - */ - int set_baudrate(unsigned baud); - - /** - * Send a reset command to the GPS - */ - void cmd_reset(); - -}; - - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int gps_main(int argc, char *argv[]); - -namespace -{ - -GPS *g_dev; - -} - - -GPS::GPS(const char* uart_path) : - CDev("gps", GPS_DEVICE_PATH), - _task_should_exit(false), - _healthy(false), - _mode_changed(false), - _mode(GPS_DRIVER_MODE_UBX), - _Helper(nullptr), - _report_pub(-1), - _rate(0.0f) -{ - /* store port name */ - strncpy(_port, uart_path, sizeof(_port)); - /* enforce null termination */ - _port[sizeof(_port) - 1] = '\0'; - - /* we need this potentially before it could be set in task_main */ - g_dev = this; - memset(&_report, 0, sizeof(_report)); - - _debug_enabled = true; -} - -GPS::~GPS() -{ - /* tell the task we want it to go away */ - _task_should_exit = true; - - /* spin waiting for the task to stop */ - for (unsigned i = 0; (i < 10) && (_task != -1); i++) { - /* give it another 100ms */ - usleep(100000); - } - - /* well, kill it anyway, though this will probably crash */ - if (_task != -1) - task_delete(_task); - g_dev = nullptr; - -} - -int -GPS::init() -{ - int ret = ERROR; - - /* do regular cdev init */ - if (CDev::init() != OK) - goto out; - - /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); - - if (_task < 0) { - warnx("task start failed: %d", errno); - return -errno; - } - - ret = OK; -out: - return ret; -} - -int -GPS::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - lock(); - - int ret = OK; - - switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - } - - unlock(); - - return ret; -} - -void -GPS::task_main_trampoline(void *arg) -{ - g_dev->task_main(); -} - -void -GPS::task_main() -{ - log("starting"); - - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR); - - if (_serial_fd < 0) { - log("failed to open serial port: %s err: %d", _port, errno); - /* tell the dtor that we are exiting, set error code */ - _task = -1; - _exit(1); - } - - uint64_t last_rate_measurement = hrt_absolute_time(); - unsigned last_rate_count = 0; - - /* loop handling received serial bytes and also configuring in between */ - while (!_task_should_exit) { - - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: - break; - } - unlock(); - if (_Helper->configure(_baudrate) == 0) { - unlock(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } - - if (!_healthy) { - warnx("module found"); - _healthy = true; - } - } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; - } - - lock(); - } - lock(); - - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; - default: - break; - } - - } - debug("exiting"); - - ::close(_serial_fd); - - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); -} - - - -void -GPS::cmd_reset() -{ - //XXX add reset? -} - -void -GPS::print_info() -{ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: - break; - } - warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - if (_report.timestamp_position != 0) { - warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); - warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("update rate: %6.2f Hz", (double)_rate); - } - - usleep(100000); -} - -/** - * Local functions in support of the shell command. - */ -namespace gps -{ - -GPS *g_dev; - -void start(const char *uart_path); -void stop(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start(const char *uart_path) -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new GPS(uart_path); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(GPS_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); - goto fail; - } - exit(0); - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Stop the driver. - */ -void -stop() -{ - delete g_dev; - g_dev = nullptr; - - exit(0); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(GPS_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - exit(0); -} - -/** - * Print the status of the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - g_dev->print_info(); - - exit(0); -} - -} // namespace - - -int -gps_main(int argc, char *argv[]) -{ - - /* set to default */ - char* device_name = GPS_DEFAULT_UART_PORT; - - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) { - /* work around getopt unreliability */ - if (argc > 3) { - if (!strcmp(argv[2], "-d")) { - device_name = argv[3]; - } else { - goto out; - } - } - gps::start(device_name); - } - - if (!strcmp(argv[1], "stop")) - gps::stop(); - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - gps::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - gps::reset(); - - /* - * Print driver status. - */ - if (!strcmp(argv[1], "status")) - gps::info(); - -out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); -} diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp deleted file mode 100644 index 9c1fad569..000000000 --- a/apps/drivers/gps/gps_helper.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include "gps_helper.h" - -/* @file gps_helper.cpp */ - -int -GPS_Helper::set_baudrate(const int &fd, unsigned baud) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - warnx("try baudrate: %d\n", speed); - - default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); - return -EINVAL; - } - struct termios uart_config; - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(fd, &uart_config); - - /* clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - /* no parity, one stop bit */ - uart_config.c_cflag &= ~(CSTOPB | PARENB); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); - return -1; - } - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); - return -1; - } - if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); - return -1; - } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ - return 0; -} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h deleted file mode 100644 index f3d3bc40b..000000000 --- a/apps/drivers/gps/gps_helper.h +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file gps_helper.h */ - -#ifndef GPS_HELPER_H -#define GPS_HELPER_H - -#include -#include - -class GPS_Helper -{ -public: - virtual int configure(unsigned &baud) = 0; - virtual int receive(unsigned timeout) = 0; - int set_baudrate(const int &fd, unsigned baud); -}; - -#endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp deleted file mode 100644 index 4762bd503..000000000 --- a/apps/drivers/gps/mtk.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file mkt.cpp */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mtk.h" - - -MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_mtk_revision(0) -{ - decode_init(); -} - -MTK::~MTK() -{ -} - -int -MTK::configure(unsigned &baudrate) -{ - /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) - return -1; - - baudrate = MTK_BAUDRATE; - - /* Write config messages, don't wait for an answer */ - if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { - warnx("mtk: config write failed"); - return -1; - } - usleep(10000); - - if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { - warnx("mtk: config write failed"); - return -1; - } - usleep(10000); - - if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { - warnx("mtk: config write failed"); - return -1; - } - usleep(10000); - - if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { - warnx("mtk: config write failed"); - return -1; - } - usleep(10000); - - if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { - warnx("mtk: config write failed"); - return -1; - } - - return 0; -} - -int -MTK::receive(unsigned timeout) -{ - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - uint8_t buf[32]; - gps_mtk_packet_t packet; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - ssize_t count = 0; - - while (true) { - - /* first read whatever is left */ - if (j < count) { - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j], packet) > 0) { - handle_message(packet); - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { - return -1; - } - j++; - } - /* everything is read */ - j = count = 0; - } - - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); - - if (ret < 0) { - /* something went wrong when polling */ - return -1; - - } else if (ret == 0) { - /* Timeout */ - return -1; - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_fd, buf, sizeof(buf)); - } - } - } -} - -void -MTK::decode_init(void) -{ - _rx_ck_a = 0; - _rx_ck_b = 0; - _rx_count = 0; - _decode_state = MTK_DECODE_UNINIT; -} -int -MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) -{ - int ret = 0; - - if (_decode_state == MTK_DECODE_UNINIT) { - - if (b == MTK_SYNC1_V16) { - _decode_state = MTK_DECODE_GOT_CK_A; - _mtk_revision = 16; - } else if (b == MTK_SYNC1_V19) { - _decode_state = MTK_DECODE_GOT_CK_A; - _mtk_revision = 19; - } - - } else if (_decode_state == MTK_DECODE_GOT_CK_A) { - if (b == MTK_SYNC2) { - _decode_state = MTK_DECODE_GOT_CK_B; - - } else { - // Second start symbol was wrong, reset state machine - decode_init(); - } - - } else if (_decode_state == MTK_DECODE_GOT_CK_B) { - // Add to checksum - if (_rx_count < 33) - add_byte_to_checksum(b); - - // Fill packet buffer - ((uint8_t*)(&packet))[_rx_count] = b; - _rx_count++; - - /* Packet size minus checksum, XXX ? */ - if (_rx_count >= sizeof(packet)) { - /* Compare checksum */ - if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { - ret = 1; - } else { - warnx("MTK Checksum invalid"); - ret = -1; - } - // Reset state machine to decode next packet - decode_init(); - } - } - return ret; -} - -void -MTK::handle_message(gps_mtk_packet_t &packet) -{ - if (_mtk_revision == 16) { - _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 - _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 - } else if (_mtk_revision == 19) { - _gps_position->lat = packet.latitude; // both degrees*1e7 - _gps_position->lon = packet.longitude; // both degrees*1e7 - } else { - warnx("mtk: unknown revision"); - _gps_position->lat = 0; - _gps_position->lon = 0; - } - _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm - _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - _gps_position->epv_m = 0.0; //unknown in mtk custom mode - _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s - _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - _gps_position->satellites_visible = packet.satellites; - - /* convert time and date information to unix timestamp */ - struct tm timeinfo; //TODO: test this conversion - uint32_t timeinfo_conversion_temp; - - timeinfo.tm_mday = packet.date * 1e-4; - timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; - timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; - timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; - - timeinfo.tm_hour = packet.utc_time * 1e-7; - timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; - timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; - timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; - timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; - timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; - time_t epoch = mktime(&timeinfo); - - _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; - _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); - - return; -} - -void -MTK::add_byte_to_checksum(uint8_t b) -{ - _rx_ck_a = _rx_ck_a + b; - _rx_ck_b = _rx_ck_b + _rx_ck_a; -} diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h deleted file mode 100644 index d4e390b01..000000000 --- a/apps/drivers/gps/mtk.h +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file mtk.h */ - -#ifndef MTK_H_ -#define MTK_H_ - -#include "gps_helper.h" - -#define MTK_SYNC1_V16 0xd0 -#define MTK_SYNC1_V19 0xd1 -#define MTK_SYNC2 0xdd - -#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" -#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" -#define SBAS_ON "$PMTK313,1*2E\r\n" -#define WAAS_ON "$PMTK301,2*2E\r\n" -#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" - -#define MTK_TIMEOUT_5HZ 400 -#define MTK_BAUDRATE 38400 - -typedef enum { - MTK_DECODE_UNINIT = 0, - MTK_DECODE_GOT_CK_A = 1, - MTK_DECODE_GOT_CK_B = 2 -} mtk_decode_state_t; - -/** the structures of the binary packets */ -#pragma pack(push, 1) - -typedef struct { - uint8_t payload; ///< Number of payload bytes - int32_t latitude; ///< Latitude in degrees * 10^7 - int32_t longitude; ///< Longitude in degrees * 10^7 - uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 - uint32_t ground_speed; ///< velocity in m/s - int32_t heading; ///< heading in degrees * 10^2 - uint8_t satellites; ///< number of sattelites used - uint8_t fix_type; ///< fix type: XXX correct for that - uint32_t date; - uint32_t utc_time; - uint16_t hdop; ///< horizontal dilution of position (without unit) - uint8_t ck_a; - uint8_t ck_b; -} gps_mtk_packet_t; - -#pragma pack(pop) - -#define MTK_RECV_BUFFER_SIZE 40 - -class MTK : public GPS_Helper -{ -public: - MTK(const int &fd, struct vehicle_gps_position_s *gps_position); - ~MTK(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); - -private: - /** - * Parse the binary MTK packet - */ - int parse_char(uint8_t b, gps_mtk_packet_t &packet); - - /** - * Handle the package once it has arrived - */ - void handle_message(gps_mtk_packet_t &packet); - - /** - * Reset the parse state machine for a fresh start - */ - void decode_init(void); - - /** - * While parsing add every byte (except the sync bytes) to the checksum - */ - void add_byte_to_checksum(uint8_t); - - int _fd; - struct vehicle_gps_position_s *_gps_position; - mtk_decode_state_t _decode_state; - uint8_t _mtk_revision; - uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; - unsigned _rx_count; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; -}; - -#endif /* MTK_H_ */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp deleted file mode 100644 index c150f5271..000000000 --- a/apps/drivers/gps/ubx.cpp +++ /dev/null @@ -1,755 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ubx.cpp - * - * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description - * including Prototol Specification. - * - * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ubx.h" - -#define UBX_CONFIG_TIMEOUT 100 - -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_waiting_for_ack(false) -{ - decode_init(); -} - -UBX::~UBX() -{ -} - -int -UBX::configure(unsigned &baudrate) -{ - _waiting_for_ack = true; - - /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - - for (int baud_i = 0; baud_i < 5; baud_i++) { - baudrate = baudrates_to_try[baud_i]; - set_baudrate(_fd, baudrate); - - /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_PRT; - - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - /* Send a CFG-PRT message again, this time change the baudrate */ - - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - receive(UBX_CONFIG_TIMEOUT); - - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); - baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - } - - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_RATE; - - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - - send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_NAV5; - - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - - send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_MSG; - - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* For satelites info 1Hz is enough */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } -// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; - - _waiting_for_ack = false; - return 0; - } - return -1; -} - -int -UBX::receive(unsigned timeout) -{ - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - uint8_t buf[32]; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - ssize_t count = 0; - - while (true) { - - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message() > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { - return -1; - } - j++; - } - - /* everything is read */ - j = count = 0; - - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); - - if (ret < 0) { - /* something went wrong when polling */ - return -1; - - } else if (ret == 0) { - /* Timeout */ - return -1; - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_fd, buf, sizeof(buf)); - } - } - } -} - -int -UBX::parse_char(uint8_t b) -{ - switch (_decode_state) { - /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } - break; - /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - } else { - /* Second start symbol was wrong, reset state machine */ - decode_init(); - } - break; - /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ - add_byte_to_checksum(b); - /* check for known class */ - switch (b) { - case UBX_CLASS_ACK: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = ACK; - break; - - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; - -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// break; - - case UBX_CLASS_CFG: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = CFG; - break; - default: //unknown class: reset state machine - decode_init(); - break; - } - break; - case UBX_DECODE_GOT_CLASS: - add_byte_to_checksum(b); - switch (_message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_POSLLH; - break; - - case UBX_MESSAGE_NAV_SOL: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SOL; - break; - - case UBX_MESSAGE_NAV_TIMEUTC: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_TIMEUTC; - break; - -// case UBX_MESSAGE_NAV_DOP: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = NAV_DOP; -// break; - - case UBX_MESSAGE_NAV_SVINFO: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; -// case RXM: -// switch (b) { -// case UBX_MESSAGE_RXM_SVSI: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = RXM_SVSI; -// break; -// -// default: //unknown class: reset state machine, should not happen -// decode_init(); -// break; -// } -// break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know"); - decode_init(); -// config_needed = true; - break; - } - break; - case UBX_DECODE_GOT_MESSAGEID: - add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - break; - case UBX_DECODE_GOT_LENGTH1: - add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - break; - case UBX_DECODE_GOT_LENGTH2: - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); - _rx_buffer[_rx_count] = b; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { - return 1; - } else { - decode_init(); - return -1; - warnx("ubx: Checksum wrong"); - } - - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; - } - return 0; //XXX ? -} - - -int -UBX::handle_message() -{ - int ret = 0; - - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; - - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; - } - break; - } - - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; - - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } - -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// _gps_position->eph_m = packet->hDOP; -// _gps_position->epv = packet->vDOP; -// -// _gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } - - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); - - if (!_waiting_for_ack) { - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - - /* Write satellite information in the global storage */ - _gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - _gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - _gps_position->satellite_used[i] = 0; - } - - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - } else { - _gps_position->satellite_info_available = false; - } - _gps_position->timestamp_satellites = hrt_absolute_time(); - } - - break; - } - - case NAV_VELNED: { - - if (!_waiting_for_ack) { - /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); - } - - break; - } - -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; -// -// _gps_position->satellites_visible = packet->numVis; -// _gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// -// break; -// } - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { - ret = 1; - } - } - } - break; - - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - ret = -1; - break; - } - - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - ret = -1; - break; - } - // end if _rx_count high enough - decode_init(); - return ret; //XXX? -} - -void -UBX::decode_init(void) -{ - _rx_ck_a = 0; - _rx_ck_b = 0; - _rx_count = 0; - _decode_state = UBX_DECODE_UNINIT; - _message_class = CLASS_UNKNOWN; - _message_id = ID_UNKNOWN; - _payload_size = 0; -} - -void -UBX::add_byte_to_checksum(uint8_t b) -{ - _rx_ck_a = _rx_ck_a + b; - _rx_ck_b = _rx_ck_b + _rx_ck_a; -} - -void -UBX::add_checksum_to_message(uint8_t* message, const unsigned length) -{ - uint8_t ck_a = 0; - uint8_t ck_b = 0; - unsigned i; - - for (i = 0; i < length-2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } - /* The checksum is written to the last to bytes of a message */ - message[length-2] = ck_a; - message[length-1] = ck_b; -} - -void -UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) -{ - ssize_t ret = 0; - - /* Calculate the checksum now */ - add_checksum_to_message(packet, length); - - const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; - - /* Start with the two sync bytes */ - ret += write(fd, sync_bytes, sizeof(sync_bytes)); - ret += write(fd, packet, length); - - if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: config write fail"); -} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h deleted file mode 100644 index e3dd1c7ea..000000000 --- a/apps/drivers/gps/ubx.h +++ /dev/null @@ -1,395 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file U-Blox protocol definitions */ - -#ifndef UBX_H_ -#define UBX_H_ - -#include "gps_helper.h" - -#define UBX_SYNC1 0xB5 -#define UBX_SYNC2 0x62 - -/* ClassIDs (the ones that are used) */ -#define UBX_CLASS_NAV 0x01 -//#define UBX_CLASS_RXM 0x02 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 - -/* MessageIDs (the ones that are used) */ -#define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 -//#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 -#define UBX_MESSAGE_NAV_VELNED 0x12 -//#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 -#define UBX_MESSAGE_ACK_NAK 0x00 -#define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 -#define UBX_MESSAGE_CFG_MSG 0x01 -#define UBX_MESSAGE_CFG_RATE 0x08 - -#define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ - -#define UBX_CFG_RATE_LENGTH 6 -#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ -#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ -#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - - -#define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ -#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ - -#define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ - -#define UBX_MAX_PAYLOAD_LENGTH 500 - -// ************ -/** the structures of the binary packets */ -#pragma pack(push, 1) - -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_posllh_packet_t; - -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ - uint8_t flags; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; - uint16_t pDOP; - uint8_t reserved1; - uint8_t numSV; - uint32_t reserved2; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_sol_packet_t; - -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ - int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - uint16_t year; /**< Year, range 1999..2099 (UTC) */ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of Month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ - uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_timeutc_packet_t; - -//typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ -// uint8_t ck_a; -// uint8_t ck_b; -//} gps_bin_nav_dop_packet_t; - -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ - uint8_t globalFlags; - uint16_t reserved2; - -} gps_bin_nav_svinfo_part1_packet_t; - -typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ - uint8_t flags; - uint8_t quality; - uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ - int8_t elev; /**< Elevation in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ - -} gps_bin_nav_svinfo_part2_packet_t; - -typedef struct { - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_svinfo_part3_packet_t; - -typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t velN; //NED north velocity, cm/s - int32_t velE; //NED east velocity, cm/s - int32_t velD; //NED down velocity, cm/s - uint32_t speed; //Speed (3-D), cm/s - uint32_t gSpeed; //Ground Speed (2-D), cm/s - int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 - uint32_t sAcc; //Speed Accuracy Estimate, cm/s - uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_velned_packet_t; - -//typedef struct { -// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ -// -// //... rest of package is not used in this implementation -// -//} gps_bin_rxm_svsi_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_ack_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_nak_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t portID; - uint8_t res0; - uint16_t res1; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t pad; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_prt_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_rate_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint32_t reserved2; - uint32_t reserved3; - uint32_t reserved4; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_nav5_packet_t; - -typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t msgClass_payload; - uint8_t msgID_payload; - uint8_t rate[6]; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_packet_t; - - -// END the structures of the binary packets -// ************ - -typedef enum { - UBX_CONFIG_STATE_PRT = 0, - UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, - UBX_CONFIG_STATE_RATE, - UBX_CONFIG_STATE_NAV5, - UBX_CONFIG_STATE_MSG_NAV_POSLLH, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, - UBX_CONFIG_STATE_MSG_NAV_DOP, - UBX_CONFIG_STATE_MSG_NAV_SVINFO, - UBX_CONFIG_STATE_MSG_NAV_SOL, - UBX_CONFIG_STATE_MSG_NAV_VELNED, -// UBX_CONFIG_STATE_MSG_RXM_SVSI, - UBX_CONFIG_STATE_CONFIGURED -} ubx_config_state_t; - -typedef enum { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -} ubx_message_class_t; - -typedef enum { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH, - NAV_SOL, - NAV_TIMEUTC, -// NAV_DOP, - NAV_SVINFO, - NAV_VELNED, -// RXM_SVSI, - CFG_NAV5, - ACK_ACK, - ACK_NAK, -} ubx_message_id_t; - -typedef enum { - UBX_DECODE_UNINIT = 0, - UBX_DECODE_GOT_SYNC1, - UBX_DECODE_GOT_SYNC2, - UBX_DECODE_GOT_CLASS, - UBX_DECODE_GOT_MESSAGEID, - UBX_DECODE_GOT_LENGTH1, - UBX_DECODE_GOT_LENGTH2 -} ubx_decode_state_t; - -//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; -#pragma pack(pop) - -#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer - -class UBX : public GPS_Helper -{ -public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position); - ~UBX(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); - -private: - - /** - * Parse the binary MTK packet - */ - int parse_char(uint8_t b); - - /** - * Handle the package once it has arrived - */ - int handle_message(void); - - /** - * Reset the parse state machine for a fresh start - */ - void decode_init(void); - - /** - * While parsing add every byte (except the sync bytes) to the checksum - */ - void add_byte_to_checksum(uint8_t); - - /** - * Add the two checksum bytes to an outgoing message - */ - void add_checksum_to_message(uint8_t* message, const unsigned length); - - /** - * Helper to send a config packet - */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); - - int _fd; - struct vehicle_gps_position_s *_gps_position; - ubx_config_state_t _config_state; - bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; - ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; - ubx_message_class_t _message_class; - ubx_message_id_t _message_id; - unsigned _payload_size; -}; - -#endif /* UBX_H_ */ diff --git a/apps/drivers/hil/Makefile b/apps/drivers/hil/Makefile deleted file mode 100644 index 1fb6e37bc..000000000 --- a/apps/drivers/hil/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Interface driver for the PX4FMU board -# - -APPNAME = hil -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp deleted file mode 100644 index d9aa772d4..000000000 --- a/apps/drivers/hil/hil.cpp +++ /dev/null @@ -1,851 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hil.cpp - * - * Driver/configurator for the virtual HIL port. - * - * This virtual driver emulates PWM / servo outputs for setups where - * the connected hardware does not provide enough or no PWM outputs. - * - * Its only function is to take actuator_control uORB messages, - * mix them with any loaded mixer and output the result to the - * actuator_output uORB topic. HIL can also be performed with normal - * PWM outputs, a special flag prevents the outputs to be operated - * during HIL mode. If HIL is not performed with a standalone FMU, - * but in a real system, it is NOT recommended to use this virtual - * driver. Use instead the normal FMU or IO driver. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -class HIL : public device::CDev -{ -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, - MODE_NONE - }; - HIL(); - virtual ~HIL(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - - virtual int init(); - - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - int _update_rate; - int _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(file *filp, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); - -}; - -namespace -{ - -HIL *g_hil; - -} // namespace - -HIL::HIL() : - CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), - _mode(MODE_NONE), - _update_rate(50), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -HIL::~HIL() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_hil = nullptr; -} - -int -HIL::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) - return ret; - - // XXX already claimed with CDEV - ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - // gpio_reset(); - - /* start the HIL interface task */ - _task = task_spawn("fmuhil", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&HIL::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -HIL::task_main_trampoline(int argc, char *argv[]) -{ - g_hil->task_main(); -} - -int -HIL::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - _update_rate = 10; - break; - - default: - return -EINVAL; - } - _mode = mode; - return OK; -} - -int -HIL::set_pwm_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) - return -EINVAL; - - _update_rate = rate; - return OK; -} - -void -HIL::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) - update_rate_in_ms = 2; - orb_set_interval(_t_actuators, update_rate_in_ms); - // up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; - } - - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < (unsigned)outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } - } - - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - } - } - - ::close(_t_actuators); - ::close(_t_armed); - - /* make sure servos are off */ - // up_pwm_servo_deinit(); - - log("stopping"); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); -} - -int -HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -int -HIL::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret; - - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(filp, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - ret = HIL::pwm_ioctl(filp, cmd, arg); - break; - default: - ret = -ENOTTY; - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) - ret = CDev::ioctl(filp, cmd, arg); - - return ret; -} - -int -HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - // int channel; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - // up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - // up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - g_hil->set_pwm_rate(arg); - break; - - case PWM_SERVO_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - break; - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - // channel = cmd - PWM_SERVO_SET(0); -// up_pwm_servo_set(channel, arg); XXX - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - - } else { - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNDEFINED = 0, - PORT1_MODE_UNSET, - PORT1_FULL_PWM, - PORT1_PWM_AND_SERIAL, - PORT1_PWM_AND_GPIO, - PORT2_MODE_UNSET, - PORT2_8PWM, - PORT2_12PWM, - PORT2_16PWM, -}; - -PortMode g_port_mode; - -int -hil_new_mode(PortMode new_mode) -{ - // uint32_t gpio_bits; - - -// /* reset to all-inputs */ -// g_hil->ioctl(0, GPIO_RESET, 0); - - // gpio_bits = 0; - - HIL::Mode servo_mode = HIL::MODE_NONE; - - switch (new_mode) { - case PORT_MODE_UNDEFINED: - case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT1_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = HIL::MODE_4PWM; - break; - - case PORT1_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; -// /* set RX/TX multi-GPIOs to serial mode */ -// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT1_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; - break; - - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; - - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; - - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; - } - -// /* adjust GPIO config for serial mode(s) */ -// if (gpio_bits != 0) -// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_hil->set_mode(servo_mode); - - return OK; -} - -int -hil_start(void) -{ - int ret = OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - -void -test(void) -{ - int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - - if (fd < 0) { - puts("open fail"); - exit(1); - } - - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); - - close(fd); - - exit(0); -} - -void -fake(int argc, char *argv[]) -{ - if (argc < 5) { - puts("hil fake (values -100 .. 100)"); - exit(1); - } - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle < 0) { - puts("advertise failed"); - exit(1); - } - - exit(0); -} - -} // namespace - -extern "C" __EXPORT int hil_main(int argc, char *argv[]); - -int -hil_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb = argv[1]; - - if (hil_start() != OK) - errx(1, "failed to start the HIL driver"); - - /* - * Mode switches. - */ - - // this was all cut-and-pasted from the FMU driver; it's junk - if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT1_FULL_PWM; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(verb, "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNDEFINED) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return OK; - - /* switch modes */ - return hil_new_mode(new_mode); - } - - if (!strcmp(verb, "test")) - test(); - - if (!strcmp(verb, "fake")) - fake(argc - 1, argv + 1); - - - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); - return -EINVAL; -} diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile deleted file mode 100644 index 4d7cb4e7b..000000000 --- a/apps/drivers/hmc5883/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# HMC5883 driver -# - -APPNAME = hmc5883 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp deleted file mode 100644 index 8ab568282..000000000 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ /dev/null @@ -1,1447 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hmc5883.cpp - * - * Driver for the HMC5883 magnetometer connected via I2C. - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include - -#include -#include - -#include - -/* - * HMC5883 internal constants and data structures. - */ - -#define HMC5883L_BUS PX4_I2C_BUS_ONBOARD -#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 - -/* Max measurement rate is 160Hz */ -#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ - -#define ADDR_CONF_A 0x00 -#define ADDR_CONF_B 0x01 -#define ADDR_MODE 0x02 -#define ADDR_DATA_OUT_X_MSB 0x03 -#define ADDR_DATA_OUT_X_LSB 0x04 -#define ADDR_DATA_OUT_Z_MSB 0x05 -#define ADDR_DATA_OUT_Z_LSB 0x06 -#define ADDR_DATA_OUT_Y_MSB 0x07 -#define ADDR_DATA_OUT_Y_LSB 0x08 -#define ADDR_STATUS 0x09 -#define ADDR_ID_A 0x0a -#define ADDR_ID_B 0x0b -#define ADDR_ID_C 0x0c - -/* modes not changeable outside of driver */ -#define HMC5883L_MODE_NORMAL (0 << 0) /* default */ -#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ -#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */ - -#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */ -#define HMC5883L_AVERAGING_2 (1 << 5) -#define HMC5883L_AVERAGING_4 (2 << 5) -#define HMC5883L_AVERAGING_8 (3 << 5) - -#define MODE_REG_CONTINOUS_MODE (0 << 0) -#define MODE_REG_SINGLE_MODE (1 << 0) /* default */ - -#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ -#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ - -#define ID_A_WHO_AM_I 'H' -#define ID_B_WHO_AM_I '4' -#define ID_C_WHO_AM_I '3' - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class HMC5883 : public device::I2C -{ -public: - HMC5883(int bus); - virtual ~HMC5883(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - work_s _work; - unsigned _measure_ticks; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; - mag_scale _scale; - float _range_scale; - float _range_ga; - bool _collect_phase; - - orb_advert_t _mag_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /* status reporting */ - bool _sensor_ok; /**< sensor was found and reports ok */ - bool _calibrated; /**< the calibration is valid */ - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Perform the on-sensor scale calibration routine. - * - * @note The sensor will continue to provide measurements, these - * will however reflect the uncalibrated sensor state until - * the calibration routine has been completed. - * - * @param enable set to 1 to enable self-test strap, 0 to disable - */ - int calibrate(struct file *filp, unsigned enable); - - /** - * Perform the on-sensor scale calibration routine. - * - * @note The sensor will continue to provide measurements, these - * will however reflect the uncalibrated sensor state until - * the calibration routine has been completed. - * - * @param enable set to 1 to enable self-test positive strap, -1 to enable - * negative strap, 0 to set to normal mode - */ - int set_excitement(unsigned enable); - - /** - * Set the sensor range. - * - * Sets the internal range to handle at least the argument in Gauss. - */ - int set_range(unsigned range); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Write a register. - * - * @param reg The register to write. - * @param val The value to write. - * @return OK on write success. - */ - int write_reg(uint8_t reg, uint8_t val); - - /** - * Read a register. - * - * @param reg The register to read. - * @param val The value read. - * @return OK on read success. - */ - int read_reg(uint8_t reg, uint8_t &val); - - /** - * Issue a measurement command. - * - * @return OK if the measurement command was successful. - */ - int measure(); - - /** - * Collect the result of the most recent measurement. - */ - int collect(); - - /** - * Convert a big-endian signed 16-bit value to a float. - * - * @param in A signed 16-bit big-endian value. - * @return The floating-point representation of the value. - */ - float meas_to_float(uint8_t in[2]); - - /** - * Check the current calibration and update device status - * - * @return 0 if calibration is ok, 1 else - */ - int check_calibration(); - - /** - * Check the current scale calibration - * - * @return 0 if scale calibration is ok, 1 else - */ - int check_scale(); - - /** - * Check the current offset calibration - * - * @return 0 if offset calibration is ok, 1 else - */ - int check_offset(); - -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); - - -HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), - _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _range_scale(0), /* default range scale from counts to gauss */ - _range_ga(1.3f), - _mag_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), - _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), - _sensor_ok(false), - _calibrated(false) -{ - // enable debug() calls - _debug_enabled = true; - - // default scaling - _scale.x_offset = 0; - _scale.x_scale = 1.0f; - _scale.y_offset = 0; - _scale.y_scale = 1.0f; - _scale.z_offset = 0; - _scale.z_scale = 1.0f; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -HMC5883::~HMC5883() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -HMC5883::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct mag_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); - - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); - - /* set range */ - set_range(_range_ga); - - ret = OK; - /* sensor is ok, but not calibrated */ - _sensor_ok = true; -out: - return ret; -} - -int HMC5883::set_range(unsigned range) -{ - uint8_t range_bits; - - if (range < 1) { - range_bits = 0x00; - _range_scale = 1.0f / 1370.0f; - _range_ga = 0.88f; - - } else if (range <= 1) { - range_bits = 0x01; - _range_scale = 1.0f / 1090.0f; - _range_ga = 1.3f; - - } else if (range <= 2) { - range_bits = 0x02; - _range_scale = 1.0f / 820.0f; - _range_ga = 1.9f; - - } else if (range <= 3) { - range_bits = 0x03; - _range_scale = 1.0f / 660.0f; - _range_ga = 2.5f; - - } else if (range <= 4) { - range_bits = 0x04; - _range_scale = 1.0f / 440.0f; - _range_ga = 4.0f; - - } else if (range <= 4.7f) { - range_bits = 0x05; - _range_scale = 1.0f / 390.0f; - _range_ga = 4.7f; - - } else if (range <= 5.6f) { - range_bits = 0x06; - _range_scale = 1.0f / 330.0f; - _range_ga = 5.6f; - - } else { - range_bits = 0x07; - _range_scale = 1.0f / 230.0f; - _range_ga = 8.1f; - } - - int ret; - - /* - * Send the command to set the range - */ - ret = write_reg(ADDR_CONF_B, (range_bits << 5)); - - if (OK != ret) - perf_count(_comms_errors); - - uint8_t range_bits_in; - ret = read_reg(ADDR_CONF_B, range_bits_in); - - if (OK != ret) - perf_count(_comms_errors); - - return !(range_bits_in == (range_bits << 5)); -} - -int -HMC5883::probe() -{ - uint8_t data[3] = {0, 0, 0}; - - _retries = 10; - - if (read_reg(ADDR_ID_A, data[0]) || - read_reg(ADDR_ID_B, data[1]) || - read_reg(ADDR_ID_C, data[2])) - debug("read_reg fail"); - - _retries = 2; - - if ((data[0] != ID_A_WHO_AM_I) || - (data[1] != ID_B_WHO_AM_I) || - (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); - return -EIO; - } - - return OK; -} - -ssize_t -HMC5883::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _oldest_report = _next_report = 0; - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(HMC5883_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; -} - -int -HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; - - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case MAGIOCSSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; - - case MAGIOCSRANGE: - return set_range(arg); - - case MAGIOCSLOWPASS: - /* not supported, no internal filtering */ - return -EINVAL; - - case MAGIOCSSCALE: - /* set new scale factors */ - memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); - /* check calibration, but not actually return an error */ - (void)check_calibration(); - return 0; - - case MAGIOCGSCALE: - /* copy out scale factors */ - memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); - return 0; - - case MAGIOCCALIBRATE: - return calibrate(filp, arg); - - case MAGIOCEXSTRAP: - return set_excitement(arg); - - case MAGIOCSELFTEST: - return check_calibration(); - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -void -HMC5883::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); -} - -void -HMC5883::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -HMC5883::cycle_trampoline(void *arg) -{ - HMC5883 *dev = (HMC5883 *)arg; - - dev->cycle(); -} - -void -HMC5883::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&HMC5883::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - if (OK != measure()) - log("measure error"); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&HMC5883::cycle_trampoline, - this, - USEC2TICK(HMC5883_CONVERSION_INTERVAL)); -} - -int -HMC5883::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); - - if (OK != ret) - perf_count(_comms_errors); - - return ret; -} - -int -HMC5883::collect() -{ -#pragma pack(push, 1) - struct { /* status register and data as read back from the device */ - uint8_t x[2]; - uint8_t z[2]; - uint8_t y[2]; - } hmc_report; -#pragma pack(pop) - struct { - int16_t x, y, z; - } report; - int ret = -EIO; - uint8_t cmd; - - - perf_begin(_sample_perf); - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - - /* - * @note We could read the status register here, which could tell us that - * we were too early and that the output registers are still being - * written. In the common case that would just slow us down, and - * we're better off just never being early. - */ - - /* get measurements from the device */ - cmd = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); - - if (ret != OK) { - perf_count(_comms_errors); - debug("data/status read error"); - goto out; - } - - /* swap the data we just received */ - report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1]; - report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1]; - report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1]; - - /* - * If any of the values are -4096, there was an internal math error in the sensor. - * Generalise this to a simple range check that will also catch some bit errors. - */ - if ((abs(report.x) > 2048) || - (abs(report.y) > 2048) || - (abs(report.z) > 2048)) - goto out; - - /* - * RAW outputs - * - * to align the sensor axes with the board, x and y need to be flipped - * and y needs to be negated - */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); - /* z remains z */ - _reports[_next_report].z_raw = report.z; - - /* scale values for output */ - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - -out: - perf_end(_sample_perf); - return ret; -} - -int HMC5883::calibrate(struct file *filp, unsigned enable) -{ - struct mag_report report; - ssize_t sz; - int ret = 1; - - // XXX do something smarter here - int fd = (int)enable; - - struct mag_scale mscale_previous = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; - - warnx("starting mag scale calibration"); - - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { - warn("failed to set queue depth"); - ret = 1; - goto out; - } - - /* start the sensor polling at 50 Hz */ - if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { - warn("failed to set 2Hz poll rate"); - ret = 1; - goto out; - } - - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { - warnx("failed to set 2.5 Ga range"); - ret = 1; - goto out; - } - - if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { - warnx("failed to enable sensor calibration mode"); - ret = 1; - goto out; - } - - if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to get scale / offsets for mag"); - ret = 1; - goto out; - } - - if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); - ret = 1; - goto out; - } - - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = ::poll(&fds, 1, 2000); - - if (ret != 1) { - warn("timed out waiting for sensor data"); - goto out; - } - - /* now go get it */ - sz = ::read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("periodic read failed"); - goto out; - - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; - } - - //warnx("periodic read %u", i); - //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - } - - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; - - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); - - float scaling[3]; - - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); - - warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - - /* set back to normal mode */ - /* Set to 1.1 Gauss */ - if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); - goto out; - } - - if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); - goto out; - } - - /* set scaling in device */ - mscale_previous.x_scale = scaling[0]; - mscale_previous.y_scale = scaling[1]; - mscale_previous.z_scale = scaling[2]; - - if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); - goto out; - } - - ret = OK; - -out: - - if (ret == OK) { - if (!check_scale()) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration finished with invalid results."); - ret = ERROR; - } - - } else { - warnx("mag scale calibration failed."); - } - - return ret; -} - -int HMC5883::check_scale() -{ - bool scale_valid; - - if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { - /* scale is one */ - scale_valid = false; - } else { - scale_valid = true; - } - - /* return 0 if calibrated, 1 else */ - return !scale_valid; -} - -int HMC5883::check_offset() -{ - bool offset_valid; - - if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { - /* offset is zero */ - offset_valid = false; - } else { - offset_valid = true; - } - - /* return 0 if calibrated, 1 else */ - return !offset_valid; -} - -int HMC5883::check_calibration() -{ - bool offset_valid = (check_offset() == OK); - bool scale_valid = (check_scale() == OK); - - if (_calibrated != (offset_valid && scale_valid)) { - warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", - (offset_valid) ? "" : "offset invalid"); - _calibrated = (offset_valid && scale_valid); - - - // XXX Change advertisement - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _calibrated, - SUBSYSTEM_TYPE_MAG}; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } - } - - /* return 0 if calibrated, 1 else */ - return (!_calibrated); -} - -int HMC5883::set_excitement(unsigned enable) -{ - int ret; - /* arm the excitement strap */ - uint8_t conf_reg; - ret = read_reg(ADDR_CONF_A, conf_reg); - - if (OK != ret) - perf_count(_comms_errors); - - if (((int)enable) < 0) { - conf_reg |= 0x01; - - } else if (enable > 0) { - conf_reg |= 0x02; - - } else { - conf_reg &= ~0x03; - } - - ret = write_reg(ADDR_CONF_A, conf_reg); - - if (OK != ret) - perf_count(_comms_errors); - - uint8_t conf_reg_ret; - read_reg(ADDR_CONF_A, conf_reg_ret); - - return !(conf_reg == conf_reg_ret); -} - -int -HMC5883::write_reg(uint8_t reg, uint8_t val) -{ - uint8_t cmd[] = { reg, val }; - - return transfer(&cmd[0], 2, nullptr, 0); -} - -int -HMC5883::read_reg(uint8_t reg, uint8_t &val) -{ - return transfer(®, 1, &val, 1); -} - -float -HMC5883::meas_to_float(uint8_t in[2]) -{ - union { - uint8_t b[2]; - int16_t w; - } u; - - u.b[0] = in[1]; - u.b[1] = in[0]; - - return (float) u.w; -} - -void -HMC5883::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); -} - -/** - * Local functions in support of the shell command. - */ -namespace hmc5883 -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -HMC5883 *g_dev; - -void start(); -void test(); -void reset(); -void info(); -int calibrate(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new HMC5883(HMC5883L_BUS); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct mag_report report; - ssize_t sz; - int ret; - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("single read"); - warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "periodic read failed"); - - warnx("periodic read %u", i); - warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - } - - errx(0, "PASS"); -} - - -/** - * Automatic scale calibration. - * - * Basic idea: - * - * output = (ext field +- 1.1 Ga self-test) * scale factor - * - * and consequently: - * - * 1.1 Ga = (excited - normal) * scale factor - * scale factor = (excited - normal) / 1.1 Ga - * - * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 - * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 - * - * By subtracting the non-excited measurement the pure 1.1 Ga reading - * can be extracted and the sensitivity of all axes can be matched. - * - * SELF TEST OPERATION - * To check the HMC5883L for proper operation, a self test feature in incorporated - * in which the sensor offset straps are excited to create a nominal field strength - * (bias field) to be measured. To implement self test, the least significant bits - * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) - * or 10 (negetive bias), e.g. 0x11 or 0x12. - * Then, by placing the mode register into single-measurement mode (0x01), - * two data acquisition cycles will be made on each magnetic vector. - * The first acquisition will be a set pulse followed shortly by measurement - * data of the external field. The second acquisition will have the offset strap - * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create - * about a Âą1.1 gauss self test field plus the external field. The first acquisition - * values will be subtracted from the second acquisition, and the net measurement - * will be placed into the data output registers. - * Since self test adds ~1.1 Gauss additional field to the existing field strength, - * using a reduced gain setting prevents sensor from being saturated and data registers - * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), - * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data - * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data - * output register. To leave the self test mode, change MS1 and MS0 bit of the - * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. - * Using the self test method described above, the user can scale sensor - */ -int calibrate() -{ - int ret; - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - - if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { - warnx("failed to enable sensor calibration mode"); - } - - close(fd); - - if (ret == OK) { - errx(0, "PASS"); - - } else { - errx(1, "FAIL"); - } -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -} // namespace - -int -hmc5883_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) - hmc5883::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - hmc5883::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - hmc5883::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - hmc5883::info(); - - /* - * Autocalibrate the scaling - */ - if (!strcmp(argv[1], "calibrate")) { - if (hmc5883::calibrate() == 0) { - errx(0, "calibration successful"); - - } else { - errx(1, "calibration failed"); - } - } - - errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); -} diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile deleted file mode 100644 index 0d2405787..000000000 --- a/apps/drivers/mb12xx/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the Maxbotix Sonar driver. -# - -APPNAME = mb12xx -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp deleted file mode 100644 index 9d0f6bddc..000000000 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ /dev/null @@ -1,840 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mb12xx.cpp - * @author Greg Hulands - * - * Driver for the Maxbotix sonar range finders connected via I2C. - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include - -#include -#include - -/* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION -#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ - -/* MB12xx Registers addresses */ - -#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ -#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ -#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ - -/* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) - -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class MB12XX : public device::I2C -{ -public: - MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); - virtual ~MB12XX(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - float _min_distance; - float _max_distance; - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - - orb_advert_t _range_finder_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE - * and MB12XX_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int measure(); - int collect(); - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); - -MB12XX::MB12XX(int bus, int address) : - I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), - _min_distance(MB12XX_MIN_DISTANCE), - _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), - _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) -{ - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -MB12XX::~MB12XX() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -MB12XX::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); - - if (_range_finder_topic < 0) - debug("failed to create sensor_range_finder object. Did you start uOrb?"); - - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; -out: - return ret; -} - -int -MB12XX::probe() -{ - return measure(); -} - -void -MB12XX::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void -MB12XX::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -MB12XX::get_minimum_distance() -{ - return _min_distance; -} - -float -MB12XX::get_maximum_distance() -{ - return _max_distance; -} - -int -MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; - - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct range_finder_report *buf = new struct range_finder_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case RANGEFINDERIOCSETMINIUMDISTANCE: - { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - case RANGEFINDERIOCSETMAXIUMDISTANCE: - { - set_maximum_distance(*(float *)arg); - return 0; - } - break; - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -MB12XX::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct range_finder_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _oldest_report = _next_report = 0; - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; -} - -int -MB12XX::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - uint8_t cmd = MB12XX_TAKE_RANGE_REG; - ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) - { - perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - return ret; - } - ret = OK; - - return ret; -} - -int -MB12XX::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - - perf_begin(_sample_perf); - - ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) - { - log("error reading from sensor: %d", ret); - return ret; - } - - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - - /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - -out: - perf_end(_sample_perf); - return ret; - - return ret; -} - -void -MB12XX::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER}; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } -} - -void -MB12XX::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -MB12XX::cycle_trampoline(void *arg) -{ - MB12XX *dev = (MB12XX *)arg; - - dev->cycle(); -} - -void -MB12XX::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MB12XX::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - if (OK != measure()) - log("measure error"); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MB12XX::cycle_trampoline, - this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); -} - -void -MB12XX::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); -} - -/** - * Local functions in support of the shell command. - */ -namespace mb12xx -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -MB12XX *g_dev; - -void start(); -void stop(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new MB12XX(MB12XX_BUS); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); - -fail: - - if (g_dev != nullptr) - { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Stop the driver - */ -void stop() -{ - if (g_dev != nullptr) - { - delete g_dev; - g_dev = nullptr; - } - else - { - errx(1, "driver not running"); - } - exit(0); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct range_finder_report report; - ssize_t sz; - int ret; - - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "periodic read failed"); - - warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); - } - - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -} // namespace - -int -mb12xx_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) - mb12xx::start(); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - mb12xx::stop(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - mb12xx::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - mb12xx::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - mb12xx::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/apps/drivers/mpu6000/Makefile b/apps/drivers/mpu6000/Makefile deleted file mode 100644 index 32df1bdae..000000000 --- a/apps/drivers/mpu6000/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the BMA180 driver. -# - -APPNAME = mpu6000 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp deleted file mode 100644 index ce7062046..000000000 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ /dev/null @@ -1,1219 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mpu6000.cpp - * - * Driver for the Invensense MPU6000 connected via SPI. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -// MPU 6000 registers -#define MPUREG_WHOAMI 0x75 -#define MPUREG_SMPLRT_DIV 0x19 -#define MPUREG_CONFIG 0x1A -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B -#define MPUREG_ACCEL_XOUT_L 0x3C -#define MPUREG_ACCEL_YOUT_H 0x3D -#define MPUREG_ACCEL_YOUT_L 0x3E -#define MPUREG_ACCEL_ZOUT_H 0x3F -#define MPUREG_ACCEL_ZOUT_L 0x40 -#define MPUREG_TEMP_OUT_H 0x41 -#define MPUREG_TEMP_OUT_L 0x42 -#define MPUREG_GYRO_XOUT_H 0x43 -#define MPUREG_GYRO_XOUT_L 0x44 -#define MPUREG_GYRO_YOUT_H 0x45 -#define MPUREG_GYRO_YOUT_L 0x46 -#define MPUREG_GYRO_ZOUT_H 0x47 -#define MPUREG_GYRO_ZOUT_L 0x48 -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 -#define MPUREG_PRODUCT_ID 0x0C - -// Configuration bits MPU 3000 and MPU 6000 (not revised)? -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 -#define BITS_DLPF_CFG_188HZ 0x01 -#define BITS_DLPF_CFG_98HZ 0x02 -#define BITS_DLPF_CFG_42HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 -#define BITS_DLPF_CFG_MASK 0x07 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 - -// Product ID Description for MPU6000 -// high 4 bits low 4 bits -// Product Name Product Revision -#define MPU6000ES_REV_C4 0x14 -#define MPU6000ES_REV_C5 0x15 -#define MPU6000ES_REV_D6 0x16 -#define MPU6000ES_REV_D7 0x17 -#define MPU6000ES_REV_D8 0x18 -#define MPU6000_REV_C4 0x54 -#define MPU6000_REV_C5 0x55 -#define MPU6000_REV_D6 0x56 -#define MPU6000_REV_D7 0x57 -#define MPU6000_REV_D8 0x58 -#define MPU6000_REV_D9 0x59 -#define MPU6000_REV_D10 0x5A - - -class MPU6000_gyro; - -class MPU6000 : public device::SPI -{ -public: - MPU6000(int bus, spi_dev_e device); - virtual ~MPU6000(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - - friend class MPU6000_gyro; - - virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); - -private: - MPU6000_gyro *_gyro; - uint8_t _product; /** product code */ - - struct hrt_call _call; - unsigned _call_interval; - - struct accel_report _accel_report; - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - struct gyro_report _gyro_report; - struct gyro_scale _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - - unsigned _reads; - perf_counter_t _sample_perf; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Read a register from the MPU6000 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - uint16_t read_reg16(unsigned reg); - - /** - * Write a register in the MPU6000 - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the MPU6000 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the MPU6000 measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_g); - - /** - * Swap a 16-bit value read from the MPU6000 to native byte order. - */ - uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - - /** - * Self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - -}; - -/** - * Helper class implementing the gyro driver node. - */ -class MPU6000_gyro : public device::CDev -{ -public: - MPU6000_gyro(MPU6000 *parent); - ~MPU6000_gyro(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - -protected: - friend class MPU6000; - - void parent_poll_notify(); -private: - MPU6000 *_parent; -}; - -/** driver 'main' command */ -extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } - -MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), - _gyro(new MPU6000_gyro(this)), - _product(0), - _call_interval(0), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _gyro_range_scale(0.0f), - _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) -{ - // disable debug() calls - _debug_enabled = false; - - // default accel scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - // default gyro scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; - - memset(&_accel_report, 0, sizeof(_accel_report)); - memset(&_gyro_report, 0, sizeof(_gyro_report)); - memset(&_call, 0, sizeof(_call)); -} - -MPU6000::~MPU6000() -{ - /* make sure we are truly inactive */ - stop(); - - /* delete the gyro subdriver */ - delete _gyro; - - /* delete the perf counter */ - perf_free(_sample_perf); -} - -int -MPU6000::init() -{ - int ret; - - /* do SPI init (and probe) first */ - ret = SPI::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - debug("SPI setup failed"); - return ret; - } - - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); - - // Chip reset - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - - // Wake up device and select GyroZ clock (better performance) - write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); - - // Disable I2C bus (recommended on datasheet) - write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - up_udelay(1000); - - // SAMPLE RATE - write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - usleep(1000); - - // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) - // was 90 Hz, but this ruins quality and does not improve the - // system response - _set_dlpf_filter(20); - usleep(1000); - // Gyro scale 2000 deg/s () - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); - usleep(1000); - - // correct gyro scale factors - // scale to rad/s in SI units - // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s - // scaling factor: - // 1/(2^15)*(2000/180)*PI - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; - _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); - _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; - - // product-specific scaling - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - break; - - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug - default: - // Accel scale 8g (4096 LSB/g) - write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); - break; - } - - // Correct accel scale factors of 4096 LSB/g - // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - _accel_range_scale = (9.81f / 4096.0f); - _accel_range_m_s2 = 8.0f * 9.81f; - - usleep(1000); - - // INT CFG => Interrupt on Data Ready - write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready - usleep(1000); - write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read - usleep(1000); - - // Oscillator set - // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); - usleep(1000); - - /* do CDev init for the gyro device node, keep it optional */ - int gyro_ret = _gyro->init(); - - if (gyro_ret != OK) { - _gyro_topic = -1; - } - - return ret; -} - -int -MPU6000::probe() -{ - - /* look for a product ID we recognise */ - _product = read_reg(MPUREG_PRODUCT_ID); - - // verify product revision - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - debug("ID 0x%02x", _product); - return OK; - } - - debug("unexpected ID 0x%02x", _product); - return -EIO; -} - -/* - set the DLPF filter frequency. This affects both accel and gyro. - */ -void -MPU6000::_set_dlpf_filter(uint16_t frequency_hz) -{ - uint8_t filter; - - /* - choose next highest filter frequency available - */ - if (frequency_hz <= 5) { - filter = BITS_DLPF_CFG_5HZ; - } else if (frequency_hz <= 10) { - filter = BITS_DLPF_CFG_10HZ; - } else if (frequency_hz <= 20) { - filter = BITS_DLPF_CFG_20HZ; - } else if (frequency_hz <= 42) { - filter = BITS_DLPF_CFG_42HZ; - } else if (frequency_hz <= 98) { - filter = BITS_DLPF_CFG_98HZ; - } else if (frequency_hz <= 188) { - filter = BITS_DLPF_CFG_188HZ; - } else if (frequency_hz <= 256) { - filter = BITS_DLPF_CFG_256HZ_NOLPF2; - } else { - filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } - write_reg(MPUREG_CONFIG, filter); -} - -ssize_t -MPU6000::read(struct file *filp, char *buffer, size_t buflen) -{ - int ret = 0; - - /* buffer must be large enough */ - if (buflen < sizeof(_accel_report)) - return -ENOSPC; - - /* if automatic measurement is not enabled */ - if (_call_interval == 0) - measure(); - - /* copy out the latest reports */ - memcpy(buffer, &_accel_report, sizeof(_accel_report)); - ret = sizeof(_accel_report); - - return ret; -} - -int -MPU6000::self_test() -{ - if (_reads == 0) { - measure(); - } - - /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; -} - -ssize_t -MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) -{ - int ret = 0; - - /* buffer must be large enough */ - if (buflen < sizeof(_gyro_report)) - return -ENOSPC; - - /* if automatic measurement is not enabled */ - if (_call_interval == 0) - measure(); - - /* copy out the latest report */ - memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); - ret = sizeof(_gyro_report); - - return ret; -} - -int -MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* XXX 500Hz is just a wild guess */ - return ioctl(filp, SENSORIOCSPOLLRATE, 500); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_interval; - - case SENSORIOCSQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; - - case SENSORIOCGQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; - - - case ACCELIOCSSAMPLERATE: - case ACCELIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; - - case ACCELIOCSLOWPASS: - case ACCELIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); - return OK; - - case ACCELIOCSSCALE: - { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; - } - } - - case ACCELIOCGSCALE: - /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); - return OK; - - case ACCELIOCSRANGE: - case ACCELIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_rad_s = 8.0f * 9.81f; - return -EINVAL; - - case ACCELIOCSELFTEST: - return self_test(); - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -int -MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - /* these are shared with the accel side */ - case SENSORIOCSPOLLRATE: - case SENSORIOCGPOLLRATE: - case SENSORIOCSQUEUEDEPTH: - case SENSORIOCGQUEUEDEPTH: - case SENSORIOCRESET: - return ioctl(filp, cmd, arg); - - case GYROIOCSSAMPLERATE: - case GYROIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; - - case GYROIOCSLOWPASS: - case GYROIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); - return OK; - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); - return OK; - - case GYROIOCGSCALE: - /* copy scale out */ - memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); - return OK; - - case GYROIOCSRANGE: - case GYROIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _gyro_range_scale = xx - // _gyro_range_m_s2 = xx - return -EINVAL; - - case GYROIOCSELFTEST: - return self_test(); - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -MPU6000::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -uint16_t -MPU6000::read_reg16(unsigned reg) -{ - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; -} - -void -MPU6000::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -MPU6000::set_range(unsigned max_g) -{ -#if 0 - uint8_t rangebits; - float rangescale; - - if (max_g > 16) { - return -ERANGE; - - } else if (max_g > 8) { /* 16G */ - rangebits = OFFSET_LSB1_RANGE_16G; - rangescale = 1.98; - - } else if (max_g > 4) { /* 8G */ - rangebits = OFFSET_LSB1_RANGE_8G; - rangescale = 0.99; - - } else if (max_g > 3) { /* 4G */ - rangebits = OFFSET_LSB1_RANGE_4G; - rangescale = 0.5; - - } else if (max_g > 2) { /* 3G */ - rangebits = OFFSET_LSB1_RANGE_3G; - rangescale = 0.38; - - } else if (max_g > 1) { /* 2G */ - rangebits = OFFSET_LSB1_RANGE_2G; - rangescale = 0.25; - - } else { /* 1G */ - rangebits = OFFSET_LSB1_RANGE_1G; - rangescale = 0.13; - } - - /* adjust sensor configuration */ - modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); - _range_scale = rangescale; -#endif - return OK; -} - -void -MPU6000::start() -{ - /* make sure we are stopped first */ - stop(); - - /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); -} - -void -MPU6000::stop() -{ - hrt_cancel(&_call); -} - -void -MPU6000::measure_trampoline(void *arg) -{ - MPU6000 *dev = (MPU6000 *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -MPU6000::measure() -{ -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) - - struct Report { - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; - int16_t temp; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - } report; - - /* start measuring */ - perf_begin(_sample_perf); - - /* - * Fetch the full set of measurements from the MPU6000 in one pass. - */ - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) - return; - - /* count measurement */ - _reads++; - - /* - * Convert from big to little endian - */ - - report.accel_x = int16_t_from_bytes(mpu_report.accel_x); - report.accel_y = int16_t_from_bytes(mpu_report.accel_y); - report.accel_z = int16_t_from_bytes(mpu_report.accel_z); - - report.temp = int16_t_from_bytes(mpu_report.temp); - - report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); - report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); - report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - - /* - * Swap axes and negate y - */ - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - - /* - * Adjust and scale results to m/s^2. - */ - _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); - - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - /* NOTE: Axes have been swapped to match the board a few lines above. */ - - _accel_report.x_raw = report.accel_x; - _accel_report.y_raw = report.accel_y; - _accel_report.z_raw = report.accel_z; - - _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - _accel_report.scaling = _accel_range_scale; - _accel_report.range_m_s2 = _accel_range_m_s2; - - _accel_report.temperature_raw = report.temp; - _accel_report.temperature = (report.temp) / 361.0f + 35.0f; - - _gyro_report.x_raw = report.gyro_x; - _gyro_report.y_raw = report.gyro_y; - _gyro_report.z_raw = report.gyro_z; - - _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - _gyro_report.scaling = _gyro_range_scale; - _gyro_report.range_rad_s = _gyro_range_rad_s; - - _gyro_report.temperature_raw = report.temp; - _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); - - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); - } - - /* stop measuring */ - perf_end(_sample_perf); -} - -void -MPU6000::print_info() -{ - printf("reads: %u\n", _reads); -} - -MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), - _parent(parent) -{ -} - -MPU6000_gyro::~MPU6000_gyro() -{ -} - -void -MPU6000_gyro::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -ssize_t -MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->gyro_read(filp, buffer, buflen); -} - -int -MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - return _parent->gyro_ioctl(filp, cmd, arg); -} - -/** - * Local functions in support of the shell command. - */ -namespace mpu6000 -{ - -MPU6000 *g_dev; - -void start(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd = -1; - int fd_gyro = -1; - struct accel_report a_report; - struct gyro_report g_report; - ssize_t sz; - - /* get the driver */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); - - /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); - - if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); - - /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) - err(1, "reset to manual polling"); - - /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) - err(1, "immediate acc read failed"); - - warnx("single read"); - warnx("time: %lld", a_report.timestamp); - warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); - warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); - warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); - warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); - warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); - warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); - warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); - - /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) - err(1, "immediate gyro read failed"); - - warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); - warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); - warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); - warnx("gyro x: \t%d\traw", (int)g_report.x_raw); - warnx("gyro y: \t%d\traw", (int)g_report.y_raw); - warnx("gyro z: \t%d\traw", (int)g_report.z_raw); - warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); - - warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); - warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); - - - /* XXX add poll-rate tests here too */ - - reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - - -} // namespace - -int -mpu6000_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - - */ - if (!strcmp(argv[1], "start")) - mpu6000::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - mpu6000::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - mpu6000::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - mpu6000::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/apps/drivers/ms5611/Makefile b/apps/drivers/ms5611/Makefile deleted file mode 100644 index d8e67cba2..000000000 --- a/apps/drivers/ms5611/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# MS5611 driver -# - -APPNAME = ms5611 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp deleted file mode 100644 index 59ab5936e..000000000 --- a/apps/drivers/ms5611/ms5611.cpp +++ /dev/null @@ -1,1214 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include - -#include -#include - -#include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; - -/** - * Grody hack for crc4() - */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) - -class MS5611 : public device::I2C -{ -public: - MS5611(int bus); - virtual ~MS5611(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - union ms5611_prom_u _prom; - - struct work_s _work; - unsigned _measure_ticks; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; - - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - - /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ - - orb_advert_t _baro_topic; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start_cycle(); - - /** - * Stop the automatic measurement state machine. - */ - void stop_cycle(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Issue a measurement command for the current state. - * - * @return OK if the measurement command was successful. - */ - int measure(); - - /** - * Collect the result of the most recent measurement. - */ - int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - int read_prom(); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ - -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ - -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - - -MS5611::MS5611(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), - _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _collect_phase(false), - _measure_phase(0), - _TEMP(0), - _OFF(0), - _SENS(0), - _msl_pressure(101325), - _baro_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), - _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) -{ - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -MS5611::~MS5611() -{ - /* make sure we are truly inactive */ - stop_cycle(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -MS5611::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - - ret = OK; -out: - return ret; -} - -int -MS5611::probe() -{ - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } - - return -EIO; -} - -int -MS5611::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - -ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct baro_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; -} - -int -MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case BAROIOCSMSLPRESSURE: - - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; - - _msl_pressure = arg; - return OK; - - case BAROIOCGMSLPRESSURE: - return _msl_pressure; - - default: - break; - } - - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); -} - -void -MS5611::start_cycle() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); -} - -void -MS5611::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611::cycle_trampoline(void *arg) -{ - MS5611 *dev = (MS5611 *)arg; - - dev->cycle(); -} - -void -MS5611::cycle() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -int -MS5611::measure() -{ - int ret; - - perf_begin(_measure_perf); - - /* - * In phase zero, request temperature; in other phases, request pressure. - */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; - - /* - * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. - */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); - - if (OK != ret) - perf_count(_comms_errors); - - perf_end(_measure_perf); - - return ret; -} - -int -MS5611::collect() -{ - int ret; - uint8_t cmd; - uint8_t data[3]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - cmd = 0; - - perf_begin(_sample_perf); - - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - - ret = transfer(&cmd, 1, &data[0], 3); - if (ret != OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - - /* handle a measurement */ - if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } - - } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - - /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us - */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - } - - /* update the measurement state machine */ - INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); - - perf_end(_sample_perf); - - return OK; -} - -int -MS5611::cmd_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; - - return result; -} - -int -MS5611::read_prom() -{ - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; - - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } - - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} - -bool -MS5611::crc4(uint16_t *n_prom) -{ - int16_t cnt; - uint16_t n_rem; - uint16_t crc_read; - uint8_t n_bit; - - n_rem = 0x00; - - /* save the read crc */ - crc_read = n_prom[7]; - - /* remove CRC byte */ - n_prom[7] = (0xFF00 & (n_prom[7])); - - for (cnt = 0; cnt < 16; cnt++) { - /* uneven bytes */ - if (cnt & 1) { - n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); - } - - for (n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & 0x8000) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - /* final 4 bit remainder is CRC value */ - n_rem = (0x000F & (n_rem >> 12)); - n_prom[7] = crc_read; - - /* return true if CRCs match */ - return (0x000F & crc_read) == (n_rem ^ 0x00); -} - -void -MS5611::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", _SENS); - printf("OFF: %lld\n", _OFF); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -MS5611 *g_dev; - -void start(); -void test(); -void reset(); -void info(); -void calibrate(unsigned altitude); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct baro_report report; - ssize_t sz; - int ret; - - int fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("single read"); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", report.timestamp); - - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "periodic read failed"); - - warnx("periodic read %u", i); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", report.timestamp); - } - - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -/** - * Calculate actual MSL pressure given current altitude - */ -void -calibrate(unsigned altitude) -{ - struct baro_report report; - float pressure; - float p1; - - int fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); - - /* start the sensor polling at max */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) - errx(1, "failed to set poll rate"); - - /* average a few measurements */ - pressure = 0.0f; - - for (unsigned i = 0; i < 20; i++) { - struct pollfd fds; - int ret; - ssize_t sz; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 1000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "sensor read failed"); - - pressure += report.pressure; - } - - pressure /= 20; /* average */ - pressure /= 10; /* scale from millibar to kPa */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); - - p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - - warnx("calculated MSL pressure %10.4fkPa", p1); - - /* save as integer Pa */ - p1 *= 1000.0f; - - if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) - err(1, "BAROIOCSMSLPRESSURE"); - - exit(0); -} - -} // namespace - -int -ms5611_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) - ms5611::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - ms5611::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - ms5611::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - ms5611::info(); - - /* - * Perform MSL pressure calibration given an altitude in metres - */ - if (!strcmp(argv[1], "calibrate")) { - if (argc < 2) - errx(1, "missing altitude"); - - long altitude = strtol(argv[2], nullptr, 10); - - ms5611::calibrate(altitude); - } - - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/apps/hott_telemetry/Makefile b/apps/hott_telemetry/Makefile deleted file mode 100644 index 8d5faa3b7..000000000 --- a/apps/hott_telemetry/Makefile +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Graupner HoTT Telemetry application. -# - -# The following line is required for accessing UARTs directly. -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -APPNAME = hott_telemetry -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c deleted file mode 100644 index 31c9247aa..000000000 --- a/apps/hott_telemetry/hott_telemetry_main.c +++ /dev/null @@ -1,312 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hott_telemetry_main.c - * - * Graupner HoTT Telemetry implementation. - * - * The HoTT receiver polls each device at a regular interval at which point - * a data packet can be returned if necessary. - * - * TODO: Add support for at least the vario and GPS sensor data. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "messages.h" - -/* The following are equired for UART direct manipulation. */ -#include -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" - -static int thread_should_exit = false; /**< Deamon exit flag */ -static int thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static char *daemon_name = "hott_telemetry"; -static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d ]"; - - -/* A little console messaging experiment - console helper macro */ -#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1); -#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); -#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg); -/** - * Deamon management function. - */ -__EXPORT int hott_telemetry_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int hott_telemetry_thread_main(int argc, char *argv[]); - -static int read_data(int uart, int *id); -static int send_data(int uart, uint8_t *buffer, int size); - -static int open_uart(const char *device, struct termios *uart_config_original) -{ - /* baud rate */ - int speed = B19200; - int uart; - - /* open uart */ - uart = open(device, O_RDWR | O_NOCTTY); - - if (uart < 0) { - char msg[80]; - sprintf(msg, "Error opening port: %s\n", device); - FATAL_MSG(msg); - } - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - - /* Back up the original uart configuration to restore it after exit */ - char msg[80]; - - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state); - close(uart); - FATAL_MSG(msg); - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", - device, termios_state); - close(uart); - FATAL_MSG(msg); - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device); - close(uart); - FATAL_MSG(msg); - } - - /* Activate single wire mode */ - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - - return uart; -} - -int read_data(int uart, int *id) -{ - const int timeout = 1000; - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; - - char mode; - - if (poll(fds, 1, timeout) > 0) { - /* Get the mode: binary or text */ - read(uart, &mode, 1); - /* Read the device ID being polled */ - read(uart, id, 1); - - /* if we have a binary mode request */ - if (mode != BINARY_MODE_REQUEST_ID) { - return ERROR; - } - - } else { - ERROR_MSG("UART timeout on TX/RX port"); - return ERROR; - } - - return OK; -} - -int send_data(int uart, uint8_t *buffer, int size) -{ - usleep(POST_READ_DELAY_IN_USECS); - - uint16_t checksum = 0; - - for (int i = 0; i < size; i++) { - if (i == size - 1) { - /* Set the checksum: the first uint8_t is taken as the checksum. */ - buffer[i] = checksum & 0xff; - - } else { - checksum += buffer[i]; - } - - write(uart, &buffer[i], 1); - - /* Sleep before sending the next byte. */ - usleep(POST_WRITE_DELAY_IN_USECS); - } - - /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ - /* TODO: Fix this!! */ - uint8_t dummy[size]; - read(uart, &dummy, size); - - return OK; -} - -int hott_telemetry_thread_main(int argc, char *argv[]) -{ - INFO_MSG("starting"); - - thread_running = true; - - char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ - - /* read commandline arguments */ - for (int i = 0; i < argc && argv[i]; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set - if (argc > i + 1) { - device = argv[i + 1]; - - } else { - thread_running = false; - ERROR_MSG("missing parameter to -d"); - ERROR_MSG(commandline_usage); - exit(1); - } - } - } - - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - int uart = open_uart(device, &uart_config_original); - - if (uart < 0) { - ERROR_MSG("Failed opening HoTT UART, exiting."); - thread_running = false; - exit(ERROR); - } - - messages_init(); - - uint8_t buffer[MESSAGE_BUFFER_SIZE]; - int size = 0; - int id = 0; - - while (!thread_should_exit) { - if (read_data(uart, &id) == OK) { - switch (id) { - case ELECTRIC_AIR_MODULE: - build_eam_response(buffer, &size); - break; - - default: - continue; // Not a module we support. - } - - send_data(uart, buffer, size); - } - } - - INFO_MSG("exiting"); - - close(uart); - - thread_running = false; - - return 0; -} - -/** - * Process command line arguments and tart the daemon. - */ -int hott_telemetry_main(int argc, char *argv[]) -{ - if (argc < 1) { - ERROR_MSG("missing command"); - ERROR_MSG(commandline_usage); - exit(1); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - INFO_MSG("deamon already running"); - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("hott_telemetry", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 2048, - hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - INFO_MSG("daemon is running"); - - } else { - INFO_MSG("daemon not started"); - } - - exit(0); - } - - ERROR_MSG("unrecognized command"); - ERROR_MSG(commandline_usage); - exit(1); -} - - - diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c deleted file mode 100644 index 8bfb99773..000000000 --- a/apps/hott_telemetry/messages.c +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file messages.c - * - */ - -#include "messages.h" - -#include -#include -#include -#include -#include - -static int battery_sub = -1; -static int sensor_sub = -1; - -void messages_init(void) -{ - battery_sub = orb_subscribe(ORB_ID(battery_status)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); -} - -void build_eam_response(uint8_t *buffer, int *size) -{ - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* get a local copy of the battery data */ - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - - struct eam_module_msg msg; - *size = sizeof(msg); - memset(&msg, 0, *size); - - msg.start = START_BYTE; - msg.eam_sensor_id = ELECTRIC_AIR_MODULE; - msg.sensor_id = EAM_SENSOR_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); - msg.temperature2 = TEMP_ZERO_CELSIUS; - msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); - msg.altitude_L = (uint8_t)alt & 0xff; - msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - - msg.stop = STOP_BYTE; - - memcpy(buffer, &msg, *size); -} \ No newline at end of file diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h deleted file mode 100644 index 44001e04f..000000000 --- a/apps/hott_telemetry/messages.h +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file messages.h - * - * Graupner HoTT Telemetry message generation. - * - */ -#ifndef MESSAGES_H_ -#define MESSAGES_H - -#include - -/* The buffer size used to store messages. This must be at least as big as the number of - * fields in the largest message struct. - */ -#define MESSAGE_BUFFER_SIZE 50 - -/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. - * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting - * the message after the read which takes some milliseconds. - */ -#define POST_READ_DELAY_IN_USECS 4000 -/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower - * values can be used in practise though. - */ -#define POST_WRITE_DELAY_IN_USECS 2000 - -// Protocol constants. -#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. -#define START_BYTE 0x7c -#define STOP_BYTE 0x7d -#define TEMP_ZERO_CELSIUS 0x14 - -/* Electric Air Module (EAM) constants. */ -#define ELECTRIC_AIR_MODULE 0x8e -#define EAM_SENSOR_ID 0xe0 - -/* The Electric Air Module message. */ -struct eam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t eam_sensor_id; /**< EAM sensor ID */ - uint8_t warning; - uint8_t sensor_id; /**< Sensor ID, why different? */ - uint8_t alarm_inverse1; - uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2_L; - uint8_t cell3_L; - uint8_t cell4_L; - uint8_t cell5_L; - uint8_t cell6_L; - uint8_t cell7_L; - uint8_t cell1_H; - uint8_t cell2_H; - uint8_t cell3_H; - uint8_t cell4_H; - uint8_t cell5_H; - uint8_t cell6_H; - uint8_t cell7_H; - uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt1_voltage_H; - uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt2_voltage_H; - uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ - uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ - uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ - uint8_t main_voltage_H; - uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ - uint8_t battery_capacity_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ - uint8_t climbrate_H; - uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ - uint8_t rpm_H; - uint8_t electric_min; /**< Flight time in minutes. */ - uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ - uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ -}; - -void messages_init(void); -void build_eam_response(uint8_t *buffer, int *size); - -#endif /* MESSAGES_H_ */ diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 57d949258..00aec3f8a 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -15,6 +15,15 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 +MODULES += drivers/bma180 +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott_telemetry +MODULES += drivers/blinkm MODULES += modules/sensors # @@ -66,19 +75,11 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, blinkm, , 2048, blinkm_main ) \ - $(call _B, bma180, , 2048, bma180_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, fixedwing_att_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_att_control_main ) \ $(call _B, fixedwing_pos_control, SCHED_PRIORITY_MAX-30, 2048, fixedwing_pos_control_main ) \ - $(call _B, gps, , 2048, gps_main ) \ - $(call _B, hil, , 2048, hil_main ) \ - $(call _B, hmc5883, , 4096, hmc5883_main ) \ - $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ - $(call _B, mpu6000, , 4096, mpu6000_main ) \ - $(call _B, ms5611, , 2048, ms5611_main ) \ $(call _B, multirotor_att_control, SCHED_PRIORITY_MAX-15, 2048, multirotor_att_control_main) \ $(call _B, multirotor_pos_control, SCHED_PRIORITY_MAX-25, 2048, multirotor_pos_control_main) \ $(call _B, sercon, , 2048, sercon_main ) \ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp new file mode 100644 index 000000000..3fabfd9a5 --- /dev/null +++ b/src/drivers/blinkm/blinkm.cpp @@ -0,0 +1,919 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file blinkm.cpp + * + * Driver for the BlinkM LED controller connected via I2C. + * + * Connect the BlinkM to I2C3 and put the following line to the rc startup-script: + * blinkm start + * + * To start the system monitor put in the next line after the blinkm start: + * blinkm systemmonitor + * + * + * Description: + * After startup, the Application checked how many lipo cells are connected to the System. + * The recognized number off cells, will be blinked 5 times in purple color. + * 2 Cells = 2 blinks + * ... + * 5 Cells = 5 blinks + * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. + * + * System disarmed: + * The BlinkM should lit solid red. + * + * System armed: + * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. + * + * X-X-X-X-_-_-_-_-_-_- + * ------------------------- + * G G G M + * P P P O + * S S S D + * E + * + * (X = on, _=off) + * + * The first 3 blinks indicates the status of the GPS-Signal (red): + * 0-4 satellites = X-X-X-X-_-_-_-_-_-_- + * 5 satellites = X-X-_-X-_-_-_-_-_-_- + * 6 satellites = X-_-_-X-_-_-_-_-_-_- + * >=7 satellites = _-_-_-X-_-_-_-_-_-_- + * If no GPS is found the first 3 blinks are white + * + * The fourth Blink indicates the Flightmode: + * MANUAL : amber + * STABILIZED : yellow + * HOLD : blue + * AUTO : green + * + * Battery Warning (low Battery Level): + * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X + * + * Battery Alert (critical Battery Level) + * Continuously blinking in red X-X-X-X-X-X-X-X-X-X + * + * General Error (no uOrb Data) + * Continuously blinking in white X-X-X-X-X-X-X-X-X-X + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +static const float MAX_CELL_VOLTAGE = 4.3f; +static const int LED_ONTIME = 120; +static const int LED_OFFTIME = 120; +static const int LED_BLINK = 1; +static const int LED_NOBLINK = 0; + +class BlinkM : public device::I2C +{ +public: + BlinkM(int bus, int blinkm); + virtual ~BlinkM(); + + + virtual int init(); + virtual int probe(); + virtual int setMode(int mode); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + static const char *const script_names[]; + +private: + enum ScriptID { + USER = 0, + RGB, + WHITE_FLASH, + RED_FLASH, + GREEN_FLASH, + BLUE_FLASH, + CYAN_FLASH, + MAGENTA_FLASH, + YELLOW_FLASH, + BLACK, + HUE_CYCLE, + MOOD_LIGHT, + VIRTUAL_CANDLE, + WATER_REFLECTIONS, + OLD_NEON, + THE_SEASONS, + THUNDERSTORM, + STOP_LIGHT, + MORSE_CODE + }; + + enum ledColors { + LED_OFF, + LED_RED, + LED_YELLOW, + LED_PURPLE, + LED_GREEN, + LED_BLUE, + LED_WHITE, + LED_AMBER + }; + + work_s _work; + + int led_color_1; + int led_color_2; + int led_color_3; + int led_color_4; + int led_color_5; + int led_color_6; + int led_color_7; + int led_color_8; + int led_blink; + + bool systemstate_run; + + void setLEDColor(int ledcolor); + static void led_trampoline(void *arg); + void led(); + + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + + int fade_rgb(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb(uint8_t h, uint8_t s, uint8_t b); + + int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); + + int set_fade_speed(uint8_t s); + + int play_script(uint8_t script_id); + int play_script(const char *script_name); + int stop_script(); + + int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); + int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); + int set_script(uint8_t length, uint8_t repeats); + + int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); + + int get_firmware_version(uint8_t version[2]); +}; + +/* for now, we only support one BlinkM */ +namespace +{ + BlinkM *g_blinkm; +} + +/* list of script names, must match script ID numbers */ +const char *const BlinkM::script_names[] = { + "USER", + "RGB", + "WHITE_FLASH", + "RED_FLASH", + "GREEN_FLASH", + "BLUE_FLASH", + "CYAN_FLASH", + "MAGENTA_FLASH", + "YELLOW_FLASH", + "BLACK", + "HUE_CYCLE", + "MOOD_LIGHT", + "VIRTUAL_CANDLE", + "WATER_REFLECTIONS", + "OLD_NEON", + "THE_SEASONS", + "THUNDERSTORM", + "STOP_LIGHT", + "MORSE_CODE", + nullptr +}; + + +extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); + +BlinkM::BlinkM(int bus, int blinkm) : + I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000), + led_color_1(LED_OFF), + led_color_2(LED_OFF), + led_color_3(LED_OFF), + led_color_4(LED_OFF), + led_color_5(LED_OFF), + led_color_6(LED_OFF), + led_color_7(LED_OFF), + led_color_8(LED_OFF), + led_blink(LED_NOBLINK), + systemstate_run(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +BlinkM::~BlinkM() +{ +} + +int +BlinkM::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + stop_script(); + set_rgb(0,0,0); + + return OK; +} + +int +BlinkM::setMode(int mode) +{ + if(mode == 1) { + if(systemstate_run == false) { + stop_script(); + set_rgb(0,0,0); + systemstate_run = true; + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); + } + } else { + systemstate_run = false; + } + + return OK; +} + +int +BlinkM::probe() +{ + uint8_t version[2]; + int ret; + + ret = get_firmware_version(version); + + if (ret == OK) + log("found BlinkM firmware version %c%c", version[1], version[0]); + + return ret; +} + +int +BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case BLINKM_PLAY_SCRIPT_NAMED: + if (arg == 0) { + ret = EINVAL; + break; + } + ret = play_script((const char *)arg); + break; + + case BLINKM_PLAY_SCRIPT: + ret = play_script(arg); + break; + + case BLINKM_SET_USER_SCRIPT: { + if (arg == 0) { + ret = EINVAL; + break; + } + + unsigned lines = 0; + const uint8_t *script = (const uint8_t *)arg; + + while ((lines < 50) && (script[1] != 0)) { + ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); + if (ret != OK) + break; + script += 5; + } + if (ret == OK) + ret = set_script(lines, 0); + break; + } + + default: + break; + } + + return ret; +} + + +void +BlinkM::led_trampoline(void *arg) +{ + BlinkM *bm = (BlinkM *)arg; + + bm->led(); +} + + + +void +BlinkM::led() +{ + + static int vehicle_status_sub_fd; + static int vehicle_gps_position_sub_fd; + + static int num_of_cells = 0; + static int detected_cells_runcount = 0; + + static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0}; + static int t_led_blink = 0; + static int led_thread_runcount=0; + static int led_interval = 1000; + + static int no_data_vehicle_status = 0; + static int no_data_vehicle_gps_position = 0; + + static bool topic_initialized = false; + static bool detected_cells_blinked = false; + static bool led_thread_ready = true; + + int num_of_used_sats = 0; + + if(!topic_initialized) { + vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(vehicle_status_sub_fd, 1000); + + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(vehicle_gps_position_sub_fd, 1000); + + topic_initialized = true; + } + + if(led_thread_ready == true) { + if(!detected_cells_blinked) { + if(num_of_cells > 0) { + t_led_color[0] = LED_PURPLE; + } + if(num_of_cells > 1) { + t_led_color[1] = LED_PURPLE; + } + if(num_of_cells > 2) { + t_led_color[2] = LED_PURPLE; + } + if(num_of_cells > 3) { + t_led_color[3] = LED_PURPLE; + } + if(num_of_cells > 4) { + t_led_color[4] = LED_PURPLE; + } + t_led_color[5] = LED_OFF; + t_led_color[6] = LED_OFF; + t_led_color[7] = LED_OFF; + t_led_blink = LED_BLINK; + } else { + t_led_color[0] = led_color_1; + t_led_color[1] = led_color_2; + t_led_color[2] = led_color_3; + t_led_color[3] = led_color_4; + t_led_color[4] = led_color_5; + t_led_color[5] = led_color_6; + t_led_color[6] = led_color_7; + t_led_color[7] = led_color_8; + t_led_blink = led_blink; + } + led_thread_ready = false; + } + + if (led_thread_runcount & 1) { + if (t_led_blink) + setLEDColor(LED_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); + //led_interval = (led_thread_runcount & 1) : LED_ONTIME; + led_interval = LED_ONTIME; + } + + if (led_thread_runcount == 15) { + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s vehicle_gps_position_raw; + + memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); + memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); + + bool new_data_vehicle_status; + bool new_data_vehicle_gps_position; + + orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); + + if (new_data_vehicle_status) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + no_data_vehicle_status = 0; + } else { + no_data_vehicle_status++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); + + if (new_data_vehicle_gps_position) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); + no_data_vehicle_gps_position = 0; + } else { + no_data_vehicle_gps_position++; + if(no_data_vehicle_gps_position >= 3) + no_data_vehicle_gps_position = 3; + } + + + + /* get number of used satellites in navigation */ + num_of_used_sats = 0; + //for(int satloop=0; satloop<20; satloop++) { + for(int satloop=0; satloop checking cells\n"); + for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + } + printf(" cells found:%d\n", num_of_cells); + + } else { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* LED Pattern for battery low warning */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_color_7 = LED_YELLOW; + led_color_8 = LED_YELLOW; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + /* LED Pattern for battery critical alerting */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_RED; + led_color_5 = LED_RED; + led_color_6 = LED_RED; + led_color_7 = LED_RED; + led_color_8 = LED_RED; + led_blink = LED_BLINK; + + } else { + /* no battery warnings here */ + + if(vehicle_status_raw.flag_system_armed == false) { + /* system not armed */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_RED; + led_color_5 = LED_RED; + led_color_6 = LED_RED; + led_color_7 = LED_RED; + led_color_8 = LED_RED; + led_blink = LED_NOBLINK; + + } else { + /* armed system - initial led pattern */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_OFF; + led_color_5 = LED_OFF; + led_color_6 = LED_OFF; + led_color_7 = LED_OFF; + led_color_8 = LED_OFF; + led_blink = LED_BLINK; + + /* handle 4th led - flightmode indicator */ + switch((int)vehicle_status_raw.flight_mode) { + case VEHICLE_FLIGHT_MODE_MANUAL: + led_color_4 = LED_AMBER; + break; + + case VEHICLE_FLIGHT_MODE_STAB: + led_color_4 = LED_YELLOW; + break; + + case VEHICLE_FLIGHT_MODE_HOLD: + led_color_4 = LED_BLUE; + break; + + case VEHICLE_FLIGHT_MODE_AUTO: + led_color_4 = LED_GREEN; + break; + } + + if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { + /* handling used satīs */ + if(num_of_used_sats >= 7) { + led_color_1 = LED_OFF; + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; + } else if(num_of_used_sats == 6) { + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; + } else if(num_of_used_sats == 5) { + led_color_3 = LED_OFF; + } + + } else { + /* no vehicle_gps_position data */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + + } + + } + } + } + } else { + /* LED Pattern for general Error - no vehicle_status can retrieved */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + led_color_4 = LED_WHITE; + led_color_5 = LED_WHITE; + led_color_6 = LED_WHITE; + led_color_7 = LED_WHITE; + led_color_8 = LED_WHITE; + led_blink = LED_BLINK; + + } + + /* + printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", + vehicle_status_raw.voltage_battery, + vehicle_status_raw.flag_system_armed, + vehicle_status_raw.flight_mode, + num_of_cells, + no_data_vehicle_status, + no_data_vehicle_gps_position, + num_of_used_sats, + vehicle_gps_position_raw.fix_type, + vehicle_gps_position_raw.satellites_visible); + */ + + led_thread_runcount=0; + led_thread_ready = true; + led_interval = LED_OFFTIME; + + if(detected_cells_runcount < 4){ + detected_cells_runcount++; + } else { + detected_cells_blinked = true; + } + + } else { + led_thread_runcount++; + } + + if(systemstate_run == true) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } else { + stop_script(); + set_rgb(0,0,0); + } +} + +void BlinkM::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_OFF: // off + set_rgb(0,0,0); + break; + case LED_RED: // red + set_rgb(255,0,0); + break; + case LED_YELLOW: // yellow + set_rgb(255,70,0); + break; + case LED_PURPLE: // purple + set_rgb(255,0,255); + break; + case LED_GREEN: // green + set_rgb(0,255,0); + break; + case LED_BLUE: // blue + set_rgb(0,0,255); + break; + case LED_WHITE: // white + set_rgb(255,255,255); + break; + case LED_AMBER: // amber + set_rgb(255,20,0); + break; + } +} + +int +BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'n', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'c', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'h', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'C', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'H', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::set_fade_speed(uint8_t s) +{ + const uint8_t msg[2] = { 'f', s }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(uint8_t script_id) +{ + const uint8_t msg[4] = { 'p', script_id, 0, 0 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(const char *script_name) +{ + /* handle HTML colour encoding */ + if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { + char code[3]; + uint8_t r, g, b; + + code[2] = '\0'; + + code[0] = script_name[1]; + code[1] = script_name[2]; + r = strtol(code, 0, 16); + code[0] = script_name[3]; + code[1] = script_name[4]; + g = strtol(code, 0, 16); + code[0] = script_name[5]; + code[1] = script_name[6]; + b = strtol(code, 0, 16); + + stop_script(); + return set_rgb(r, g, b); + } + + for (unsigned i = 0; script_names[i] != nullptr; i++) + if (!strcasecmp(script_name, script_names[i])) + return play_script(i); + + return -1; +} + +int +BlinkM::stop_script() +{ + const uint8_t msg[1] = { 'o' }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) +{ + const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) +{ + const uint8_t msg[3] = { 'R', 0, line }; + uint8_t result[5]; + + int ret = transfer(msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + ticks = result[0]; + cmd[0] = result[1]; + cmd[1] = result[2]; + cmd[2] = result[3]; + cmd[3] = result[4]; + } + + return ret; +} + +int +BlinkM::set_script(uint8_t len, uint8_t repeats) +{ + const uint8_t msg[4] = { 'L', 0, len, repeats }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) +{ + const uint8_t msg = 'g'; + uint8_t result[3]; + + int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + r = result[0]; + g = result[1]; + b = result[2]; + } + + return ret; +} + +int +BlinkM::get_firmware_version(uint8_t version[2]) +{ + const uint8_t msg = 'Z'; + + return transfer(&msg, sizeof(msg), version, 2); +} + +void blinkm_usage() { + fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); + fprintf(stderr, "options:\n"); + fprintf(stderr, "\t-b --bus i2cbus (3)\n"); + fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); +} + +int +blinkm_main(int argc, char *argv[]) +{ + + int i2cdevice = PX4_I2C_BUS_EXPANSION; + int blinkmadr = 9; + + int x; + + for (x = 1; x < argc; x++) { + if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { + if (argc > x + 1) { + i2cdevice = atoi(argv[x + 1]); + } + } + + if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) { + if (argc > x + 1) { + blinkmadr = atoi(argv[x + 1]); + } + } + + } + + if (!strcmp(argv[1], "start")) { + if (g_blinkm != nullptr) + errx(1, "already started"); + + g_blinkm = new BlinkM(i2cdevice, blinkmadr); + + if (g_blinkm == nullptr) + errx(1, "new failed"); + + if (OK != g_blinkm->init()) { + delete g_blinkm; + g_blinkm = nullptr; + errx(1, "init failed"); + } + + exit(0); + } + + + if (g_blinkm == nullptr) { + fprintf(stderr, "not started\n"); + blinkm_usage(); + exit(0); + } + + if (!strcmp(argv[1], "systemstate")) { + g_blinkm->setMode(1); + exit(0); + } + + if (!strcmp(argv[1], "ledoff")) { + g_blinkm->setMode(0); + exit(0); + } + + + if (!strcmp(argv[1], "list")) { + for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) + fprintf(stderr, " %s\n", BlinkM::script_names[i]); + fprintf(stderr, " \n"); + exit(0); + } + + /* things that require access to the device */ + int fd = open(BLINKM_DEVICE_PATH, 0); + if (fd < 0) + err(1, "can't open BlinkM device"); + + g_blinkm->setMode(0); + if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + exit(0); + + blinkm_usage(); + exit(0); +} diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk new file mode 100644 index 000000000..b48b90f3f --- /dev/null +++ b/src/drivers/blinkm/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# BlinkM I2C LED driver +# + +MODULE_COMMAND = blinkm + +SRCS = blinkm.cpp diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp new file mode 100644 index 000000000..4409a8a9c --- /dev/null +++ b/src/drivers/bma180/bma180.cpp @@ -0,0 +1,929 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bma180.cpp + * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +#define ADDR_CHIP_ID 0x00 +#define CHIP_ID 0x03 + +#define ADDR_ACC_X_LSB 0x02 +#define ADDR_ACC_Y_LSB 0x04 +#define ADDR_ACC_Z_LSB 0x06 +#define ADDR_TEMPERATURE 0x08 + +#define ADDR_CTRL_REG0 0x0D +#define REG0_WRITE_ENABLE 0x10 + +#define ADDR_RESET 0x10 +#define SOFT_RESET 0xB6 + +#define ADDR_BW_TCS 0x20 +#define BW_TCS_BW_MASK (0xf<<4) +#define BW_TCS_BW_10HZ (0<<4) +#define BW_TCS_BW_20HZ (1<<4) +#define BW_TCS_BW_40HZ (2<<4) +#define BW_TCS_BW_75HZ (3<<4) +#define BW_TCS_BW_150HZ (4<<4) +#define BW_TCS_BW_300HZ (5<<4) +#define BW_TCS_BW_600HZ (6<<4) +#define BW_TCS_BW_1200HZ (7<<4) + +#define ADDR_HIGH_DUR 0x27 +#define HIGH_DUR_DIS_I2C (1<<0) + +#define ADDR_TCO_Z 0x30 +#define TCO_Z_MODE_MASK 0x3 + +#define ADDR_GAIN_Y 0x33 +#define GAIN_Y_SHADOW_DIS (1<<0) + +#define ADDR_OFFSET_LSB1 0x35 +#define OFFSET_LSB1_RANGE_MASK (7<<1) +#define OFFSET_LSB1_RANGE_1G (0<<1) +#define OFFSET_LSB1_RANGE_2G (2<<1) +#define OFFSET_LSB1_RANGE_3G (3<<1) +#define OFFSET_LSB1_RANGE_4G (4<<1) +#define OFFSET_LSB1_RANGE_8G (5<<1) +#define OFFSET_LSB1_RANGE_16G (6<<1) + +#define ADDR_OFFSET_T 0x37 +#define OFFSET_T_READOUT_12BIT (1<<0) + +extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); } + +class BMA180 : public device::SPI +{ +public: + BMA180(int bus, spi_dev_e device); + virtual ~BMA180(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct accel_report *_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + unsigned _current_lowpass; + unsigned _current_range; + + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the BMA180 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the BMA180 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the BMA180 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the BMA180 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); + + /** + * Set the BMA180 internal lowpass filter frequency. + * + * @param frequency The internal lowpass filter frequency is set to a value + * equal or greater to this. + * Zero selects the highest frequency supported. + * @return OK if the value can be supported. + */ + int set_lowpass(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +BMA180::BMA180(int bus, spi_dev_e device) : + SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), + _call_interval(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _current_lowpass(0), + _current_range(0), + _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; +} + +BMA180::~BMA180() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +BMA180::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct accel_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + /* advertise sensor topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + + /* perform soft reset (p48) */ + write_reg(ADDR_RESET, SOFT_RESET); + + /* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */ + usleep(10000); + + /* enable writing to chip config */ + modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); + + /* disable I2C interface */ + modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0); + + /* switch to low-noise mode */ + modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0); + + /* disable 12-bit mode */ + modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0); + + /* disable shadow-disable mode */ + modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0); + + /* disable writing to chip config */ + modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); + + if (set_range(4)) warnx("Failed setting range"); + + if (set_lowpass(75)) warnx("Failed setting lowpass"); + + if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { + ret = OK; + + } else { + ret = ERROR; + } + +out: + return ret; +} + +int +BMA180::probe() +{ + /* dummy read to ensure SPI state machine is sane */ + read_reg(ADDR_CHIP_ID); + + if (read_reg(ADDR_CHIP_ID) == CHIP_ID) + return OK; + + return -EIO; +} + +ssize_t +BMA180::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + return ret; +} + +int +BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ + return -EINVAL; + + case ACCELIOCGSAMPLERATE: + return 1200; /* always operating in low-noise mode */ + + case ACCELIOCSLOWPASS: + return set_lowpass(arg); + + case ACCELIOCGLOWPASS: + return _current_lowpass; + + case ACCELIOCSSCALE: + /* copy scale in */ + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); + return OK; + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSRANGE: + return set_range(arg); + + case ACCELIOCGRANGE: + return _current_range; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +BMA180::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +BMA180::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +BMA180::set_range(unsigned max_g) +{ + uint8_t rangebits; + + if (max_g == 0) + max_g = 16; + + if (max_g > 16) + return -ERANGE; + + if (max_g <= 2) { + _current_range = 2; + rangebits = OFFSET_LSB1_RANGE_2G; + + } else if (max_g <= 3) { + _current_range = 3; + rangebits = OFFSET_LSB1_RANGE_3G; + + } else if (max_g <= 4) { + _current_range = 4; + rangebits = OFFSET_LSB1_RANGE_4G; + + } else if (max_g <= 8) { + _current_range = 8; + rangebits = OFFSET_LSB1_RANGE_8G; + + } else if (max_g <= 16) { + _current_range = 16; + rangebits = OFFSET_LSB1_RANGE_16G; + + } else { + return -EINVAL; + } + + /* set new range scaling factor */ + _accel_range_m_s2 = _current_range * 9.80665f; + _accel_range_scale = _accel_range_m_s2 / 8192.0f; + + /* enable writing to chip config */ + modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); + + /* adjust sensor configuration */ + modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); + + /* block writing to chip config */ + modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); + + /* check if wanted value is now in register */ + return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) == + (OFFSET_LSB1_RANGE_MASK & rangebits)); +} + +int +BMA180::set_lowpass(unsigned frequency) +{ + uint8_t bwbits; + + if (frequency > 1200) { + return -ERANGE; + + } else if (frequency > 600) { + bwbits = BW_TCS_BW_1200HZ; + + } else if (frequency > 300) { + bwbits = BW_TCS_BW_600HZ; + + } else if (frequency > 150) { + bwbits = BW_TCS_BW_300HZ; + + } else if (frequency > 75) { + bwbits = BW_TCS_BW_150HZ; + + } else if (frequency > 40) { + bwbits = BW_TCS_BW_75HZ; + + } else if (frequency > 20) { + bwbits = BW_TCS_BW_40HZ; + + } else if (frequency > 10) { + bwbits = BW_TCS_BW_20HZ; + + } else { + bwbits = BW_TCS_BW_10HZ; + } + + /* enable writing to chip config */ + modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); + + /* adjust sensor configuration */ + modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits); + + /* block writing to chip config */ + modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); + + /* check if wanted value is now in register */ + return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == + (BW_TCS_BW_MASK & bwbits)); +} + +void +BMA180::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); +} + +void +BMA180::stop() +{ + hrt_cancel(&_call); +} + +void +BMA180::measure_trampoline(void *arg) +{ + BMA180 *dev = (BMA180 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +BMA180::measure() +{ + /* BMA180 measurement registers */ +// #pragma pack(push, 1) +// struct { +// uint8_t cmd; +// int16_t x; +// int16_t y; +// int16_t z; +// } raw_report; +// #pragma pack(pop) + + accel_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the BMA180 in one pass; + * starting from the X LSB. + */ + //raw_report.cmd = ADDR_ACC_X_LSB; + // XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); + + /* + * Adjust and scale results to SI units. + * + * Note that we ignore the "new data" bits. At any time we read, each + * of the axis measurements are the "most recent", even if we've seen + * them before. There is no good way to synchronise with the internal + * measurement flow without using the external interrupt. + */ + _reports[_next_report].timestamp = hrt_absolute_time(); + /* + * y of board is x of sensor and x of board is -y of sensor + * perform only the axis assignment here. + * Two non-value bits are discarded directly + */ + report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + + /* discard two non-value bits in the 16 bit measurement */ + report->x_raw = (report->x_raw / 4); + report->y_raw = (report->y_raw / 4); + report->z_raw = (report->z_raw / 4); + + /* invert y axis, due to 14 bit data no overflow can occur in the negation */ + report->y_raw = -report->y_raw; + + report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report->scaling = _accel_range_scale; + report->range_m_s2 = _accel_range_m_s2; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +BMA180::print_info() +{ + perf_print_counter(_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace bma180 +{ + +BMA180 *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd = -1; + struct accel_report a_report; + ssize_t sz; + + /* get the driver */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'bma180 start' if the driver is not running)", + ACCEL_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate acc read failed"); + + warnx("single read"); + warnx("time: %lld", a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / 9.81f)); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "BMA180: driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +bma180_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + bma180::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + bma180::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + bma180::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + bma180::info(); + + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk new file mode 100644 index 000000000..4c60ee082 --- /dev/null +++ b/src/drivers/bma180/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the BMA180 driver. +# + +MODULE_COMMAND = bma180 + +SRCS = bma180.cpp diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp new file mode 100644 index 000000000..e35bdb944 --- /dev/null +++ b/src/drivers/gps/gps.cpp @@ -0,0 +1,536 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps.cpp + * Driver for the GPS on a serial port + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" +#include "mtk.h" + + +#define TIMEOUT_5HZ 500 +#define RATE_MEASUREMENT_PERIOD 5000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + + + +class GPS : public device::CDev +{ +public: + GPS(const char* uart_path); + virtual ~GPS(); + + virtual int init(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + volatile int _task; //< worker task + bool _healthy; ///< flag to signal if the GPS is ok + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< instance of GPS parser + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate + + + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ + void config(); + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(void *arg); + + + /** + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running + */ + void task_main(void); + + /** + * Set the baudrate of the UART to the GPS + */ + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_main(int argc, char *argv[]); + +namespace +{ + +GPS *g_dev; + +} + + +GPS::GPS(const char* uart_path) : + CDev("gps", GPS_DEVICE_PATH), + _task_should_exit(false), + _healthy(false), + _mode_changed(false), + _mode(GPS_DRIVER_MODE_UBX), + _Helper(nullptr), + _report_pub(-1), + _rate(0.0f) +{ + /* store port name */ + strncpy(_port, uart_path, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + + /* we need this potentially before it could be set in task_main */ + g_dev = this; + memset(&_report, 0, sizeof(_report)); + + _debug_enabled = true; +} + +GPS::~GPS() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + task_delete(_task); + g_dev = nullptr; + +} + +int +GPS::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + + /* start the GPS driver worker task */ + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + + if (_task < 0) { + warnx("task start failed: %d", errno); + return -errno; + } + + ret = OK; +out: + return ret; +} + +int +GPS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + lock(); + + int ret = OK; + + switch (cmd) { + case SENSORIOCRESET: + cmd_reset(); + break; + } + + unlock(); + + return ret; +} + +void +GPS::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +void +GPS::task_main() +{ + log("starting"); + + /* open the serial port */ + _serial_fd = ::open(_port, O_RDWR); + + if (_serial_fd < 0) { + log("failed to open serial port: %s err: %d", _port, errno); + /* tell the dtor that we are exiting, set error code */ + _task = -1; + _exit(1); + } + + uint64_t last_rate_measurement = hrt_absolute_time(); + unsigned last_rate_count = 0; + + /* loop handling received serial bytes and also configuring in between */ + while (!_task_should_exit) { + + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); //TODO: add NMEA + break; + default: + break; + } + unlock(); + if (_Helper->configure(_baudrate) == 0) { + unlock(); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { +// lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } + + if (!_healthy) { + warnx("module found"); + _healthy = true; + } + } + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } + + lock(); + } + lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + // case GPS_DRIVER_MODE_NMEA: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + default: + break; + } + + } + debug("exiting"); + + ::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + + + +void +GPS::cmd_reset() +{ + //XXX add reset? +} + +void +GPS::print_info() +{ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; + case GPS_DRIVER_MODE_NMEA: + warnx("protocol: NMEA"); + break; + default: + break; + } + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + if (_report.timestamp_position != 0) { + warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("update rate: %6.2f Hz", (double)_rate); + } + + usleep(100000); +} + +/** + * Local functions in support of the shell command. + */ +namespace gps +{ + +GPS *g_dev; + +void start(const char *uart_path); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *uart_path) +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new GPS(uart_path); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); + goto fail; + } + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver. + */ +void +stop() +{ + delete g_dev; + g_dev = nullptr; + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + exit(0); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +int +gps_main(int argc, char *argv[]) +{ + + /* set to default */ + char* device_name = GPS_DEFAULT_UART_PORT; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + } else { + goto out; + } + } + gps::start(device_name); + } + + if (!strcmp(argv[1], "stop")) + gps::stop(); + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps::info(); + +out: + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); +} diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp new file mode 100644 index 000000000..9c1fad569 --- /dev/null +++ b/src/drivers/gps/gps_helper.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include "gps_helper.h" + +/* @file gps_helper.cpp */ + +int +GPS_Helper::set_baudrate(const int &fd, unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + warnx("try baudrate: %d\n", speed); + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + return -1; + } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; +} diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h new file mode 100644 index 000000000..f3d3bc40b --- /dev/null +++ b/src/drivers/gps/gps_helper.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file gps_helper.h */ + +#ifndef GPS_HELPER_H +#define GPS_HELPER_H + +#include +#include + +class GPS_Helper +{ +public: + virtual int configure(unsigned &baud) = 0; + virtual int receive(unsigned timeout) = 0; + int set_baudrate(const int &fd, unsigned baud); +}; + +#endif /* GPS_HELPER_H */ diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk new file mode 100644 index 000000000..097db2abf --- /dev/null +++ b/src/drivers/gps/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# GPS driver +# + +MODULE_COMMAND = gps + +SRCS = gps.cpp \ + gps_helper.cpp \ + mtk.cpp \ + ubx.cpp diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp new file mode 100644 index 000000000..4762bd503 --- /dev/null +++ b/src/drivers/gps/mtk.cpp @@ -0,0 +1,274 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mkt.cpp */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mtk.h" + + +MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), +_mtk_revision(0) +{ + decode_init(); +} + +MTK::~MTK() +{ +} + +int +MTK::configure(unsigned &baudrate) +{ + /* set baudrate first */ + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + return -1; + + baudrate = MTK_BAUDRATE; + + /* Write config messages, don't wait for an answer */ + if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + warnx("mtk: config write failed"); + return -1; + } + + return 0; +} + +int +MTK::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + gps_mtk_packet_t packet; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* first read whatever is left */ + if (j < count) { + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j], packet) > 0) { + handle_message(packet); + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } + j++; + } + /* everything is read */ + j = count = 0; + } + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +void +MTK::decode_init(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = MTK_DECODE_UNINIT; +} +int +MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) +{ + int ret = 0; + + if (_decode_state == MTK_DECODE_UNINIT) { + + if (b == MTK_SYNC1_V16) { + _decode_state = MTK_DECODE_GOT_CK_A; + _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { + _decode_state = MTK_DECODE_GOT_CK_A; + _mtk_revision = 19; + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_A) { + if (b == MTK_SYNC2) { + _decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + decode_init(); + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (_rx_count < 33) + add_byte_to_checksum(b); + + // Fill packet buffer + ((uint8_t*)(&packet))[_rx_count] = b; + _rx_count++; + + /* Packet size minus checksum, XXX ? */ + if (_rx_count >= sizeof(packet)) { + /* Compare checksum */ + if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { + ret = 1; + } else { + warnx("MTK Checksum invalid"); + ret = -1; + } + // Reset state machine to decode next packet + decode_init(); + } + } + return ret; +} + +void +MTK::handle_message(gps_mtk_packet_t &packet) +{ + if (_mtk_revision == 16) { + _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + _gps_position->lat = packet.latitude; // both degrees*1e7 + _gps_position->lon = packet.longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + _gps_position->lat = 0; + _gps_position->lon = 0; + } + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + _gps_position->fix_type = packet.fix_type; + _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + _gps_position->satellites_visible = packet.satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet.date * 1e-4; + timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet.utc_time * 1e-7; + timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); + + return; +} + +void +MTK::add_byte_to_checksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h new file mode 100644 index 000000000..d4e390b01 --- /dev/null +++ b/src/drivers/gps/mtk.h @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mtk.h */ + +#ifndef MTK_H_ +#define MTK_H_ + +#include "gps_helper.h" + +#define MTK_SYNC1_V16 0xd0 +#define MTK_SYNC1_V19 0xd1 +#define MTK_SYNC2 0xdd + +#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define WAAS_ON "$PMTK301,2*2E\r\n" +#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" + +#define MTK_TIMEOUT_5HZ 400 +#define MTK_BAUDRATE 38400 + +typedef enum { + MTK_DECODE_UNINIT = 0, + MTK_DECODE_GOT_CK_A = 1, + MTK_DECODE_GOT_CK_B = 2 +} mtk_decode_state_t; + +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint8_t payload; ///< Number of payload bytes + int32_t latitude; ///< Latitude in degrees * 10^7 + int32_t longitude; ///< Longitude in degrees * 10^7 + uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 + uint32_t ground_speed; ///< velocity in m/s + int32_t heading; ///< heading in degrees * 10^2 + uint8_t satellites; ///< number of sattelites used + uint8_t fix_type; ///< fix type: XXX correct for that + uint32_t date; + uint32_t utc_time; + uint16_t hdop; ///< horizontal dilution of position (without unit) + uint8_t ck_a; + uint8_t ck_b; +} gps_mtk_packet_t; + +#pragma pack(pop) + +#define MTK_RECV_BUFFER_SIZE 40 + +class MTK : public GPS_Helper +{ +public: + MTK(const int &fd, struct vehicle_gps_position_s *gps_position); + ~MTK(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + +private: + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b, gps_mtk_packet_t &packet); + + /** + * Handle the package once it has arrived + */ + void handle_message(gps_mtk_packet_t &packet); + + /** + * Reset the parse state machine for a fresh start + */ + void decode_init(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void add_byte_to_checksum(uint8_t); + + int _fd; + struct vehicle_gps_position_s *_gps_position; + mtk_decode_state_t _decode_state; + uint8_t _mtk_revision; + uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; +}; + +#endif /* MTK_H_ */ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp new file mode 100644 index 000000000..c150f5271 --- /dev/null +++ b/src/drivers/gps/ubx.cpp @@ -0,0 +1,755 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" + +#define UBX_CONFIG_TIMEOUT 100 + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), +_waiting_for_ack(false) +{ + decode_init(); +} + +UBX::~UBX() +{ +} + +int +UBX::configure(unsigned &baudrate) +{ + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* Define the package contents, don't change the baudrate */ + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* Send a CFG-PRT message again, this time change the baudrate */ + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ + receive(UBX_CONFIG_TIMEOUT); + + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } + + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; +} + +int +UBX::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +int +UBX::parse_char(uint8_t b) +{ + switch (_decode_state) { + /* First, look for sync1 */ + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + break; + /* Second, look for sync2 */ + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + } + break; + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + /* check for known class */ + switch (b) { + case UBX_CLASS_ACK: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = ACK; + break; + + case UBX_CLASS_NAV: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = NAV; + break; + +// case UBX_CLASS_RXM: +// _decode_state = UBX_DECODE_GOT_CLASS; +// _message_class = RXM; +// break; + + case UBX_CLASS_CFG: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = CFG; + break; + default: //unknown class: reset state machine + decode_init(); + break; + } + break; + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + switch (_message_class) { + case NAV: + switch (b) { + case UBX_MESSAGE_NAV_POSLLH: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_POSLLH; + break; + + case UBX_MESSAGE_NAV_SOL: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SOL; + break; + + case UBX_MESSAGE_NAV_TIMEUTC: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_TIMEUTC; + break; + +// case UBX_MESSAGE_NAV_DOP: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = NAV_DOP; +// break; + + case UBX_MESSAGE_NAV_SVINFO: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SVINFO; + break; + + case UBX_MESSAGE_NAV_VELNED: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_VELNED; + break; + + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; +// case RXM: +// switch (b) { +// case UBX_MESSAGE_RXM_SVSI: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = RXM_SVSI; +// break; +// +// default: //unknown class: reset state machine, should not happen +// decode_init(); +// break; +// } +// break; + + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; + + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; + default: //should not happen because we set the class + warnx("UBX Error, we set a class that we don't know"); + decode_init(); +// config_needed = true; + break; + } + break; + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + case UBX_DECODE_GOT_LENGTH1: + add_byte_to_checksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + case UBX_DECODE_GOT_LENGTH2: + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) + add_byte_to_checksum(b); + _rx_buffer[_rx_count] = b; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); + } + + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + break; + default: + break; + } + return 0; //XXX ? +} + + +int +UBX::handle_message() +{ + int ret = 0; + + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + + _gps_position->timestamp_variance = hrt_absolute_time(); + } + break; + } + +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; +// +// _gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// break; +// } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + _gps_position->timestamp_time = hrt_absolute_time(); + } + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + _gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + _gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + _gps_position->satellite_used[i] = 0; + } + + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + } else { + _gps_position->satellite_info_available = false; + } + _gps_position->timestamp_satellites = hrt_absolute_time(); + } + + break; + } + + case NAV_VELNED: { + + if (!_waiting_for_ack) { + /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + } + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? +} + +void +UBX::decode_init(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = UBX_DECODE_UNINIT; + _message_class = CLASS_UNKNOWN; + _message_id = ID_UNKNOWN; + _payload_size = 0; +} + +void +UBX::add_byte_to_checksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} + +void +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) +{ + uint8_t ck_a = 0; + uint8_t ck_b = 0; + unsigned i; + + for (i = 0; i < length-2; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + /* The checksum is written to the last to bytes of a message */ + message[length-2] = ck_a; + message[length-1] = ck_b; +} + +void +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +{ + ssize_t ret = 0; + + /* Calculate the checksum now */ + add_checksum_to_message(packet, length); + + const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + + /* Start with the two sync bytes */ + ret += write(fd, sync_bytes, sizeof(sync_bytes)); + ret += write(fd, packet, length); + + if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? + warnx("ubx: config write fail"); +} diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h new file mode 100644 index 000000000..e3dd1c7ea --- /dev/null +++ b/src/drivers/gps/ubx.h @@ -0,0 +1,395 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef UBX_H_ +#define UBX_H_ + +#include "gps_helper.h" + +#define UBX_SYNC1 0xB5 +#define UBX_SYNC2 0x62 + +/* ClassIDs (the ones that are used) */ +#define UBX_CLASS_NAV 0x01 +//#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 + +/* MessageIDs (the ones that are used) */ +#define UBX_MESSAGE_NAV_POSLLH 0x02 +#define UBX_MESSAGE_NAV_SOL 0x06 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +//#define UBX_MESSAGE_NAV_DOP 0x04 +#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_VELNED 0x12 +//#define UBX_MESSAGE_RXM_SVSI 0x20 +#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_PRT 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_CFG_MSG 0x01 +#define UBX_MESSAGE_CFG_RATE 0x08 + +#define UBX_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ + +#define UBX_CFG_RATE_LENGTH 6 +#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ +#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ +#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ + + +#define UBX_CFG_NAV5_LENGTH 36 +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ + +#define UBX_CFG_MSG_LENGTH 8 +#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ + +#define UBX_MAX_PAYLOAD_LENGTH 500 + +// ************ +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_posllh_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; + uint32_t reserved2; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_sol_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ + int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_timeutc_packet_t; + +//typedef struct { +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint8_t ck_a; +// uint8_t ck_b; +//} gps_bin_nav_dop_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ + uint8_t globalFlags; + uint16_t reserved2; + +} gps_bin_nav_svinfo_part1_packet_t; + +typedef struct { + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ + uint8_t flags; + uint8_t quality; + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ + +} gps_bin_nav_svinfo_part2_packet_t; + +typedef struct { + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_svinfo_part3_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t velN; //NED north velocity, cm/s + int32_t velE; //NED east velocity, cm/s + int32_t velD; //NED down velocity, cm/s + uint32_t speed; //Speed (3-D), cm/s + uint32_t gSpeed; //Ground Speed (2-D), cm/s + int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 + uint32_t sAcc; //Speed Accuracy Estimate, cm/s + uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_velned_packet_t; + +//typedef struct { +// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ +// +// //... rest of package is not used in this implementation +// +//} gps_bin_rxm_svsi_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_ack_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_nak_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t portID; + uint8_t res0; + uint16_t res1; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t pad; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_rate_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint32_t reserved2; + uint32_t reserved3; + uint32_t reserved4; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_nav5_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t msgClass_payload; + uint8_t msgID_payload; + uint8_t rate[6]; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_msg_packet_t; + + +// END the structures of the binary packets +// ************ + +typedef enum { + UBX_CONFIG_STATE_PRT = 0, + UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, + UBX_CONFIG_STATE_RATE, + UBX_CONFIG_STATE_NAV5, + UBX_CONFIG_STATE_MSG_NAV_POSLLH, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, + UBX_CONFIG_STATE_MSG_NAV_DOP, + UBX_CONFIG_STATE_MSG_NAV_SVINFO, + UBX_CONFIG_STATE_MSG_NAV_SOL, + UBX_CONFIG_STATE_MSG_NAV_VELNED, +// UBX_CONFIG_STATE_MSG_RXM_SVSI, + UBX_CONFIG_STATE_CONFIGURED +} ubx_config_state_t; + +typedef enum { + CLASS_UNKNOWN = 0, + NAV = 1, + RXM = 2, + ACK = 3, + CFG = 4 +} ubx_message_class_t; + +typedef enum { + //these numbers do NOT correspond to the message id numbers of the ubx protocol + ID_UNKNOWN = 0, + NAV_POSLLH, + NAV_SOL, + NAV_TIMEUTC, +// NAV_DOP, + NAV_SVINFO, + NAV_VELNED, +// RXM_SVSI, + CFG_NAV5, + ACK_ACK, + ACK_NAK, +} ubx_message_id_t; + +typedef enum { + UBX_DECODE_UNINIT = 0, + UBX_DECODE_GOT_SYNC1, + UBX_DECODE_GOT_SYNC2, + UBX_DECODE_GOT_CLASS, + UBX_DECODE_GOT_MESSAGEID, + UBX_DECODE_GOT_LENGTH1, + UBX_DECODE_GOT_LENGTH2 +} ubx_decode_state_t; + +//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; +#pragma pack(pop) + +#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer + +class UBX : public GPS_Helper +{ +public: + UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + ~UBX(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + +private: + + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b); + + /** + * Handle the package once it has arrived + */ + int handle_message(void); + + /** + * Reset the parse state machine for a fresh start + */ + void decode_init(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void add_byte_to_checksum(uint8_t); + + /** + * Add the two checksum bytes to an outgoing message + */ + void add_checksum_to_message(uint8_t* message, const unsigned length); + + /** + * Helper to send a config packet + */ + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + + int _fd; + struct vehicle_gps_position_s *_gps_position; + ubx_config_state_t _config_state; + bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; + ubx_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; + ubx_message_id_t _message_id; + unsigned _payload_size; +}; + +#endif /* UBX_H_ */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp new file mode 100644 index 000000000..d9aa772d4 --- /dev/null +++ b/src/drivers/hil/hil.cpp @@ -0,0 +1,851 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hil.cpp + * + * Driver/configurator for the virtual HIL port. + * + * This virtual driver emulates PWM / servo outputs for setups where + * the connected hardware does not provide enough or no PWM outputs. + * + * Its only function is to take actuator_control uORB messages, + * mix them with any loaded mixer and output the result to the + * actuator_output uORB topic. HIL can also be performed with normal + * PWM outputs, a special flag prevents the outputs to be operated + * during HIL mode. If HIL is not performed with a standalone FMU, + * but in a real system, it is NOT recommended to use this virtual + * driver. Use instead the normal FMU or IO driver. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +class HIL : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, + MODE_NONE + }; + HIL(); + virtual ~HIL(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +namespace +{ + +HIL *g_hil; + +} // namespace + +HIL::HIL() : + CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), + _mode(MODE_NONE), + _update_rate(50), + _current_update_rate(0), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +HIL::~HIL() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_hil = nullptr; +} + +int +HIL::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + // XXX already claimed with CDEV + ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + // gpio_reset(); + + /* start the HIL interface task */ + _task = task_spawn("fmuhil", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&HIL::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +HIL::task_main_trampoline(int argc, char *argv[]) +{ + g_hil->task_main(); +} + +int +HIL::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + debug("MODE_2PWM"); + /* multi-port with flow control lines as PWM */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_4PWM: + debug("MODE_4PWM"); + /* multi-port as 4 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_8PWM: + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_NONE: + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + break; + + default: + return -EINVAL; + } + _mode = mode; + return OK; +} + +int +HIL::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +void +HIL::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs; + + /* select the number of virtual outputs */ + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + // XXX only support the lower 8 - trivial to extend + num_outputs = 8; + break; + + case MODE_NONE: + default: + num_outputs = 0; + break; + } + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) + update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = ::poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + } + } + + ::close(_t_actuators); + ::close(_t_armed); + + /* make sure servos are off */ + // up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +HIL::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +HIL::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + debug("ioctl 0x%04x 0x%08x", cmd, arg); + + // /* try it as a GPIO ioctl first */ + // ret = HIL::gpio_ioctl(filp, cmd, arg); + // if (ret != -ENOTTY) + // return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch(_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + ret = HIL::pwm_ioctl(filp, cmd, arg); + break; + default: + ret = -ENOTTY; + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + // int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + g_hil->set_pwm_rate(arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + // channel = cmd - PWM_SERVO_SET(0); +// up_pwm_servo_set(channel, arg); XXX + + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + // channel = cmd - PWM_SERVO_SET(0); + // *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNDEFINED = 0, + PORT1_MODE_UNSET, + PORT1_FULL_PWM, + PORT1_PWM_AND_SERIAL, + PORT1_PWM_AND_GPIO, + PORT2_MODE_UNSET, + PORT2_8PWM, + PORT2_12PWM, + PORT2_16PWM, +}; + +PortMode g_port_mode; + +int +hil_new_mode(PortMode new_mode) +{ + // uint32_t gpio_bits; + + +// /* reset to all-inputs */ +// g_hil->ioctl(0, GPIO_RESET, 0); + + // gpio_bits = 0; + + HIL::Mode servo_mode = HIL::MODE_NONE; + + switch (new_mode) { + case PORT_MODE_UNDEFINED: + case PORT1_MODE_UNSET: + case PORT2_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT1_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = HIL::MODE_4PWM; + break; + + case PORT1_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; +// /* set RX/TX multi-GPIOs to serial mode */ +// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT1_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; + break; + + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; + + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; + + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; + } + +// /* adjust GPIO config for serial mode(s) */ +// if (gpio_bits != 0) +// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_hil->set_mode(servo_mode); + + return OK; +} + +int +hil_start(void) +{ + int ret = OK; + + if (g_hil == nullptr) { + + g_hil = new HIL; + + if (g_hil == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_hil->init(); + + if (ret != OK) { + delete g_hil; + g_hil = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + exit(1); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + ioctl(fd, PWM_SERVO_SET(0), 1000); + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) { + puts("hil fake (values -100 .. 100)"); + exit(1); + } + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) { + puts("advertise failed"); + exit(1); + } + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int hil_main(int argc, char *argv[]); + +int +hil_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNDEFINED; + const char *verb = argv[1]; + + if (hil_start() != OK) + errx(1, "failed to start the HIL driver"); + + /* + * Mode switches. + */ + + // this was all cut-and-pasted from the FMU driver; it's junk + if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT1_FULL_PWM; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; + + } else if (!strcmp(verb, "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNDEFINED) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + return hil_new_mode(new_mode); + } + + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + + + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + return -EINVAL; +} diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk new file mode 100644 index 000000000..f8895f5d5 --- /dev/null +++ b/src/drivers/hil/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Hardware in the Loop (HIL) simulation actuator output bank +# + +MODULE_COMMAND = hil + +SRCS = hil.cpp diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp new file mode 100644 index 000000000..8ab568282 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -0,0 +1,1447 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hmc5883.cpp + * + * Driver for the HMC5883 magnetometer connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +/* + * HMC5883 internal constants and data structures. + */ + +#define HMC5883L_BUS PX4_I2C_BUS_ONBOARD +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + +/* Max measurement rate is 160Hz */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ + +#define ADDR_CONF_A 0x00 +#define ADDR_CONF_B 0x01 +#define ADDR_MODE 0x02 +#define ADDR_DATA_OUT_X_MSB 0x03 +#define ADDR_DATA_OUT_X_LSB 0x04 +#define ADDR_DATA_OUT_Z_MSB 0x05 +#define ADDR_DATA_OUT_Z_LSB 0x06 +#define ADDR_DATA_OUT_Y_MSB 0x07 +#define ADDR_DATA_OUT_Y_LSB 0x08 +#define ADDR_STATUS 0x09 +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +/* modes not changeable outside of driver */ +#define HMC5883L_MODE_NORMAL (0 << 0) /* default */ +#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ +#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */ + +#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */ +#define HMC5883L_AVERAGING_2 (1 << 5) +#define HMC5883L_AVERAGING_4 (2 << 5) +#define HMC5883L_AVERAGING_8 (3 << 5) + +#define MODE_REG_CONTINOUS_MODE (0 << 0) +#define MODE_REG_SINGLE_MODE (1 << 0) /* default */ + +#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ +#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class HMC5883 : public device::I2C +{ +public: + HMC5883(int bus); + virtual ~HMC5883(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + mag_report *_reports; + mag_scale _scale; + float _range_scale; + float _range_ga; + bool _collect_phase; + + orb_advert_t _mag_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /* status reporting */ + bool _sensor_ok; /**< sensor was found and reports ok */ + bool _calibrated; /**< the calibration is valid */ + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test strap, 0 to disable + */ + int calibrate(struct file *filp, unsigned enable); + + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test positive strap, -1 to enable + * negative strap, 0 to set to normal mode + */ + int set_excitement(unsigned enable); + + /** + * Set the sensor range. + * + * Sets the internal range to handle at least the argument in Gauss. + */ + int set_range(unsigned range); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Write a register. + * + * @param reg The register to write. + * @param val The value to write. + * @return OK on write success. + */ + int write_reg(uint8_t reg, uint8_t val); + + /** + * Read a register. + * + * @param reg The register to read. + * @param val The value read. + * @return OK on read success. + */ + int read_reg(uint8_t reg, uint8_t &val); + + /** + * Issue a measurement command. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Convert a big-endian signed 16-bit value to a float. + * + * @param in A signed 16-bit big-endian value. + * @return The floating-point representation of the value. + */ + float meas_to_float(uint8_t in[2]); + + /** + * Check the current calibration and update device status + * + * @return 0 if calibration is ok, 1 else + */ + int check_calibration(); + + /** + * Check the current scale calibration + * + * @return 0 if scale calibration is ok, 1 else + */ + int check_scale(); + + /** + * Check the current offset calibration + * + * @return 0 if offset calibration is ok, 1 else + */ + int check_offset(); + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); + + +HMC5883::HMC5883(int bus) : + I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(1.3f), + _mag_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), + _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _sensor_ok(false), + _calibrated(false) +{ + // enable debug() calls + _debug_enabled = true; + + // default scaling + _scale.x_offset = 0; + _scale.x_scale = 1.0f; + _scale.y_offset = 0; + _scale.y_scale = 1.0f; + _scale.z_offset = 0; + _scale.z_scale = 1.0f; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +HMC5883::~HMC5883() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +HMC5883::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct mag_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the mag topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + + /* set range */ + set_range(_range_ga); + + ret = OK; + /* sensor is ok, but not calibrated */ + _sensor_ok = true; +out: + return ret; +} + +int HMC5883::set_range(unsigned range) +{ + uint8_t range_bits; + + if (range < 1) { + range_bits = 0x00; + _range_scale = 1.0f / 1370.0f; + _range_ga = 0.88f; + + } else if (range <= 1) { + range_bits = 0x01; + _range_scale = 1.0f / 1090.0f; + _range_ga = 1.3f; + + } else if (range <= 2) { + range_bits = 0x02; + _range_scale = 1.0f / 820.0f; + _range_ga = 1.9f; + + } else if (range <= 3) { + range_bits = 0x03; + _range_scale = 1.0f / 660.0f; + _range_ga = 2.5f; + + } else if (range <= 4) { + range_bits = 0x04; + _range_scale = 1.0f / 440.0f; + _range_ga = 4.0f; + + } else if (range <= 4.7f) { + range_bits = 0x05; + _range_scale = 1.0f / 390.0f; + _range_ga = 4.7f; + + } else if (range <= 5.6f) { + range_bits = 0x06; + _range_scale = 1.0f / 330.0f; + _range_ga = 5.6f; + + } else { + range_bits = 0x07; + _range_scale = 1.0f / 230.0f; + _range_ga = 8.1f; + } + + int ret; + + /* + * Send the command to set the range + */ + ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + + if (OK != ret) + perf_count(_comms_errors); + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + + if (OK != ret) + perf_count(_comms_errors); + + return !(range_bits_in == (range_bits << 5)); +} + +int +HMC5883::probe() +{ + uint8_t data[3] = {0, 0, 0}; + + _retries = 10; + + if (read_reg(ADDR_ID_A, data[0]) || + read_reg(ADDR_ID_B, data[1]) || + read_reg(ADDR_ID_C, data[2])) + debug("read_reg fail"); + + _retries = 2; + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +ssize_t +HMC5883::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(HMC5883_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct mag_report *buf = new struct mag_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case MAGIOCSSAMPLERATE: + /* not supported, always 1 sample per poll */ + return -EINVAL; + + case MAGIOCSRANGE: + return set_range(arg); + + case MAGIOCSLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + case MAGIOCSSCALE: + /* set new scale factors */ + memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + /* check calibration, but not actually return an error */ + (void)check_calibration(); + return 0; + + case MAGIOCGSCALE: + /* copy out scale factors */ + memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); + return 0; + + case MAGIOCCALIBRATE: + return calibrate(filp, arg); + + case MAGIOCEXSTRAP: + return set_excitement(arg); + + case MAGIOCSELFTEST: + return check_calibration(); + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +void +HMC5883::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); +} + +void +HMC5883::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +HMC5883::cycle_trampoline(void *arg) +{ + HMC5883 *dev = (HMC5883 *)arg; + + dev->cycle(); +} + +void +HMC5883::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&HMC5883::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&HMC5883::cycle_trampoline, + this, + USEC2TICK(HMC5883_CONVERSION_INTERVAL)); +} + +int +HMC5883::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); + + if (OK != ret) + perf_count(_comms_errors); + + return ret; +} + +int +HMC5883::collect() +{ +#pragma pack(push, 1) + struct { /* status register and data as read back from the device */ + uint8_t x[2]; + uint8_t z[2]; + uint8_t y[2]; + } hmc_report; +#pragma pack(pop) + struct { + int16_t x, y, z; + } report; + int ret = -EIO; + uint8_t cmd; + + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + /* + * @note We could read the status register here, which could tell us that + * we were too early and that the output registers are still being + * written. In the common case that would just slow us down, and + * we're better off just never being early. + */ + + /* get measurements from the device */ + cmd = ADDR_DATA_OUT_X_MSB; + ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + + if (ret != OK) { + perf_count(_comms_errors); + debug("data/status read error"); + goto out; + } + + /* swap the data we just received */ + report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1]; + report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1]; + report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1]; + + /* + * If any of the values are -4096, there was an internal math error in the sensor. + * Generalise this to a simple range check that will also catch some bit errors. + */ + if ((abs(report.x) > 2048) || + (abs(report.y) > 2048) || + (abs(report.z) > 2048)) + goto out; + + /* + * RAW outputs + * + * to align the sensor axes with the board, x and y need to be flipped + * and y needs to be negated + */ + _reports[_next_report].x_raw = report.y; + _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + /* z remains z */ + _reports[_next_report].z_raw = report.z; + + /* scale values for output */ + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + /* to align the sensor axes with the board, x and y need to be flipped */ + _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; +} + +int HMC5883::calibrate(struct file *filp, unsigned enable) +{ + struct mag_report report; + ssize_t sz; + int ret = 1; + + // XXX do something smarter here + int fd = (int)enable; + + struct mag_scale mscale_previous = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + unsigned i; + + warnx("starting mag scale calibration"); + + /* do a simple demand read */ + sz = read(filp, (char *)&report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("immediate read failed"); + ret = 1; + goto out; + } + + warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + warnx("sampling 500 samples for scaling offset"); + + /* set the queue depth to 10 */ + if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { + warn("failed to set queue depth"); + ret = 1; + goto out; + } + + /* start the sensor polling at 50 Hz */ + if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { + warn("failed to set 2Hz poll rate"); + ret = 1; + goto out; + } + + /* Set to 2.5 Gauss */ + if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { + warnx("failed to enable sensor calibration mode"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to get scale / offsets for mag"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + ret = 1; + goto out; + } + + /* read the sensor 10x and report each value */ + for (i = 0; i < 500; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + goto out; + + } else { + avg_excited[0] += report.x; + avg_excited[1] += report.y; + avg_excited[2] += report.z; + } + + //warnx("periodic read %u", i); + //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("done. Performed %u reads", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = fabsf(1.16f / avg_excited[0]); + /* second axis inverted */ + scaling[1] = fabsf(1.16f / -avg_excited[1]); + scaling[2] = fabsf(1.08f / avg_excited[2]); + + warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + goto out; + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + goto out; + } + + /* set scaling in device */ + mscale_previous.x_scale = scaling[0]; + mscale_previous.y_scale = scaling[1]; + mscale_previous.z_scale = scaling[2]; + + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + goto out; + } + + ret = OK; + +out: + + if (ret == OK) { + if (!check_scale()) { + warnx("mag scale calibration successfully finished."); + } else { + warnx("mag scale calibration finished with invalid results."); + ret = ERROR; + } + + } else { + warnx("mag scale calibration failed."); + } + + return ret; +} + +int HMC5883::check_scale() +{ + bool scale_valid; + + if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + /* scale is one */ + scale_valid = false; + } else { + scale_valid = true; + } + + /* return 0 if calibrated, 1 else */ + return !scale_valid; +} + +int HMC5883::check_offset() +{ + bool offset_valid; + + if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + /* offset is zero */ + offset_valid = false; + } else { + offset_valid = true; + } + + /* return 0 if calibrated, 1 else */ + return !offset_valid; +} + +int HMC5883::check_calibration() +{ + bool offset_valid = (check_offset() == OK); + bool scale_valid = (check_scale() == OK); + + if (_calibrated != (offset_valid && scale_valid)) { + warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", + (offset_valid) ? "" : "offset invalid"); + _calibrated = (offset_valid && scale_valid); + + + // XXX Change advertisement + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _calibrated, + SUBSYSTEM_TYPE_MAG}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + } + + /* return 0 if calibrated, 1 else */ + return (!_calibrated); +} + +int HMC5883::set_excitement(unsigned enable) +{ + int ret; + /* arm the excitement strap */ + uint8_t conf_reg; + ret = read_reg(ADDR_CONF_A, conf_reg); + + if (OK != ret) + perf_count(_comms_errors); + + if (((int)enable) < 0) { + conf_reg |= 0x01; + + } else if (enable > 0) { + conf_reg |= 0x02; + + } else { + conf_reg &= ~0x03; + } + + ret = write_reg(ADDR_CONF_A, conf_reg); + + if (OK != ret) + perf_count(_comms_errors); + + uint8_t conf_reg_ret; + read_reg(ADDR_CONF_A, conf_reg_ret); + + return !(conf_reg == conf_reg_ret); +} + +int +HMC5883::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t cmd[] = { reg, val }; + + return transfer(&cmd[0], 2, nullptr, 0); +} + +int +HMC5883::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + +float +HMC5883::meas_to_float(uint8_t in[2]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[0] = in[1]; + u.b[1] = in[0]; + + return (float) u.w; +} + +void +HMC5883::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace hmc5883 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +HMC5883 *g_dev; + +void start(); +void test(); +void reset(); +void info(); +int calibrate(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new HMC5883(HMC5883L_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct mag_report report; + ssize_t sz; + int ret; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + + +/** + * Automatic scale calibration. + * + * Basic idea: + * + * output = (ext field +- 1.1 Ga self-test) * scale factor + * + * and consequently: + * + * 1.1 Ga = (excited - normal) * scale factor + * scale factor = (excited - normal) / 1.1 Ga + * + * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 + * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 + * + * By subtracting the non-excited measurement the pure 1.1 Ga reading + * can be extracted and the sensitivity of all axes can be matched. + * + * SELF TEST OPERATION + * To check the HMC5883L for proper operation, a self test feature in incorporated + * in which the sensor offset straps are excited to create a nominal field strength + * (bias field) to be measured. To implement self test, the least significant bits + * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) + * or 10 (negetive bias), e.g. 0x11 or 0x12. + * Then, by placing the mode register into single-measurement mode (0x01), + * two data acquisition cycles will be made on each magnetic vector. + * The first acquisition will be a set pulse followed shortly by measurement + * data of the external field. The second acquisition will have the offset strap + * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create + * about a Âą1.1 gauss self test field plus the external field. The first acquisition + * values will be subtracted from the second acquisition, and the net measurement + * will be placed into the data output registers. + * Since self test adds ~1.1 Gauss additional field to the existing field strength, + * using a reduced gain setting prevents sensor from being saturated and data registers + * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), + * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data + * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data + * output register. To leave the self test mode, change MS1 and MS0 bit of the + * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. + * Using the self test method described above, the user can scale sensor + */ +int calibrate() +{ + int ret; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { + warnx("failed to enable sensor calibration mode"); + } + + close(fd); + + if (ret == OK) { + errx(0, "PASS"); + + } else { + errx(1, "FAIL"); + } +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +hmc5883_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + hmc5883::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + hmc5883::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + hmc5883::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + hmc5883::info(); + + /* + * Autocalibrate the scaling + */ + if (!strcmp(argv[1], "calibrate")) { + if (hmc5883::calibrate() == 0) { + errx(0, "calibration successful"); + + } else { + errx(1, "calibration failed"); + } + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); +} diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk new file mode 100644 index 000000000..07377556d --- /dev/null +++ b/src/drivers/hmc5883/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# HMC5883 driver +# + +MODULE_COMMAND = hmc5883 + +# XXX seems excessive, check if 2048 is sufficient +MODULE_STACKSIZE = 4096 + +SRCS = hmc5883.cpp diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c new file mode 100644 index 000000000..a13a6ef58 --- /dev/null +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hott_telemetry_main.c + * @author Simon Wilks + * + * Graupner HoTT Telemetry implementation. + * + * The HoTT receiver polls each device at a regular interval at which point + * a data packet can be returned if necessary. + * + * TODO: Add support for at least the vario and GPS sensor data. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "messages.h" + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static char *daemon_name = "hott_telemetry"; +static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d ]"; + + +/* A little console messaging experiment - console helper macro */ +#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1); +#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); +#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg); +/** + * Deamon management function. + */ +__EXPORT int hott_telemetry_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int hott_telemetry_thread_main(int argc, char *argv[]); + +static int read_data(int uart, int *id); +static int send_data(int uart, uint8_t *buffer, int size); + +static int open_uart(const char *device, struct termios *uart_config_original) +{ + /* baud rate */ + int speed = B19200; + int uart; + + /* open uart */ + uart = open(device, O_RDWR | O_NOCTTY); + + if (uart < 0) { + char msg[80]; + sprintf(msg, "Error opening port: %s\n", device); + FATAL_MSG(msg); + } + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + char msg[80]; + + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state); + close(uart); + FATAL_MSG(msg); + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", + device, termios_state); + close(uart); + FATAL_MSG(msg); + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device); + close(uart); + FATAL_MSG(msg); + } + + /* Activate single wire mode */ + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + + return uart; +} + +int read_data(int uart, int *id) +{ + const int timeout = 1000; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + char mode; + + if (poll(fds, 1, timeout) > 0) { + /* Get the mode: binary or text */ + read(uart, &mode, 1); + /* Read the device ID being polled */ + read(uart, id, 1); + + /* if we have a binary mode request */ + if (mode != BINARY_MODE_REQUEST_ID) { + return ERROR; + } + + } else { + ERROR_MSG("UART timeout on TX/RX port"); + return ERROR; + } + + return OK; +} + +int send_data(int uart, uint8_t *buffer, int size) +{ + usleep(POST_READ_DELAY_IN_USECS); + + uint16_t checksum = 0; + + for (int i = 0; i < size; i++) { + if (i == size - 1) { + /* Set the checksum: the first uint8_t is taken as the checksum. */ + buffer[i] = checksum & 0xff; + + } else { + checksum += buffer[i]; + } + + write(uart, &buffer[i], 1); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int hott_telemetry_thread_main(int argc, char *argv[]) +{ + INFO_MSG("starting"); + + thread_running = true; + + char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + ERROR_MSG("missing parameter to -d"); + ERROR_MSG(commandline_usage); + exit(1); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + struct termios uart_config_original; + int uart = open_uart(device, &uart_config_original); + + if (uart < 0) { + ERROR_MSG("Failed opening HoTT UART, exiting."); + thread_running = false; + exit(ERROR); + } + + messages_init(); + + uint8_t buffer[MESSAGE_BUFFER_SIZE]; + int size = 0; + int id = 0; + + while (!thread_should_exit) { + if (read_data(uart, &id) == OK) { + switch (id) { + case ELECTRIC_AIR_MODULE: + build_eam_response(buffer, &size); + break; + + default: + continue; // Not a module we support. + } + + send_data(uart, buffer, size); + } + } + + INFO_MSG("exiting"); + + close(uart); + + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and tart the daemon. + */ +int hott_telemetry_main(int argc, char *argv[]) +{ + if (argc < 1) { + ERROR_MSG("missing command"); + ERROR_MSG(commandline_usage); + exit(1); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + INFO_MSG("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("hott_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 2048, + hott_telemetry_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + INFO_MSG("daemon is running"); + + } else { + INFO_MSG("daemon not started"); + } + + exit(0); + } + + ERROR_MSG("unrecognized command"); + ERROR_MSG(commandline_usage); + exit(1); +} + + + diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c new file mode 100644 index 000000000..5fbee16ce --- /dev/null +++ b/src/drivers/hott_telemetry/messages.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file messages.c + * @author Simon Wilks + */ + +#include "messages.h" + +#include +#include +#include +#include +#include + +static int battery_sub = -1; +static int sensor_sub = -1; + +void messages_init(void) +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); +} + +void build_eam_response(uint8_t *buffer, int *size) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + struct eam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.eam_sensor_id = ELECTRIC_AIR_MODULE; + msg.sensor_id = EAM_SENSOR_ID; + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + msg.temperature2 = TEMP_ZERO_CELSIUS; + msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); + + uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + msg.altitude_L = (uint8_t)alt & 0xff; + msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + + msg.stop = STOP_BYTE; + + memcpy(buffer, &msg, *size); +} \ No newline at end of file diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h new file mode 100644 index 000000000..dd38075fa --- /dev/null +++ b/src/drivers/hott_telemetry/messages.h @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file messages.h + * @author Simon Wilks + * + * Graupner HoTT Telemetry message generation. + * + */ +#ifndef MESSAGES_H_ +#define MESSAGES_H + +#include + +/* The buffer size used to store messages. This must be at least as big as the number of + * fields in the largest message struct. + */ +#define MESSAGE_BUFFER_SIZE 50 + +/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. + * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting + * the message after the read which takes some milliseconds. + */ +#define POST_READ_DELAY_IN_USECS 4000 +/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower + * values can be used in practise though. + */ +#define POST_WRITE_DELAY_IN_USECS 2000 + +// Protocol constants. +#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. +#define START_BYTE 0x7c +#define STOP_BYTE 0x7d +#define TEMP_ZERO_CELSIUS 0x14 + +/* Electric Air Module (EAM) constants. */ +#define ELECTRIC_AIR_MODULE 0x8e +#define EAM_SENSOR_ID 0xe0 + +/* The Electric Air Module message. */ +struct eam_module_msg { + uint8_t start; /**< Start byte */ + uint8_t eam_sensor_id; /**< EAM sensor ID */ + uint8_t warning; + uint8_t sensor_id; /**< Sensor ID, why different? */ + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2_L; + uint8_t cell3_L; + uint8_t cell4_L; + uint8_t cell5_L; + uint8_t cell6_L; + uint8_t cell7_L; + uint8_t cell1_H; + uint8_t cell2_H; + uint8_t cell3_H; + uint8_t cell4_H; + uint8_t cell5_H; + uint8_t cell6_H; + uint8_t cell7_H; + uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt1_voltage_H; + uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt2_voltage_H; + uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ + uint8_t temperature2; + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_H; + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ + uint8_t main_voltage_H; + uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ + uint8_t battery_capacity_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ + uint8_t climbrate_H; + uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_H; + uint8_t electric_min; /**< Flight time in minutes. */ + uint8_t electric_sec; /**< Flight time in seconds. */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_H; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ +}; + +void messages_init(void); +void build_eam_response(uint8_t *buffer, int *size); + +#endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott_telemetry/module.mk new file mode 100644 index 000000000..def1d59e9 --- /dev/null +++ b/src/drivers/hott_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Graupner HoTT Telemetry application. +# + +MODULE_COMMAND = hott_telemetry + +SRCS = hott_telemetry_main.c \ + messages.c diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 000000000..397686e8b --- /dev/null +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,840 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + virtual ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + return measure(); +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/mb12xx/module.mk b/src/drivers/mb12xx/module.mk new file mode 100644 index 000000000..4e00ada02 --- /dev/null +++ b/src/drivers/mb12xx/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +MODULE_COMMAND = mb12xx + +SRCS = mb12xx.cpp diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk new file mode 100644 index 000000000..c7d9cd3ef --- /dev/null +++ b/src/drivers/mpu6000/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the MPU6000 driver. +# + +MODULE_COMMAND = mpu6000 + +# XXX seems excessive, check if 2048 is not sufficient +MODULE_STACKSIZE = 4096 + +SRCS = mpu6000.cpp diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp new file mode 100644 index 000000000..df1958186 --- /dev/null +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -0,0 +1,1219 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mpu6000.cpp + * + * Driver for the Invensense MPU6000 connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +// MPU 6000 registers +#define MPUREG_WHOAMI 0x75 +#define MPUREG_SMPLRT_DIV 0x19 +#define MPUREG_CONFIG 0x1A +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_INT_STATUS 0x3A +#define MPUREG_ACCEL_XOUT_H 0x3B +#define MPUREG_ACCEL_XOUT_L 0x3C +#define MPUREG_ACCEL_YOUT_H 0x3D +#define MPUREG_ACCEL_YOUT_L 0x3E +#define MPUREG_ACCEL_ZOUT_H 0x3F +#define MPUREG_ACCEL_ZOUT_L 0x40 +#define MPUREG_TEMP_OUT_H 0x41 +#define MPUREG_TEMP_OUT_L 0x42 +#define MPUREG_GYRO_XOUT_H 0x43 +#define MPUREG_GYRO_XOUT_L 0x44 +#define MPUREG_GYRO_YOUT_H 0x45 +#define MPUREG_GYRO_YOUT_L 0x46 +#define MPUREG_GYRO_ZOUT_H 0x47 +#define MPUREG_GYRO_ZOUT_L 0x48 +#define MPUREG_USER_CTRL 0x6A +#define MPUREG_PWR_MGMT_1 0x6B +#define MPUREG_PWR_MGMT_2 0x6C +#define MPUREG_FIFO_COUNTH 0x72 +#define MPUREG_FIFO_COUNTL 0x73 +#define MPUREG_FIFO_R_W 0x74 +#define MPUREG_PRODUCT_ID 0x0C + +// Configuration bits MPU 3000 and MPU 6000 (not revised)? +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_INT_STATUS_DATA 0x01 + +// Product ID Description for MPU6000 +// high 4 bits low 4 bits +// Product Name Product Revision +#define MPU6000ES_REV_C4 0x14 +#define MPU6000ES_REV_C5 0x15 +#define MPU6000ES_REV_D6 0x16 +#define MPU6000ES_REV_D7 0x17 +#define MPU6000ES_REV_D8 0x18 +#define MPU6000_REV_C4 0x54 +#define MPU6000_REV_C5 0x55 +#define MPU6000_REV_D6 0x56 +#define MPU6000_REV_D7 0x57 +#define MPU6000_REV_D8 0x58 +#define MPU6000_REV_D9 0x59 +#define MPU6000_REV_D10 0x5A + + +class MPU6000_gyro; + +class MPU6000 : public device::SPI +{ +public: + MPU6000(int bus, spi_dev_e device); + virtual ~MPU6000(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + + friend class MPU6000_gyro; + + virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); + virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + MPU6000_gyro *_gyro; + uint8_t _product; /** product code */ + + struct hrt_call _call; + unsigned _call_interval; + + struct accel_report _accel_report; + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + struct gyro_report _gyro_report; + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + orb_advert_t _gyro_topic; + + unsigned _reads; + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the MPU6000 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the MPU6000 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the MPU6000 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the MPU6000 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); + + /** + * Swap a 16-bit value read from the MPU6000 to native byte order. + */ + uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + +}; + +/** + * Helper class implementing the gyro driver node. + */ +class MPU6000_gyro : public device::CDev +{ +public: + MPU6000_gyro(MPU6000 *parent); + ~MPU6000_gyro(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class MPU6000; + + void parent_poll_notify(); +private: + MPU6000 *_parent; +}; + +/** driver 'main' command */ +extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } + +MPU6000::MPU6000(int bus, spi_dev_e device) : + SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + _gyro(new MPU6000_gyro(this)), + _product(0), + _call_interval(0), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(-1), + _reads(0), + _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) +{ + // disable debug() calls + _debug_enabled = false; + + // default accel scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + // default gyro scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + memset(&_accel_report, 0, sizeof(_accel_report)); + memset(&_gyro_report, 0, sizeof(_gyro_report)); + memset(&_call, 0, sizeof(_call)); +} + +MPU6000::~MPU6000() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the gyro subdriver */ + delete _gyro; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +MPU6000::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("SPI setup failed"); + return ret; + } + + /* advertise sensor topics */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); + + // Chip reset + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + // Wake up device and select GyroZ clock (better performance) + write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + up_udelay(1000); + + // SAMPLE RATE + write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + usleep(1000); + + // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) + // was 90 Hz, but this ruins quality and does not improve the + // system response + _set_dlpf_filter(20); + usleep(1000); + // Gyro scale 2000 deg/s () + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + usleep(1000); + + // correct gyro scale factors + // scale to rad/s in SI units + // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s + // scaling factor: + // 1/(2^15)*(2000/180)*PI + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; + + // product-specific scaling + switch (_product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + // Accel scale 8g (4096 LSB/g) + // Rev C has different scaling than rev D + write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + break; + + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + // default case to cope with new chip revisions, which + // presumably won't have the accel scaling bug + default: + // Accel scale 8g (4096 LSB/g) + write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); + break; + } + + // Correct accel scale factors of 4096 LSB/g + // scale to m/s^2 ( 1g = 9.81 m/s^2) + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + _accel_range_scale = (9.81f / 4096.0f); + _accel_range_m_s2 = 8.0f * 9.81f; + + usleep(1000); + + // INT CFG => Interrupt on Data Ready + write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + usleep(1000); + write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + usleep(1000); + + // Oscillator set + // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); + usleep(1000); + + /* do CDev init for the gyro device node, keep it optional */ + int gyro_ret = _gyro->init(); + + if (gyro_ret != OK) { + _gyro_topic = -1; + } + + return ret; +} + +int +MPU6000::probe() +{ + + /* look for a product ID we recognise */ + _product = read_reg(MPUREG_PRODUCT_ID); + + // verify product revision + switch (_product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + debug("ID 0x%02x", _product); + return OK; + } + + debug("unexpected ID 0x%02x", _product); + return -EIO; +} + +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +MPU6000::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz <= 5) { + filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { + filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { + filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { + filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { + filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { + filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { + filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } + write_reg(MPUREG_CONFIG, filter); +} + +ssize_t +MPU6000::read(struct file *filp, char *buffer, size_t buflen) +{ + int ret = 0; + + /* buffer must be large enough */ + if (buflen < sizeof(_accel_report)) + return -ENOSPC; + + /* if automatic measurement is not enabled */ + if (_call_interval == 0) + measure(); + + /* copy out the latest reports */ + memcpy(buffer, &_accel_report, sizeof(_accel_report)); + ret = sizeof(_accel_report); + + return ret; +} + +int +MPU6000::self_test() +{ + if (_reads == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (_reads > 0) ? 0 : 1; +} + +ssize_t +MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) +{ + int ret = 0; + + /* buffer must be large enough */ + if (buflen < sizeof(_gyro_report)) + return -ENOSPC; + + /* if automatic measurement is not enabled */ + if (_call_interval == 0) + measure(); + + /* copy out the latest report */ + memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); + ret = sizeof(_gyro_report); + + return ret; +} + +int +MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* XXX 500Hz is just a wild guess */ + return ioctl(filp, SENSORIOCSPOLLRATE, 500); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: + /* XXX not implemented */ + return -EINVAL; + + case SENSORIOCGQUEUEDEPTH: + /* XXX not implemented */ + return -EINVAL; + + + case ACCELIOCSSAMPLERATE: + case ACCELIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case ACCELIOCSLOWPASS: + case ACCELIOCGLOWPASS: + _set_dlpf_filter((uint16_t)arg); + return OK; + + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSRANGE: + case ACCELIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _accel_range_scale = (9.81f / 4096.0f); + // _accel_range_rad_s = 8.0f * 9.81f; + return -EINVAL; + + case ACCELIOCSELFTEST: + return self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case GYROIOCSSAMPLERATE: + case GYROIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case GYROIOCSLOWPASS: + case GYROIOCGLOWPASS: + _set_dlpf_filter((uint16_t)arg); + return OK; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + case GYROIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_m_s2 = xx + return -EINVAL; + + case GYROIOCSELFTEST: + return self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +MPU6000::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +MPU6000::read_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +MPU6000::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +MPU6000::set_range(unsigned max_g) +{ +#if 0 + uint8_t rangebits; + float rangescale; + + if (max_g > 16) { + return -ERANGE; + + } else if (max_g > 8) { /* 16G */ + rangebits = OFFSET_LSB1_RANGE_16G; + rangescale = 1.98; + + } else if (max_g > 4) { /* 8G */ + rangebits = OFFSET_LSB1_RANGE_8G; + rangescale = 0.99; + + } else if (max_g > 3) { /* 4G */ + rangebits = OFFSET_LSB1_RANGE_4G; + rangescale = 0.5; + + } else if (max_g > 2) { /* 3G */ + rangebits = OFFSET_LSB1_RANGE_3G; + rangescale = 0.38; + + } else if (max_g > 1) { /* 2G */ + rangebits = OFFSET_LSB1_RANGE_2G; + rangescale = 0.25; + + } else { /* 1G */ + rangebits = OFFSET_LSB1_RANGE_1G; + rangescale = 0.13; + } + + /* adjust sensor configuration */ + modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); + _range_scale = rangescale; +#endif + return OK; +} + +void +MPU6000::start() +{ + /* make sure we are stopped first */ + stop(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); +} + +void +MPU6000::stop() +{ + hrt_cancel(&_call); +} + +void +MPU6000::measure_trampoline(void *arg) +{ + MPU6000 *dev = (MPU6000 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +MPU6000::measure() +{ +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + } mpu_report; +#pragma pack(pop) + + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the MPU6000 in one pass. + */ + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + return; + + /* count measurement */ + _reads++; + + /* + * Convert from big to little endian + */ + + report.accel_x = int16_t_from_bytes(mpu_report.accel_x); + report.accel_y = int16_t_from_bytes(mpu_report.accel_y); + report.accel_z = int16_t_from_bytes(mpu_report.accel_z); + + report.temp = int16_t_from_bytes(mpu_report.temp); + + report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); + report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); + report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + + /* + * Swap axes and negate y + */ + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; + + /* + * Adjust and scale results to m/s^2. + */ + _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + _accel_report.x_raw = report.accel_x; + _accel_report.y_raw = report.accel_y; + _accel_report.z_raw = report.accel_z; + + _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + _accel_report.scaling = _accel_range_scale; + _accel_report.range_m_s2 = _accel_range_m_s2; + + _accel_report.temperature_raw = report.temp; + _accel_report.temperature = (report.temp) / 361.0f + 35.0f; + + _gyro_report.x_raw = report.gyro_x; + _gyro_report.y_raw = report.gyro_y; + _gyro_report.z_raw = report.gyro_z; + + _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + _gyro_report.scaling = _gyro_range_scale; + _gyro_report.range_rad_s = _gyro_range_rad_s; + + _gyro_report.temperature_raw = report.temp; + _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + _gyro->parent_poll_notify(); + + /* and publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); + if (_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +MPU6000::print_info() +{ + printf("reads: %u\n", _reads); +} + +MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : + CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + _parent(parent) +{ +} + +MPU6000_gyro::~MPU6000_gyro() +{ +} + +void +MPU6000_gyro::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->gyro_read(filp, buffer, buflen); +} + +int +MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->gyro_ioctl(filp, cmd, arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace mpu6000 +{ + +MPU6000 *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd = -1; + int fd_gyro = -1; + struct accel_report a_report; + struct gyro_report g_report; + ssize_t sz; + + /* get the driver */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + ACCEL_DEVICE_PATH); + + /* get the driver */ + fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd_gyro < 0) + err(1, "%s open failed", GYRO_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate acc read failed"); + + warnx("single read"); + warnx("time: %lld", a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / 9.81f)); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) + err(1, "immediate gyro read failed"); + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +mpu6000_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + mpu6000::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mpu6000::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mpu6000::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + mpu6000::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk new file mode 100644 index 000000000..3c4b0f093 --- /dev/null +++ b/src/drivers/ms5611/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# MS5611 driver +# + +MODULE_COMMAND = ms5611 + +SRCS = ms5611.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp new file mode 100644 index 000000000..59ab5936e --- /dev/null +++ b/src/drivers/ms5611/ms5611.cpp @@ -0,0 +1,1214 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ms5611.cpp + * Driver for the MS5611 barometric pressure sensor connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct ms5611_prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union ms5611_prom_u { + uint16_t c[8]; + struct ms5611_prom_s s; +}; +#pragma pack(pop) + +class MS5611 : public device::I2C +{ +public: + MS5611(int bus); + virtual ~MS5611(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + union ms5611_prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in kPa */ + + orb_advert_t _baro_topic; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int cmd_reset(); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int read_prom(); + + /** + * PROM CRC routine ported from MS5611 application note + * + * @param n_prom Pointer to words read from PROM. + * @return True if the CRC matches. + */ + bool crc4(uint16_t *n_prom); + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * MS5611 internal constants and data structures. + */ + +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +#define MS5611_BUS PX4_I2C_BUS_ONBOARD +#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); + + +MS5611::MS5611(int bus) : + I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MS5611::~MS5611() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MS5611::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct baro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the baro topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + + ret = OK; +out: + return ret; +} + +int +MS5611::probe() +{ + _retries = 10; + + if ((OK == probe_address(MS5611_ADDRESS_1)) || + (OK == probe_address(MS5611_ADDRESS_2))) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return OK; + } + + return -EIO; +} + +int +MS5611::probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != cmd_reset()) + return -EIO; + + /* read PROM */ + if (OK != read_prom()) + return -EIO; + + return OK; +} + +ssize_t +MS5611::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); +} + +void +MS5611::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); +} + +void +MS5611::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611::cycle_trampoline(void *arg) +{ + MS5611 *dev = (MS5611 *)arg; + + dev->cycle(); +} + +void +MS5611::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. + */ + _retries = 0; + ret = transfer(&cmd_data, 1, nullptr, 0); + + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611::collect() +{ + int ret; + uint8_t cmd; + uint8_t data[3]; + union { + uint8_t b[4]; + uint32_t w; + } cvt; + + /* read the most recent measurement */ + cmd = 0; + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + ret = transfer(&cmd, 1, &data[0], 3); + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* fetch the raw value */ + cvt.b[0] = data[2]; + cvt.b[1] = data[1]; + cvt.b[2] = data[0]; + cvt.b[3] = 0; + uint32_t raw = cvt.w; + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + + /* generate a new report */ + _reports[_next_report].temperature = _TEMP / 100.0f; + _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ +#if 0/* USE_FLOAT */ + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ + const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + float p1 = _msl_pressure / 1000.0f; + + /* measured pressure in kPa */ + float p = P / 1000.0f; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; +#else + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; +#endif + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +int +MS5611::cmd_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611::read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return crc4(&_prom.c[0]) ? OK : -EIO; +} + +bool +MS5611::crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + +void +MS5611::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.s.factory_setup); + printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.s.serial_and_crc); +} + +/** + * Local functions in support of the shell command. + */ +namespace ms5611 +{ + +MS5611 *g_dev; + +void start(); +void test(); +void reset(); +void info(); +void calibrate(unsigned altitude); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MS5611(MS5611_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(BARO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct baro_report report; + ssize_t sz; + int ret; + + int fd = open(BARO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(BARO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +/** + * Calculate actual MSL pressure given current altitude + */ +void +calibrate(unsigned altitude) +{ + struct baro_report report; + float pressure; + float p1; + + int fd = open(BARO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + + /* start the sensor polling at max */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) + errx(1, "failed to set poll rate"); + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "sensor read failed"); + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) + err(1, "BAROIOCSMSLPRESSURE"); + + exit(0); +} + +} // namespace + +int +ms5611_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + ms5611::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + ms5611::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + ms5611::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + ms5611::info(); + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + if (!strcmp(argv[1], "calibrate")) { + if (argc < 2) + errx(1, "missing altitude"); + + long altitude = strtol(argv[2], nullptr, 10); + + ms5611::calibrate(altitude); + } + + errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3