From 41cef1d6c5420ddf1193760678a908b4e624586f Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sat, 12 Jan 2013 10:26:37 +0100 Subject: [PATCH 01/47] merged systemstate into blinkm driver --- apps/drivers/blinkm/blinkm.cpp | 496 +++++++++++++++++++++++++++++++-- 1 file changed, 474 insertions(+), 22 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index d589025cce..19f3b4d731 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -35,9 +35,57 @@ * @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 blinm 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: + * 0-4 satellites = X-X-X-X-_-_-_-_- + * 5 satellites = X-X-_-X-_-_-_-_- + * 6 satellites = X-_-_-X-_-_-_-_- + * >=7 satellites = _-_-_-X-_-_-_-_- + * + * The fourth Blink indicates the Flightmode: + * MANUAL : off + * STABILIZED : yellow + * HOLD : blue + * AUTO : green + * + * Battery Warning (low Battery Level): + * Continuously blinking in yellow 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 + * */ - #include #include @@ -59,6 +107,12 @@ #include #include +#include +#include +#include +#include +#include + class BlinkM : public device::I2C { public: @@ -67,7 +121,7 @@ public: 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 *script_names[]; @@ -96,10 +150,20 @@ private: }; work_s _work; - static const unsigned _monitor_interval = 250; - static void monitor_trampoline(void *arg); - void monitor(); + static int led_color_1; + static int led_color_2; + static int led_color_3; + static int led_color_4; + static int led_color_5; + static int led_color_6; + static int led_blink; + + static int 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); @@ -127,8 +191,7 @@ private: /* for now, we only support one BlinkM */ namespace { -BlinkM *g_blinkm; - + BlinkM *g_blinkm; } /* list of script names, must match script ID numbers */ @@ -155,11 +218,36 @@ const char *BlinkM::script_names[] = { nullptr }; +#define MAX_CELL_VOLTAGE 43 /* cell voltage if full charged */ + +#define LED_OFF 0 +#define LED_RED 1 +#define LED_YELLOW 2 +#define LED_PURPLE 3 +#define LED_GREEN 4 +#define LED_BLUE 5 +#define LED_WHITE 6 +#define LED_ONTIME 100 +#define LED_OFFTIME 100 +#define LED_BLINK 1 +#define LED_NOBLINK 0 + +int BlinkM::led_color_1 = LED_OFF; +int BlinkM::led_color_2 = LED_OFF; +int BlinkM::led_color_3 = LED_OFF; +int BlinkM::led_color_4 = LED_OFF; +int BlinkM::led_color_5 = LED_OFF; +int BlinkM::led_color_6 = LED_OFF; +int BlinkM::led_blink = LED_NOBLINK; + +int BlinkM::systemstate_run = 0; + extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus) : I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000) { + memset(&_work, 0, sizeof(_work)); } BlinkM::~BlinkM() @@ -170,7 +258,6 @@ int BlinkM::init() { int ret; - ret = I2C::init(); if (ret != OK) { @@ -179,13 +266,44 @@ BlinkM::init() } /* set some sensible defaults */ - set_fade_speed(25); + set_fade_speed(255); /* turn off by default */ play_script(BLACK); + set_fade_speed(255); + stop_script(); + set_rgb(0,0,0); - /* start the system monitor as a low-priority workqueue entry */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1); + return OK; +} + +int +BlinkM::setMode(int mode) +{ + if(mode == 1) { + if(BlinkM::systemstate_run == 0) { + /* set some sensible defaults */ + set_fade_speed(255); + + /* turn off by default */ + play_script(BLACK); + set_fade_speed(255); + stop_script(); + set_rgb(0,0,0); + BlinkM::systemstate_run = 1; + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); + } + } else { + BlinkM::systemstate_run = 0; + usleep(1000000); + /* set some sensible defaults */ + set_fade_speed(255); + /* turn off by default */ + play_script(BLACK); + set_fade_speed(255); + stop_script(); + set_rgb(0,0,0); + } return OK; } @@ -249,21 +367,343 @@ BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } + void -BlinkM::monitor_trampoline(void *arg) +BlinkM::led_trampoline(void *arg) { BlinkM *bm = (BlinkM *)arg; - bm->monitor(); + bm->led(); } -void -BlinkM::monitor() -{ - /* check system state, possibly update LED to suit */ - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval); + +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_1 = 0; + static int t_led_color_2 = 0; + static int t_led_color_3 = 0; + static int t_led_color_4 = 0; + static int t_led_color_5 = 0; + static int t_led_color_6 = 0; + static int t_led_blink = 0; + static int led_thread_runcount=1; + static int led_interval = 1000; + + static bool topic_initialized = false; + static bool detected_cells_blinked = false; + static bool led_thread_ready = true; + + int system_voltage = 0; + int num_of_used_sats = 0; + int poll_ret; + + 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; + } + + pollfd fds[2]; + fds[0].fd = vehicle_status_sub_fd; + fds[0].events = POLLIN; + fds[1].fd = vehicle_gps_position_sub_fd; + fds[1].events = POLLIN; + + if(led_thread_ready == true) { + if(!detected_cells_blinked) { + if(num_of_cells > 0) { + t_led_color_1 = LED_PURPLE; + } + if(num_of_cells > 1) { + t_led_color_2 = LED_PURPLE; + } + if(num_of_cells > 2) { + t_led_color_3 = LED_PURPLE; + } + if(num_of_cells > 3) { + t_led_color_4 = LED_PURPLE; + } + if(num_of_cells > 4) { + t_led_color_5 = LED_PURPLE; + } + t_led_color_6 = LED_OFF; + t_led_blink = LED_BLINK; + } else { + t_led_color_1 = BlinkM::led_color_1; + t_led_color_2 = BlinkM::led_color_2; + t_led_color_3 = BlinkM::led_color_3; + t_led_color_4 = BlinkM::led_color_4; + t_led_color_5 = BlinkM::led_color_5; + t_led_color_6 = BlinkM::led_color_6; + t_led_blink = BlinkM::led_blink; + } + led_thread_ready = false; + } + + switch(led_thread_runcount) { + case 1: // 1. LED on + BlinkM::setLEDColor(t_led_color_1); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 2: // 1. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + led_thread_runcount++; + led_interval = LED_OFFTIME; + break; + case 3: // 2. LED on + BlinkM::setLEDColor(t_led_color_2); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 4: // 2. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + led_thread_runcount++; + led_interval = LED_OFFTIME; + break; + case 5: // 3. LED on + BlinkM::setLEDColor(t_led_color_3); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 6: // 3. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + led_thread_runcount++; + led_interval = LED_OFFTIME; + break; + case 7: // 4. LED on + BlinkM::setLEDColor(t_led_color_4); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 8: // 4. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + led_thread_runcount++; + led_interval = LED_OFFTIME; + break; + case 9: // 5. LED on + BlinkM::setLEDColor(t_led_color_5); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 10: // 5. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + led_thread_runcount++; + led_interval = LED_OFFTIME; + break; + case 11: // 6. LED on + BlinkM::setLEDColor(t_led_color_6); + led_thread_runcount++; + led_interval = LED_ONTIME; + break; + case 12: // 6. LED off + if(t_led_blink == LED_BLINK) { + BlinkM::setLEDColor(LED_OFF); + } + + //poll_ret = ::poll(&fds[0],1,1000); + poll_ret = ::poll(fds, 2, 1000); + + if (poll_ret == 0) { + /* this means none of our providers is giving us data */ + printf("[blinkm_systemstate_sensor] Got no data within a second\n"); + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + log("poll error %d", errno); + usleep(1000000); + } else { + if (fds[0].revents & POLLIN) { + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s vehicle_gps_position_raw; + /* copy sensors raw data into local buffer */ + + /* vehicle_status */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + + /* vehicle_gps_position */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_status_sub_fd, &vehicle_gps_position_raw); + + /* get actual battery voltage */ + system_voltage = (int)vehicle_status_raw.voltage_battery*10; + + /* get number of used satellites in navigation */ + num_of_used_sats = 0; + for(int satloop=0; satloop<20; satloop++) { + if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { + num_of_used_sats++; + } + } + + if(num_of_cells == 0) { + /* looking for lipo cells that are connected */ + printf(" checking cells\n"); + for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; + } + printf(" cells found:%u\n", num_of_cells); + } else { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* LED Pattern for battery low warning */ + BlinkM::led_color_1 = LED_YELLOW; + BlinkM::led_color_2 = LED_YELLOW; + BlinkM::led_color_3 = LED_YELLOW; + BlinkM::led_color_4 = LED_YELLOW; + BlinkM::led_color_5 = LED_YELLOW; + BlinkM::led_color_6 = LED_YELLOW; + BlinkM::led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + /* LED Pattern for battery critical alerting */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_RED; + BlinkM::led_color_5 = LED_RED; + BlinkM::led_color_6 = LED_RED; + BlinkM::led_blink = LED_BLINK; + + } else { + /* no battery warnings here */ + + if(vehicle_status_raw.flag_system_armed == false) { + /* system not armed */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_RED; + BlinkM::led_color_5 = LED_RED; + BlinkM::led_color_6 = LED_RED; + BlinkM::led_blink = LED_NOBLINK; + + } else { + /* armed system - initial led pattern */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_OFF; + BlinkM::led_color_5 = LED_OFF; + BlinkM::led_color_6 = LED_OFF; + BlinkM::led_blink = LED_BLINK; + + /* handle 4th led - flightmode indicator */ + switch((int)vehicle_status_raw.flight_mode) { + case VEHICLE_FLIGHT_MODE_MANUAL: + BlinkM::led_color_4 = LED_OFF; + break; + + case VEHICLE_FLIGHT_MODE_STAB: + BlinkM::led_color_4 = LED_YELLOW; + break; + + case VEHICLE_FLIGHT_MODE_HOLD: + BlinkM::led_color_4 = LED_BLUE; + break; + + case VEHICLE_FLIGHT_MODE_AUTO: + BlinkM::led_color_4 = LED_GREEN; + break; + } + + /* handling used satīs */ + if(num_of_used_sats >= 7) { + BlinkM::led_color_1 = LED_OFF; + BlinkM::led_color_2 = LED_OFF; + BlinkM::led_color_3 = LED_OFF; + } else if(num_of_used_sats == 6) { + BlinkM::led_color_2 = LED_OFF; + BlinkM::led_color_3 = LED_OFF; + } else if(num_of_used_sats == 5) { + BlinkM::led_color_3 = LED_OFF; + } + } + } + } + + + printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%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, + vehicle_status_raw.battery_warning, + num_of_used_sats, + vehicle_gps_position_raw.fix_type, + vehicle_gps_position_raw.satellites_visible); + + } + } + + led_thread_runcount=1; + led_thread_ready = true; + led_interval = LED_OFFTIME; + + if(detected_cells_runcount < 5){ + detected_cells_runcount++; + } else { + detected_cells_blinked = true; + } + + break; + default: + led_thread_runcount=1; + t_led_blink = 0; + led_thread_ready = true; + break; + } + + if(BlinkM::systemstate_run == 1) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } +} + +void BlinkM::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_OFF: // off + BlinkM::set_rgb(0,0,0); + break; + case LED_RED: // red + BlinkM::set_rgb(255,0,0); + break; + case LED_YELLOW: // yellow + BlinkM::set_rgb(255,70,0); + break; + case LED_PURPLE: // purple + BlinkM::set_rgb(255,0,255); + break; + case LED_GREEN: // green + BlinkM::set_rgb(0,255,0); + break; + case LED_BLUE: // blue + BlinkM::set_rgb(0,0,255); + break; + case LED_WHITE: // white + BlinkM::set_rgb(255,255,255); + break; + } } int @@ -413,7 +853,6 @@ BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) return ret; } - int BlinkM::get_firmware_version(uint8_t version[2]) { @@ -443,9 +882,21 @@ blinkm_main(int argc, char *argv[]) exit(0); } + if (g_blinkm == nullptr) errx(1, "not started"); + 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]); @@ -458,8 +909,9 @@ blinkm_main(int argc, char *argv[]) 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); - errx(1, "missing command, try 'start', 'list' or a script name"); -} \ No newline at end of file + errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name."); +} From 8ec566d0cb0078e027355798d2ed2670ee23d928 Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sat, 12 Jan 2013 21:54:39 +0100 Subject: [PATCH 02/47] fix number of satellites --- apps/drivers/blinkm/blinkm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 19f3b4d731..ebe38ca43c 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -544,7 +544,7 @@ BlinkM::led() orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); /* vehicle_gps_position */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_status_sub_fd, &vehicle_gps_position_raw); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); /* get actual battery voltage */ system_voltage = (int)vehicle_status_raw.voltage_battery*10; @@ -642,7 +642,7 @@ BlinkM::led() } } - +/* printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", vehicle_status_raw.voltage_battery, vehicle_status_raw.flag_system_armed, @@ -652,7 +652,7 @@ BlinkM::led() num_of_used_sats, vehicle_gps_position_raw.fix_type, vehicle_gps_position_raw.satellites_visible); - +*/ } } From edc3ae7b6cbbbd5d61837cbd44a12d9c22770faf Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sun, 13 Jan 2013 13:50:07 +0100 Subject: [PATCH 03/47] some changes in structure --- Makefile | 3 +- apps/drivers/blinkm/blinkm.cpp | 295 ++++++++++++++++----------------- 2 files changed, 143 insertions(+), 155 deletions(-) diff --git a/Makefile b/Makefile index d9469bb49a..d3f859a251 100644 --- a/Makefile +++ b/Makefile @@ -109,7 +109,8 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +#SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" endif upload: $(FIRMWARE_BUNDLE) $(UPLOADER) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index ebe38ca43c..90dfa3eeaf 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -149,6 +149,21 @@ private: MORSE_CODE }; + enum systemDefines { + LED_OFF, + LED_RED, + LED_YELLOW, + LED_PURPLE, + LED_GREEN, + LED_BLUE, + LED_WHITE, + LED_ONTIME=100, + LED_OFFTIME=100, + LED_NOBLINK=0, + LED_BLINK=1, + MAX_CELL_VOLTAGE=43 + }; + work_s _work; static int led_color_1; @@ -159,7 +174,7 @@ private: static int led_color_6; static int led_blink; - static int systemstate_run; + static bool systemstate_run; void setLEDColor(int ledcolor); static void led_trampoline(void *arg); @@ -218,19 +233,6 @@ const char *BlinkM::script_names[] = { nullptr }; -#define MAX_CELL_VOLTAGE 43 /* cell voltage if full charged */ - -#define LED_OFF 0 -#define LED_RED 1 -#define LED_YELLOW 2 -#define LED_PURPLE 3 -#define LED_GREEN 4 -#define LED_BLUE 5 -#define LED_WHITE 6 -#define LED_ONTIME 100 -#define LED_OFFTIME 100 -#define LED_BLINK 1 -#define LED_NOBLINK 0 int BlinkM::led_color_1 = LED_OFF; int BlinkM::led_color_2 = LED_OFF; @@ -240,7 +242,8 @@ int BlinkM::led_color_5 = LED_OFF; int BlinkM::led_color_6 = LED_OFF; int BlinkM::led_blink = LED_NOBLINK; -int BlinkM::systemstate_run = 0; +bool BlinkM::systemstate_run = false; + extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); @@ -281,28 +284,26 @@ int BlinkM::setMode(int mode) { if(mode == 1) { - if(BlinkM::systemstate_run == 0) { + if(BlinkM::systemstate_run == false) { /* set some sensible defaults */ set_fade_speed(255); /* turn off by default */ play_script(BLACK); - set_fade_speed(255); stop_script(); - set_rgb(0,0,0); - BlinkM::systemstate_run = 1; + //set_rgb(0,0,0); + BlinkM::systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } } else { - BlinkM::systemstate_run = 0; + BlinkM::systemstate_run = false; usleep(1000000); /* set some sensible defaults */ set_fade_speed(255); /* turn off by default */ play_script(BLACK); - set_fade_speed(255); stop_script(); - set_rgb(0,0,0); + //set_rgb(0,0,0); } return OK; @@ -403,7 +404,6 @@ BlinkM::led() int system_voltage = 0; int num_of_used_sats = 0; - int poll_ret; if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); @@ -415,12 +415,6 @@ BlinkM::led() topic_initialized = true; } - pollfd fds[2]; - fds[0].fd = vehicle_status_sub_fd; - fds[0].events = POLLIN; - fds[1].fd = vehicle_gps_position_sub_fd; - fds[1].events = POLLIN; - if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { @@ -523,139 +517,132 @@ BlinkM::led() BlinkM::setLEDColor(LED_OFF); } - //poll_ret = ::poll(&fds[0],1,1000); - poll_ret = ::poll(fds, 2, 1000); + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s vehicle_gps_position_raw; - if (poll_ret == 0) { - /* this means none of our providers is giving us data */ - printf("[blinkm_systemstate_sensor] Got no data within a second\n"); - } else if (poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - log("poll error %d", errno); - usleep(1000000); - } else { - if (fds[0].revents & POLLIN) { - /* obtained data for the first file descriptor */ - struct vehicle_status_s vehicle_status_raw; - struct vehicle_gps_position_s vehicle_gps_position_raw; - /* copy sensors raw data into local buffer */ + bool new_data; + orb_check(vehicle_status_sub_fd, &new_data); - /* vehicle_status */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + if (new_data) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + } - /* vehicle_gps_position */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); + orb_check(vehicle_gps_position_sub_fd, &new_data); - /* get actual battery voltage */ - system_voltage = (int)vehicle_status_raw.voltage_battery*10; + if (new_data) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); + } - /* get number of used satellites in navigation */ - num_of_used_sats = 0; - for(int satloop=0; satloop<20; satloop++) { - if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { - num_of_used_sats++; - } - } + /* get actual battery voltage */ + system_voltage = (int)vehicle_status_raw.voltage_battery*10; - if(num_of_cells == 0) { - /* looking for lipo cells that are connected */ - printf(" checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; - } - printf(" cells found:%u\n", num_of_cells); - } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - /* LED Pattern for battery low warning */ - BlinkM::led_color_1 = LED_YELLOW; - BlinkM::led_color_2 = LED_YELLOW; - BlinkM::led_color_3 = LED_YELLOW; - BlinkM::led_color_4 = LED_YELLOW; - BlinkM::led_color_5 = LED_YELLOW; - BlinkM::led_color_6 = LED_YELLOW; - BlinkM::led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - /* LED Pattern for battery critical alerting */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_RED; - BlinkM::led_color_5 = LED_RED; - BlinkM::led_color_6 = LED_RED; - BlinkM::led_blink = LED_BLINK; - - } else { - /* no battery warnings here */ - - if(vehicle_status_raw.flag_system_armed == false) { - /* system not armed */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_RED; - BlinkM::led_color_5 = LED_RED; - BlinkM::led_color_6 = LED_RED; - BlinkM::led_blink = LED_NOBLINK; - - } else { - /* armed system - initial led pattern */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_OFF; - BlinkM::led_color_5 = LED_OFF; - BlinkM::led_color_6 = LED_OFF; - BlinkM::led_blink = LED_BLINK; - - /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - BlinkM::led_color_4 = LED_OFF; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - BlinkM::led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - BlinkM::led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - BlinkM::led_color_4 = LED_GREEN; - break; - } - - /* handling used satīs */ - if(num_of_used_sats >= 7) { - BlinkM::led_color_1 = LED_OFF; - BlinkM::led_color_2 = LED_OFF; - BlinkM::led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { - BlinkM::led_color_2 = LED_OFF; - BlinkM::led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { - BlinkM::led_color_3 = LED_OFF; - } - } - } - } - -/* - printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%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, - vehicle_status_raw.battery_warning, - num_of_used_sats, - vehicle_gps_position_raw.fix_type, - vehicle_gps_position_raw.satellites_visible); -*/ + /* get number of used satellites in navigation */ + num_of_used_sats = 0; + for(int satloop=0; satloop<20; satloop++) { + if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { + num_of_used_sats++; } } + if(num_of_cells == 0) { + /* looking for lipo cells that are connected */ + printf(" checking cells\n"); + for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; + } + printf(" cells found:%u\n", num_of_cells); + } else { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* LED Pattern for battery low warning */ + BlinkM::led_color_1 = LED_YELLOW; + BlinkM::led_color_2 = LED_YELLOW; + BlinkM::led_color_3 = LED_YELLOW; + BlinkM::led_color_4 = LED_YELLOW; + BlinkM::led_color_5 = LED_YELLOW; + BlinkM::led_color_6 = LED_YELLOW; + BlinkM::led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + /* LED Pattern for battery critical alerting */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_RED; + BlinkM::led_color_5 = LED_RED; + BlinkM::led_color_6 = LED_RED; + BlinkM::led_blink = LED_BLINK; + + } else { + /* no battery warnings here */ + + if(vehicle_status_raw.flag_system_armed == false) { + /* system not armed */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_RED; + BlinkM::led_color_5 = LED_RED; + BlinkM::led_color_6 = LED_RED; + BlinkM::led_blink = LED_NOBLINK; + + } else { + /* armed system - initial led pattern */ + BlinkM::led_color_1 = LED_RED; + BlinkM::led_color_2 = LED_RED; + BlinkM::led_color_3 = LED_RED; + BlinkM::led_color_4 = LED_OFF; + BlinkM::led_color_5 = LED_OFF; + BlinkM::led_color_6 = LED_OFF; + BlinkM::led_blink = LED_BLINK; + + /* handle 4th led - flightmode indicator */ + switch((int)vehicle_status_raw.flight_mode) { + case VEHICLE_FLIGHT_MODE_MANUAL: + BlinkM::led_color_4 = LED_OFF; + break; + + case VEHICLE_FLIGHT_MODE_STAB: + BlinkM::led_color_4 = LED_YELLOW; + break; + + case VEHICLE_FLIGHT_MODE_HOLD: + BlinkM::led_color_4 = LED_BLUE; + break; + + case VEHICLE_FLIGHT_MODE_AUTO: + BlinkM::led_color_4 = LED_GREEN; + break; + } + + /* handling used satīs */ + if(num_of_used_sats >= 7) { + BlinkM::led_color_1 = LED_OFF; + BlinkM::led_color_2 = LED_OFF; + BlinkM::led_color_3 = LED_OFF; + } else if(num_of_used_sats == 6) { + BlinkM::led_color_2 = LED_OFF; + BlinkM::led_color_3 = LED_OFF; + } else if(num_of_used_sats == 5) { + BlinkM::led_color_3 = LED_OFF; + } + } + } + } + + /* + printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%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, + vehicle_status_raw.battery_warning, + num_of_used_sats, + vehicle_gps_position_raw.fix_type, + vehicle_gps_position_raw.satellites_visible); + */ + + led_thread_runcount=1; led_thread_ready = true; led_interval = LED_OFFTIME; @@ -674,7 +661,7 @@ BlinkM::led() break; } - if(BlinkM::systemstate_run == 1) { + if(BlinkM::systemstate_run == true) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); } From a2a87d101903fd9f703a58950a758b30f18fd384 Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sun, 13 Jan 2013 13:51:02 +0100 Subject: [PATCH 04/47] some changes in structure --- Makefile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Makefile b/Makefile index d3f859a251..d9469bb49a 100644 --- a/Makefile +++ b/Makefile @@ -109,8 +109,7 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -#SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" -SERIAL_PORTS = "\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" endif upload: $(FIRMWARE_BUNDLE) $(UPLOADER) From 825012b029a09e51828136deed3a24fab95c4780 Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Sun, 13 Jan 2013 18:04:07 +0100 Subject: [PATCH 05/47] switched to initialiser list and member variables --- apps/drivers/blinkm/blinkm.cpp | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 90dfa3eeaf..b1e20d4f85 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -119,6 +119,7 @@ public: BlinkM(int bus); ~BlinkM(); + virtual int init(); virtual int probe(); virtual int setMode(int mode); @@ -166,15 +167,15 @@ private: work_s _work; - static int led_color_1; - static int led_color_2; - static int led_color_3; - static int led_color_4; - static int led_color_5; - static int led_color_6; - static int led_blink; + 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_blink; - static bool systemstate_run; + bool systemstate_run; void setLEDColor(int ledcolor); static void led_trampoline(void *arg); @@ -233,7 +234,7 @@ const char *BlinkM::script_names[] = { nullptr }; - +/* int BlinkM::led_color_1 = LED_OFF; int BlinkM::led_color_2 = LED_OFF; int BlinkM::led_color_3 = LED_OFF; @@ -243,12 +244,20 @@ int BlinkM::led_color_6 = LED_OFF; int BlinkM::led_blink = LED_NOBLINK; bool BlinkM::systemstate_run = false; - +*/ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus) : - I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000) + I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 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_blink(LED_NOBLINK), + systemstate_run(false) { memset(&_work, 0, sizeof(_work)); } From 5745cfae385e5a7fe7948977ea90facb9360c2c7 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 13 Jan 2013 21:12:24 -0500 Subject: [PATCH 06/47] Tracking down gps ekf bug, not enough precision for GPS in rad. --- apps/examples/kalman_demo/KalmanNav.cpp | 42 +++++++++++++++++++------ 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 240564b568..18bca6d955 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -316,7 +316,11 @@ void KalmanNav::predictFast(float dt) float cosL = cosf(lat); float cosLSing = cosf(lat); - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } // position update // neglects angular deflections in local gravity @@ -577,11 +581,11 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y.dot((S*S.transpose()).inverse() * y); - if (beta > 1000.0f) { + if (beta > 1.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + printf("y:\n"); y.print(); } // update quaternions from euler @@ -592,7 +596,7 @@ void KalmanNav::correctAtt() void KalmanNav::correctPos() { using namespace math; - Vector y(6); + Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat; @@ -604,7 +608,10 @@ void KalmanNav::correctPos() float R = R0 + float(alt); // prevent singularity - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } float noiseLat = _rGpsPos.get() / R; float noiseLon = _rGpsPos.get() / (R * cosLSing); @@ -650,11 +657,23 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y.dot((S*S.transpose()).inverse() * y); - if (beta > 1000.0f) { + if (beta > 1.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + printf("y:\n"); y.print(); + printf("R:\n"); RPos.print(); + printf("S:\n"); S.print(); + printf("S*S^T:\n"); (S*S.transpose()).print(); + printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print(); + printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print(); + printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", + double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d), + double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG, + double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG, + double(_gps.alt)/ 1.0e3); + printf("x : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", + double(vN), double(vE), double(vD), lat, lon, alt); } } @@ -692,7 +711,10 @@ void KalmanNav::updateParams() float R = R0 + float(alt); // prevent singularity - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } float noiseVel = _rGpsVel.get(); float noiseLat = _rGpsPos.get() / R; From 3db216380bed13fd25c21e49a1fbd66680968937 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 14 Jan 2013 01:09:02 -0500 Subject: [PATCH 07/47] Changing measurement units for gps, not working well yet. --- apps/examples/kalman_demo/KalmanNav.cpp | 26 ++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 18bca6d955..788cb5a716 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -141,8 +141,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // HPos is constant HPos(0, 3) = 1.0f; HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0f; - HPos(3, 7) = 1.0f; + HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; HPos(4, 8) = 1.0f; // initialize all parameters @@ -599,9 +599,9 @@ void KalmanNav::correctPos() Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; - y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat; - y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon; - y(4) = double(_gps.alt) / 1.0e3 - alt; + y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; + y(4) = double(_gps.alt)/1.0e3 - alt; // update gps noise float cosLSing = cosf(lat); @@ -613,10 +613,10 @@ void KalmanNav::correctPos() else cosLSing = -0.01; } - float noiseLat = _rGpsPos.get() / R; - float noiseLon = _rGpsPos.get() / (R * cosLSing); - RPos(2, 2) = noiseLat * noiseLat; - RPos(3, 3) = noiseLon * noiseLon; + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -717,12 +717,12 @@ void KalmanNav::updateParams() } float noiseVel = _rGpsVel.get(); - float noiseLat = _rGpsPos.get() / R; - float noiseLon = _rGpsPos.get() / (R * cosLSing); + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseAlt = _rGpsAlt.get(); RPos(0, 0) = noiseVel * noiseVel; // vn RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLat * noiseLat; // lat - RPos(3, 3) = noiseLon * noiseLon; // lon + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon RPos(4, 4) = noiseAlt * noiseAlt; // h } From f2d2aafb8d340bdad7e75e9df27b8fd002da1344 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 14 Jan 2013 01:32:34 -0500 Subject: [PATCH 08/47] Fault detection working, but GPS velocity measurement causing fault. Possible error in HIL script or progpagation/ F matrix of EKF. --- apps/examples/kalman_demo/KalmanNav.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 788cb5a716..c3093fe7f5 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -613,7 +613,7 @@ void KalmanNav::correctPos() else cosLSing = -0.01; } - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; @@ -662,11 +662,11 @@ void KalmanNav::correctPos() if (beta > 1.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); - printf("R:\n"); RPos.print(); - printf("S:\n"); S.print(); - printf("S*S^T:\n"); (S*S.transpose()).print(); - printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print(); - printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print(); + //printf("R:\n"); RPos.print(); + //printf("S:\n"); S.print(); + //printf("S*S^T:\n"); (S*S.transpose()).print(); + //printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print(); + //printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print(); printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d), double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG, @@ -717,7 +717,7 @@ void KalmanNav::updateParams() } float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseAlt = _rGpsAlt.get(); RPos(0, 0) = noiseVel * noiseVel; // vn From a13cf90e0ac89527830b6426e70913d67ec093b2 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 13:12:01 -0500 Subject: [PATCH 09/47] Increased KF process noise. --- apps/examples/kalman_demo/KalmanNav.cpp | 21 ++++++--------------- apps/examples/kalman_demo/params.c | 4 ++-- 2 files changed, 8 insertions(+), 17 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index c3093fe7f5..d28cba42c9 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -583,7 +583,7 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot((S*S.transpose()).inverse() * y); - if (beta > 1.0f) { + if (beta > 10.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); } @@ -613,8 +613,10 @@ void KalmanNav::correctPos() else cosLSing = -0.01; } + float noiseVel = _rGpsVel.get(); float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + float noiseAlt = _rGpsAlt.get(); RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; @@ -659,21 +661,10 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot((S*S.transpose()).inverse() * y); - if (beta > 1.0f) { + if (beta > 10.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); - printf("y:\n"); y.print(); - //printf("R:\n"); RPos.print(); - //printf("S:\n"); S.print(); - //printf("S*S^T:\n"); (S*S.transpose()).print(); - //printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print(); - //printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print(); - printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", - double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d), - double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG, - double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG, - double(_gps.alt)/ 1.0e3); - printf("x : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", - double(vN), double(vE), double(vD), lat, lon, alt); + printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt); } } diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 03cdca5ed2..f36ac334f9 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,8 +1,8 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); From c49320a03ef7f57ae01e9943fc1b4c340c648050 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 13:49:30 -0500 Subject: [PATCH 10/47] Working on fault detection tolerances. --- apps/examples/control_demo/params.c | 4 +-- apps/examples/kalman_demo/KalmanNav.cpp | 40 ++++++++++++++----------- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 8f923f5a1c..aea2603306 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -24,7 +24,7 @@ PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f); +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); @@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index d28cba42c9..e804fbca5c 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -316,11 +316,11 @@ void KalmanNav::predictFast(float dt) float cosL = cosf(lat); float cosLSing = cosf(lat); - // prevent singularity + // prevent singularity if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } // position update // neglects angular deflections in local gravity @@ -581,9 +581,9 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot((S*S.transpose()).inverse() * y); + float beta = y.dot((S * S.transpose()).inverse() * y); - if (beta > 10.0f) { + if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); } @@ -601,7 +601,7 @@ void KalmanNav::correctPos() y(1) = _gps.vel_e - vE; y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; - y(4) = double(_gps.alt)/1.0e3 - alt; + y(4) = double(_gps.alt) / 1.0e3 - alt; // update gps noise float cosLSing = cosf(lat); @@ -609,9 +609,9 @@ void KalmanNav::correctPos() // prevent singularity if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } float noiseVel = _rGpsVel.get(); float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; @@ -659,12 +659,16 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot((S*S.transpose()).inverse() * y); + float beta = y.dot((S * S.transpose()).inverse() * y); - if (beta > 10.0f) { + if (beta > 1000.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); - printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt); + printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(y(0) / noiseVel), + double(y(1) / noiseVel), + double(y(2) / noiseLatDegE7), + double(y(3) / noiseLonDegE7), + double(y(4) / noiseAlt)); } } @@ -702,10 +706,10 @@ void KalmanNav::updateParams() float R = R0 + float(alt); // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } float noiseVel = _rGpsVel.get(); float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; From f0edb59d7e8ef3aa23ae14a47bacc8276c4cdf0b Mon Sep 17 00:00:00 2001 From: Marco Bauer Date: Mon, 14 Jan 2013 21:58:42 +0100 Subject: [PATCH 11/47] some major changes --- apps/drivers/blinkm/blinkm.cpp | 515 +++++++++++++++------------------ 1 file changed, 231 insertions(+), 284 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index b1e20d4f85..aeee80905a 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -39,7 +39,7 @@ * 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 blinm start: + * To start the system monitor put in the next line after the blinkm start: * blinkm systemmonitor * * @@ -66,11 +66,12 @@ * * (X = on, _=off) * - * The first 3 Blinks indicates the status of the GPS-Signal: + * 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 : off @@ -84,6 +85,9 @@ * Battery Alert (critical Battery Level) * Continuously blinking in red 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 + * */ #include @@ -113,6 +117,12 @@ #include #include +static const float MAX_CELL_VOLTAGE = 4.3f; +static const int LED_ONTIME = 100; +static const int LED_OFFTIME = 100; +static const int LED_BLINK = 1; +static const int LED_NOBLINK = 0; + class BlinkM : public device::I2C { public: @@ -150,19 +160,14 @@ private: MORSE_CODE }; - enum systemDefines { + enum ledColors { LED_OFF, LED_RED, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, - LED_WHITE, - LED_ONTIME=100, - LED_OFFTIME=100, - LED_NOBLINK=0, - LED_BLINK=1, - MAX_CELL_VOLTAGE=43 + LED_WHITE }; work_s _work; @@ -234,17 +239,6 @@ const char *BlinkM::script_names[] = { nullptr }; -/* -int BlinkM::led_color_1 = LED_OFF; -int BlinkM::led_color_2 = LED_OFF; -int BlinkM::led_color_3 = LED_OFF; -int BlinkM::led_color_4 = LED_OFF; -int BlinkM::led_color_5 = LED_OFF; -int BlinkM::led_color_6 = LED_OFF; -int BlinkM::led_blink = LED_NOBLINK; - -bool BlinkM::systemstate_run = false; -*/ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); @@ -277,12 +271,6 @@ BlinkM::init() return ret; } - /* set some sensible defaults */ - set_fade_speed(255); - - /* turn off by default */ - play_script(BLACK); - set_fade_speed(255); stop_script(); set_rgb(0,0,0); @@ -293,26 +281,14 @@ int BlinkM::setMode(int mode) { if(mode == 1) { - if(BlinkM::systemstate_run == false) { - /* set some sensible defaults */ - set_fade_speed(255); - - /* turn off by default */ - play_script(BLACK); + if(systemstate_run == false) { stop_script(); - //set_rgb(0,0,0); - BlinkM::systemstate_run = true; + set_rgb(0,0,0); + systemstate_run = true; work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); } } else { - BlinkM::systemstate_run = false; - usleep(1000000); - /* set some sensible defaults */ - set_fade_speed(255); - /* turn off by default */ - play_script(BLACK); - stop_script(); - //set_rgb(0,0,0); + systemstate_run = false; } return OK; @@ -397,21 +373,19 @@ BlinkM::led() static int num_of_cells = 0; static int detected_cells_runcount = 0; - static int t_led_color_1 = 0; - static int t_led_color_2 = 0; - static int t_led_color_3 = 0; - static int t_led_color_4 = 0; - static int t_led_color_5 = 0; - static int t_led_color_6 = 0; + + static int t_led_color[6] = { 0, 0, 0, 0, 0, 0}; static int t_led_blink = 0; - static int led_thread_runcount=1; + 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 system_voltage = 0; int num_of_used_sats = 0; if(!topic_initialized) { @@ -427,277 +401,250 @@ BlinkM::led() if(led_thread_ready == true) { if(!detected_cells_blinked) { if(num_of_cells > 0) { - t_led_color_1 = LED_PURPLE; + t_led_color[0] = LED_PURPLE; } if(num_of_cells > 1) { - t_led_color_2 = LED_PURPLE; + t_led_color[1] = LED_PURPLE; } if(num_of_cells > 2) { - t_led_color_3 = LED_PURPLE; + t_led_color[2] = LED_PURPLE; } if(num_of_cells > 3) { - t_led_color_4 = LED_PURPLE; + t_led_color[3] = LED_PURPLE; } if(num_of_cells > 4) { - t_led_color_5 = LED_PURPLE; + t_led_color[4] = LED_PURPLE; } - t_led_color_6 = LED_OFF; + t_led_color[5] = LED_OFF; t_led_blink = LED_BLINK; } else { - t_led_color_1 = BlinkM::led_color_1; - t_led_color_2 = BlinkM::led_color_2; - t_led_color_3 = BlinkM::led_color_3; - t_led_color_4 = BlinkM::led_color_4; - t_led_color_5 = BlinkM::led_color_5; - t_led_color_6 = BlinkM::led_color_6; - t_led_blink = BlinkM::led_blink; + 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_blink = led_blink; } led_thread_ready = false; } - switch(led_thread_runcount) { - case 1: // 1. LED on - BlinkM::setLEDColor(t_led_color_1); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 2: // 1. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - led_thread_runcount++; - led_interval = LED_OFFTIME; - break; - case 3: // 2. LED on - BlinkM::setLEDColor(t_led_color_2); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 4: // 2. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - led_thread_runcount++; - led_interval = LED_OFFTIME; - break; - case 5: // 3. LED on - BlinkM::setLEDColor(t_led_color_3); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 6: // 3. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - led_thread_runcount++; - led_interval = LED_OFFTIME; - break; - case 7: // 4. LED on - BlinkM::setLEDColor(t_led_color_4); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 8: // 4. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - led_thread_runcount++; - led_interval = LED_OFFTIME; - break; - case 9: // 5. LED on - BlinkM::setLEDColor(t_led_color_5); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 10: // 5. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - led_thread_runcount++; - led_interval = LED_OFFTIME; - break; - case 11: // 6. LED on - BlinkM::setLEDColor(t_led_color_6); - led_thread_runcount++; - led_interval = LED_ONTIME; - break; - case 12: // 6. LED off - if(t_led_blink == LED_BLINK) { - BlinkM::setLEDColor(LED_OFF); - } - - /* obtained data for the first file descriptor */ - struct vehicle_status_s vehicle_status_raw; - struct vehicle_gps_position_s vehicle_gps_position_raw; - - bool new_data; - orb_check(vehicle_status_sub_fd, &new_data); - - if (new_data) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); - } - - orb_check(vehicle_gps_position_sub_fd, &new_data); - - if (new_data) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); - } - - /* get actual battery voltage */ - system_voltage = (int)vehicle_status_raw.voltage_battery*10; - - /* get number of used satellites in navigation */ - num_of_used_sats = 0; - for(int satloop=0; satloop<20; satloop++) { - if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { - num_of_used_sats++; - } - } - - if(num_of_cells == 0) { - /* looking for lipo cells that are connected */ - printf(" checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; - } - printf(" cells found:%u\n", num_of_cells); - } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - /* LED Pattern for battery low warning */ - BlinkM::led_color_1 = LED_YELLOW; - BlinkM::led_color_2 = LED_YELLOW; - BlinkM::led_color_3 = LED_YELLOW; - BlinkM::led_color_4 = LED_YELLOW; - BlinkM::led_color_5 = LED_YELLOW; - BlinkM::led_color_6 = LED_YELLOW; - BlinkM::led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - /* LED Pattern for battery critical alerting */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_RED; - BlinkM::led_color_5 = LED_RED; - BlinkM::led_color_6 = LED_RED; - BlinkM::led_blink = LED_BLINK; - - } else { - /* no battery warnings here */ - - if(vehicle_status_raw.flag_system_armed == false) { - /* system not armed */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_RED; - BlinkM::led_color_5 = LED_RED; - BlinkM::led_color_6 = LED_RED; - BlinkM::led_blink = LED_NOBLINK; - - } else { - /* armed system - initial led pattern */ - BlinkM::led_color_1 = LED_RED; - BlinkM::led_color_2 = LED_RED; - BlinkM::led_color_3 = LED_RED; - BlinkM::led_color_4 = LED_OFF; - BlinkM::led_color_5 = LED_OFF; - BlinkM::led_color_6 = LED_OFF; - BlinkM::led_blink = LED_BLINK; - - /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - BlinkM::led_color_4 = LED_OFF; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - BlinkM::led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - BlinkM::led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - BlinkM::led_color_4 = LED_GREEN; - break; - } - - /* handling used satīs */ - if(num_of_used_sats >= 7) { - BlinkM::led_color_1 = LED_OFF; - BlinkM::led_color_2 = LED_OFF; - BlinkM::led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { - BlinkM::led_color_2 = LED_OFF; - BlinkM::led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { - BlinkM::led_color_3 = LED_OFF; - } - } - } - } - - /* - printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%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, - vehicle_status_raw.battery_warning, - num_of_used_sats, - vehicle_gps_position_raw.fix_type, - vehicle_gps_position_raw.satellites_visible); - */ - - - led_thread_runcount=1; - led_thread_ready = true; - led_interval = LED_OFFTIME; - - if(detected_cells_runcount < 5){ - detected_cells_runcount++; - } else { - detected_cells_blinked = true; - } - - break; - default: - led_thread_runcount=1; - t_led_blink = 0; - led_thread_ready = true; - break; + 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) % 6]); + //led_interval = (led_thread_runcount & 1) : LED_ONTIME; + led_interval = LED_ONTIME; } - if(BlinkM::systemstate_run == true) { + if (led_thread_runcount == 11) { + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_gps_position_s 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++) { + if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { + num_of_used_sats++; + } + } + + if(new_data_vehicle_status || no_data_vehicle_status < 3){ + if(num_of_cells == 0) { + /* looking for lipo cells that are connected */ + printf(" 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:%u\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_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_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_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_blink = LED_BLINK; + + /* handle 4th led - flightmode indicator */ + switch((int)vehicle_status_raw.flight_mode) { + case VEHICLE_FLIGHT_MODE_MANUAL: + led_color_4 = LED_OFF; + 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_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 - BlinkM::set_rgb(0,0,0); + set_rgb(0,0,0); break; case LED_RED: // red - BlinkM::set_rgb(255,0,0); + set_rgb(255,0,0); break; case LED_YELLOW: // yellow - BlinkM::set_rgb(255,70,0); + set_rgb(255,70,0); break; case LED_PURPLE: // purple - BlinkM::set_rgb(255,0,255); + set_rgb(255,0,255); break; case LED_GREEN: // green - BlinkM::set_rgb(0,255,0); + set_rgb(0,255,0); break; case LED_BLUE: // blue - BlinkM::set_rgb(0,0,255); + set_rgb(0,0,255); break; case LED_WHITE: // white - BlinkM::set_rgb(255,255,255); + set_rgb(255,255,255); break; } } From 4613d1247d0dc7091be4ac477053e73866455e2f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 17:15:43 -0500 Subject: [PATCH 12/47] Added param comments for FWB controller. --- apps/examples/control_demo/params.c | 36 ++++++++++++++--------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index aea2603306..422e9b90e6 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -24,40 +24,40 @@ PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); -PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f); // pitch angle to pitch-rate PID PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); // h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); // crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f); +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f); // cross-track to yaw angle gain // speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); -PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); -PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); +PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity // trim -PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); -PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f); +PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f); // trim throttle (0,1) From 5b0aa490d68741fc067923e7ef801f672dcb5819 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 18:38:17 -0500 Subject: [PATCH 13/47] Added P0. Hid some printing. Corrected fault detection. --- apps/examples/kalman_demo/KalmanNav.cpp | 26 ++++++++++++++++++------- apps/examples/kalman_demo/KalmanNav.hpp | 1 + 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index e804fbca5c..a735de406f 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -45,8 +45,6 @@ // Titterton pg. 52 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s static const float R0 = 6378137.0f; // earth radius, m - -static const float RSq = 4.0680631591e+13; // earth radius squared static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : @@ -55,6 +53,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : F(9, 9), G(9, 6), P(9, 9), + P0(9, 9), V(6, 6), // attitude measurement ekf matrices HAtt(6, 9), @@ -100,7 +99,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P = Matrix::identity(9) * 1.0f; + P0 = Matrix::identity(9) * 1.0f; + P = P0; // wait for gps lock while (1) { @@ -132,6 +132,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : setLonDegE7(_gps.lon); setAltE3(_gps.alt); + printf("[kalman_demo] initializing EKF state with GPS\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); + // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -335,6 +342,10 @@ void KalmanNav::predictFast(float dt) vN * LDot + g; float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; + printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", + double(vNDot), double(vEDot), double(vDDot)); + printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", + double(LDot), double(lDot), double(rotRate)); // rectangular integration //printf("dt: %8.4f\n", double(dt)); @@ -360,6 +371,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); + float RSq = R*R; // F Matrix // Titterton pg. 291 @@ -558,7 +570,7 @@ void KalmanNav::correctAtt() // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); // reset P matrix to identity - P = Matrix::identity(9) * 1.0f; + P = P0; return; } } @@ -581,7 +593,7 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot((S * S.transpose()).inverse() * y); + float beta = y.dot(S.inverse()*y); if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); @@ -641,7 +653,7 @@ void KalmanNav::correctPos() setLonDegE7(_gps.lon); setAltE3(_gps.alt); // reset P matrix to identity - P = Matrix::identity(9) * 1.0f; + P = P0; return; } } @@ -659,7 +671,7 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot((S * S.transpose()).inverse() * y); + float beta = y.dot(S.inverse()*y); if (beta > 1000.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index 1ea46d40ff..c074cb7c31 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -81,6 +81,7 @@ protected: math::Matrix F; math::Matrix G; math::Matrix P; + math::Matrix P0; math::Matrix V; math::Matrix HAtt; math::Matrix RAtt; From d02a24ec82e35016545d519ea88d46881c42df2d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 11:00:52 -0500 Subject: [PATCH 14/47] Adding comments to ekf. --- apps/examples/kalman_demo/KalmanNav.cpp | 39 ++++---- apps/examples/kalman_demo/KalmanNav.hpp | 117 ++++++++++++++++-------- 2 files changed, 102 insertions(+), 54 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index a735de406f..5f71da58e7 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // miss counts _missFast(0), _missSlow(0), - // state + // accelerations fN(0), fE(0), fD(0), + // state phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), @@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsVel(this, "R_GPS_VEL"), _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), - _rAccel(this, "R_ACCEL") + _rAccel(this, "R_ACCEL"), + _magDip(this, "MAG_DIP"), + _magDec(this, "MAG_DEC") { using namespace math; // initial state covariance matrix P0 = Matrix::identity(9) * 1.0f; - P = P0; + P = P0; // wait for gps lock while (1) { @@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : setLonDegE7(_gps.lon); setAltE3(_gps.alt); - printf("[kalman_demo] initializing EKF state with GPS\n"); - printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", - double(phi), double(theta), double(psi)); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(vN), double(vE), double(vD), - lat, lon, alt); + printf("[kalman_demo] initializing EKF state with GPS\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt) vN * LDot + g; float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; - printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", - double(vNDot), double(vEDot), double(vDDot)); - printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", - double(LDot), double(lDot), double(rotRate)); + printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", + double(vNDot), double(vEDot), double(vDDot)); + printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", + double(LDot), double(lDot), double(rotRate)); // rectangular integration //printf("dt: %8.4f\n", double(dt)); @@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); - float RSq = R*R; + float RSq = R * R; // F Matrix // Titterton pg. 291 @@ -472,8 +475,8 @@ void KalmanNav::correctAtt() // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); @@ -593,7 +596,7 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse()*y); + float beta = y.dot(S.inverse() * y); if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); @@ -671,7 +674,7 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse()*y); + float beta = y.dot(S.inverse() * y); if (beta > 1000.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index c074cb7c31..af4588e20e 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -68,53 +68,98 @@ class KalmanNav : public control::SuperBlock { public: + /** + * Constructor + */ KalmanNav(SuperBlock *parent, const char *name); + + /** + * Deconstuctor + */ + virtual ~KalmanNav() {}; + /** + * The main callback function for the class + */ void update(); + + /** + * Fast prediction + * Contains: state integration + */ virtual void updatePublications(); void predictFast(float dt); + + /** + * Slow prediction + * Contains: covariance integration + */ void predictSlow(float dt); + + /** + * Attitude correction + */ void correctAtt(); + + /** + * Position correction + */ void correctPos(); + + /** + * Overloaded update parameters + */ virtual void updateParams(); protected: - math::Matrix F; - math::Matrix G; - math::Matrix P; - math::Matrix P0; - math::Matrix V; - math::Matrix HAtt; - math::Matrix RAtt; - math::Matrix HPos; - math::Matrix RPos; - math::Dcm C_nb; - math::Quaternion q; - control::UOrbSubscription _sensors; - control::UOrbSubscription _gps; - control::UOrbSubscription _param_update; - control::UOrbPublication _pos; - control::UOrbPublication _att; - uint64_t _pubTimeStamp; - uint64_t _fastTimeStamp; - uint64_t _slowTimeStamp; - uint64_t _attTimeStamp; - uint64_t _outTimeStamp; - uint16_t _navFrames; - uint16_t _missFast; - uint16_t _missSlow; - float fN, fE, fD; + // kalman filter + math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix G; /**< noise shaping matrix for gyro/accel */ + math::Matrix P; /**< state covariance matrix */ + math::Matrix P0; /**< initial state covariance matrix */ + math::Matrix V; /**< gyro/ accel noise matrix */ + math::Matrix HAtt; /**< attitude measurement matrix */ + math::Matrix RAtt; /**< attitude measurement noise matrix */ + math::Matrix HPos; /**< position measurement jacobian matrix */ + math::Matrix RPos; /**< position measurement noise matrix */ + // attitude + math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + control::UOrbSubscription _sensors; /**< sensors sub. */ + control::UOrbSubscription _gps; /**< gps sub. */ + control::UOrbSubscription _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication _pos; /**< position pub. */ + control::UOrbPublication _att; /**< attitude pub. */ + // time stamps + uint64_t _pubTimeStamp; /**< output data publication time stamp */ + uint64_t _fastTimeStamp; /**< fast prediction time stamp */ + uint64_t _slowTimeStamp; /**< slow prediction time stamp */ + uint64_t _attTimeStamp; /**< attitude correction time stamp */ + uint64_t _outTimeStamp; /**< output time stamp */ + // frame count + uint16_t _navFrames; /**< navigation frames completed in output cycle */ + // miss counts + uint16_t _missFast; /**< number of times fast prediction loop missed */ + uint16_t _missSlow; /**< number of times slow prediction loop missed */ + // accelerations + float fN, fE, fD; /**< navigation frame acceleration */ // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; - float phi, theta, psi; - float vN, vE, vD; - double lat, lon, alt; - control::BlockParam _vGyro; - control::BlockParam _vAccel; - control::BlockParam _rMag; - control::BlockParam _rGpsVel; - control::BlockParam _rGpsPos; - control::BlockParam _rGpsAlt; - control::BlockParam _rAccel; + enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ + float phi, theta, psi; /**< 3-2-1 euler angles */ + float vN, vE, vD; /**< navigation velocity, m/s */ + double lat, lon, alt; /**< lat, lon, alt, radians */ + // parameters + control::BlockParam _vGyro; /**< gyro process noise */ + control::BlockParam _vAccel; /**< accelerometer process noise */ + control::BlockParam _rMag; /**< magnetometer measurement noise */ + control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam _rGpsPos; /**< gps position measurement noise */ + control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam _rAccel; /**< accelerometer measurement noise */ + control::BlockParam _magDip; /**< magnetic inclination with level */ + control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } From 8b6660fc369806da509da5b5b98acc51da83811f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 11:16:28 -0500 Subject: [PATCH 15/47] Fixed param issue. --- apps/examples/kalman_demo/KalmanNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 5f71da58e7..081664d173 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -475,8 +475,8 @@ void KalmanNav::correctAtt() // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); From 281372ef3ad14d61eb720ddbe05e701e82aabad0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 11:36:49 -0500 Subject: [PATCH 16/47] Added mag dip/dec as parameters. --- apps/examples/kalman_demo/params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index f36ac334f9..93571d433b 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -8,3 +8,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); From 022c30ea4f8355799b8ef25d8fb31037df527bb9 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 12:17:09 -0500 Subject: [PATCH 17/47] Enabled kf to run w/o gps. --- apps/examples/kalman_demo/KalmanNav.cpp | 68 +++++++++++-------------- apps/examples/kalman_demo/KalmanNav.hpp | 2 + 2 files changed, 33 insertions(+), 37 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 081664d173..b5c8a10738 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -97,50 +97,27 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsAlt(this, "R_GPS_ALT"), _rAccel(this, "R_ACCEL"), _magDip(this, "MAG_DIP"), - _magDec(this, "MAG_DEC") + _magDec(this, "MAG_DEC"), + _positionInitialized(false) { using namespace math; // initial state covariance matrix - P0 = Matrix::identity(9) * 1.0f; + P0 = Matrix::identity(9) * 0.01f; P = P0; - // wait for gps lock - while (1) { - struct pollfd fds[1]; - fds[0].fd = _gps.getHandle(); - fds[0].events = POLLIN; - - // poll 10 seconds for new data - int ret = poll(fds, 1, 10000); - - if (ret > 0) { - updateSubscriptions(); - - if (_gps.fix_type > 2) break; - - } else if (ret == 0) { - printf("[kalman_demo] waiting for gps lock\n"); - } - } - // initial state phi = 0.0f; theta = 0.0f; psi = 0.0f; - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); + vN = 0.0f; + vE = 0.0f; + vD = 0.0f; + lat = 0.0f; + lon = 0.0f; + alt = 0.0f; + - printf("[kalman_demo] initializing EKF state with GPS\n"); - printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", - double(phi), double(theta), double(psi)); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(vN), double(vE), double(vD), - lat, lon, alt); // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -199,6 +176,23 @@ void KalmanNav::update() // abort update if no new data if (!(sensorsUpdate || gpsUpdate)) return; + // if received gps for first time, reset position to gps + if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) { + vN = _gps.vel_n; + vE = _gps.vel_e; + vD = _gps.vel_d; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + _positionInitialized = true; + printf("[kalman_demo] initializing EKF state with GPS\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); + } + // fast prediciton step // note, using sensors timestamp so we can account // for packet lag @@ -345,10 +339,10 @@ void KalmanNav::predictFast(float dt) vN * LDot + g; float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; - printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", - double(vNDot), double(vEDot), double(vDDot)); - printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", - double(LDot), double(lDot), double(rotRate)); + //printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", + //double(vNDot), double(vEDot), double(vDDot)); + //printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", + //double(LDot), double(lDot), double(rotRate)); // rectangular integration //printf("dt: %8.4f\n", double(dt)); diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index af4588e20e..ba3756f468 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -159,6 +159,8 @@ protected: control::BlockParam _rAccel; /**< accelerometer measurement noise */ control::BlockParam _magDip; /**< magnetic inclination with level */ control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + // status + bool _positionInitialized; /**< status, if position has been init. */ // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } From edf0a6bae7fa5ae8c6d4df63489dcf75ec008517 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 12:37:12 -0500 Subject: [PATCH 18/47] Added check for valid attitude data. --- apps/examples/kalman_demo/KalmanNav.cpp | 25 +++++++++++++++++-------- apps/examples/kalman_demo/params.c | 4 ++-- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index b5c8a10738..68deae1856 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -177,7 +177,10 @@ void KalmanNav::update() if (!(sensorsUpdate || gpsUpdate)) return; // if received gps for first time, reset position to gps - if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) { + if (!_positionInitialized && + gpsUpdate && + _gps.fix_type > 2 && + _gps.counter_pos_valid > 10) { vN = _gps.vel_n; vE = _gps.vel_e; vD = _gps.vel_d; @@ -187,10 +190,10 @@ void KalmanNav::update() _positionInitialized = true; printf("[kalman_demo] initializing EKF state with GPS\n"); printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", - double(phi), double(theta), double(psi)); + double(phi), double(theta), double(psi)); printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(vN), double(vE), double(vD), - lat, lon, alt); + double(vN), double(vE), double(vD), + lat, lon, alt); } // fast prediciton step @@ -340,9 +343,9 @@ void KalmanNav::predictFast(float dt) float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; //printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", - //double(vNDot), double(vEDot), double(vDDot)); + //double(vNDot), double(vEDot), double(vDDot)); //printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", - //double(LDot), double(lDot), double(rotRate)); + //double(LDot), double(lDot), double(rotRate)); // rectangular integration //printf("dt: %8.4f\n", double(dt)); @@ -455,6 +458,12 @@ void KalmanNav::correctAtt() { using namespace math; + // check for valid data, return if not ready + if (_sensors.accelerometer_counter < 10 || + _sensors.gyro_counter < 10 || + _sensors.magnetometer_counter < 10) + return; + // trig float cosPhi = cosf(phi); float cosTheta = cosf(theta); @@ -566,7 +575,7 @@ void KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); - // reset P matrix to identity + // reset P matrix to P0 P = P0; return; } @@ -649,7 +658,7 @@ void KalmanNav::correctPos() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); - // reset P matrix to identity + // reset P matrix to P0 P = P0; return; } diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 93571d433b..55be116fa5 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,8 +1,8 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); From 9cf3d51aec4e73f626cfdea29b19bdfe80eea384 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 13:12:00 -0500 Subject: [PATCH 19/47] Made fault tolerances adjustable. --- apps/examples/kalman_demo/KalmanNav.cpp | 6 ++++-- apps/examples/kalman_demo/KalmanNav.hpp | 2 ++ apps/examples/kalman_demo/params.c | 2 ++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 68deae1856..918dfd9df8 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -98,6 +98,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rAccel(this, "R_ACCEL"), _magDip(this, "MAG_DIP"), _magDec(this, "MAG_DEC"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), _positionInitialized(false) { using namespace math; @@ -601,7 +603,7 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); } @@ -679,7 +681,7 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultPos.get()) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(y(0) / noiseVel), diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index ba3756f468..4af8bbf5c2 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -159,6 +159,8 @@ protected: control::BlockParam _rAccel; /**< accelerometer measurement noise */ control::BlockParam _magDip; /**< magnetic inclination with level */ control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam _faultPos; /**< fault detection threshold for position */ + control::BlockParam _faultAtt; /**< fault detection threshold for attitude */ // status bool _positionInitialized; /**< status, if position has been init. */ // accessors diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 55be116fa5..b98dd8fd72 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -10,3 +10,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); From 68b92cd4fc2c4df3de15ef19e723edb67a108ea0 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 13:21:13 -0500 Subject: [PATCH 20/47] Slowed HIL status updates. Also prevented posCor. when gps not init. --- apps/examples/kalman_demo/KalmanNav.cpp | 8 ++++++-- apps/mavlink/mavlink_receiver.c | 12 ++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 918dfd9df8..38cdb6ca01 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -239,9 +239,10 @@ void KalmanNav::update() } // output - if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow); + printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", + _navFrames/10, _missFast/10, _missSlow/10); _navFrames = 0; _missFast = 0; _missSlow = 0; @@ -616,6 +617,9 @@ void KalmanNav::correctAtt() void KalmanNav::correctPos() { using namespace math; + + if (!_positionInitialized) return; + Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index fa63c419f0..0b63c30a4a 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -367,8 +367,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil imu at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -412,8 +412,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil gps at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil gps at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -454,8 +454,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil pressure at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil pressure at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } From 28ef7aa1bea97593637537c832b07459a2520b86 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 14:03:04 -0500 Subject: [PATCH 21/47] Refactored RPos. Increased global pos output rate for debugging. --- apps/examples/kalman_demo/KalmanNav.cpp | 46 +++++++++---------------- apps/mavlink/orb_listener.c | 2 +- 2 files changed, 18 insertions(+), 30 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 38cdb6ca01..0f3069d11e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -574,7 +574,6 @@ void KalmanNav::correctAtt() // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); - if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); @@ -589,9 +588,7 @@ void KalmanNav::correctAtt() phi += xCorrect(PHI); theta += xCorrect(THETA); } - psi += xCorrect(PSI); - // attitude also affects nav velocities vN += xCorrect(VN); vE += xCorrect(VE); @@ -603,7 +600,6 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot(S.inverse() * y); - if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); @@ -620,6 +616,7 @@ void KalmanNav::correctPos() if (!_positionInitialized) return; + // residual Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; @@ -627,23 +624,6 @@ void KalmanNav::correctPos() y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; y(4) = double(_gps.alt) / 1.0e3 - alt; - // update gps noise - float cosLSing = cosf(lat); - float R = R0 + float(alt); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseAlt = _rGpsAlt.get(); - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; - // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance @@ -653,7 +633,6 @@ void KalmanNav::correctPos() // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); - if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); @@ -684,15 +663,14 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > _faultPos.get()) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(y(0) / noiseVel), - double(y(1) / noiseVel), - double(y(2) / noiseLatDegE7), - double(y(3) / noiseLonDegE7), - double(y(4) / noiseAlt)); + double(y(0) / sqrtf(RPos(0,0))), + double(y(1) / sqrtf(RPos(1,1))), + double(y(2) / sqrtf(RPos(2,2))), + double(y(3) / sqrtf(RPos(3,3))), + double(y(4) / sqrtf(RPos(3,3)))); } } @@ -702,7 +680,6 @@ void KalmanNav::updateParams() using namespace control; SuperBlock::updateParams(); - // gyro noise V(0, 0) = _vGyro.get(); // gyro x, rad/s V(1, 1) = _vGyro.get(); // gyro y @@ -714,13 +691,17 @@ void KalmanNav::updateParams() V(5, 5) = _vAccel.get(); // accel z // magnetometer noise + float noiseMin = 1e-6f; float noiseMagSq = _rMag.get() * _rMag.get(); + if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; RAtt(0, 0) = noiseMagSq; // normalized direction RAtt(1, 1) = noiseMagSq; RAtt(2, 2) = noiseMagSq; // accelerometer noise float noiseAccelSq = _rAccel.get() * _rAccel.get(); + // bound noise to prevent singularities + if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; RAtt(3, 3) = noiseAccelSq; // normalized direction RAtt(4, 4) = noiseAccelSq; RAtt(5, 5) = noiseAccelSq; @@ -739,9 +720,16 @@ void KalmanNav::updateParams() float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseAlt = _rGpsAlt.get(); + // bound noise to prevent singularities + if (noiseVel < noiseMin) noiseVel = noiseMin; + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + if (noiseAlt < noiseMin) noiseAlt = noiseMin; RPos(0, 0) = noiseVel * noiseVel; // vn RPos(1, 1) = noiseVel * noiseVel; // ve RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon RPos(4, 4) = noiseAlt * noiseAlt; // h + // XXX, note that RPos depends on lat, so updateParams should + // be called if lat changes significantly } diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f5253ea358..a7b43e2b9b 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -702,7 +702,7 @@ uorb_receive_start(void) /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ + orb_set_interval(mavlink_subs.global_pos_sub, 5); /* 200 Hz active updates */ /* --- LOCAL POS VALUE --- */ mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); From afb69cd05450d6df1bf2233b95030c9b93daaf1e Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 15:11:24 -0500 Subject: [PATCH 22/47] Reducing pos/att correction update rates for debugging. --- apps/examples/kalman_demo/KalmanNav.cpp | 8 +++----- apps/mavlink/orb_listener.c | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 0f3069d11e..3966b4fcd1 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -65,8 +65,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : C_nb(), q(), // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz + _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), @@ -208,7 +208,6 @@ void KalmanNav::update() predictFast(dtFast); // count fast frames _navFrames += 1; - } else _missFast++; // slow prediction step @@ -216,7 +215,6 @@ void KalmanNav::update() if (dtSlow > 1.0f / 100) { // 100 Hz _slowTimeStamp = _sensors.timestamp; - if (dtSlow < 1.0f / 50) predictSlow(dtSlow); else _missSlow ++; } @@ -227,7 +225,7 @@ void KalmanNav::update() } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz + if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz _attTimeStamp = _sensors.timestamp; correctAtt(); } diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index a7b43e2b9b..43bb4fb284 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -702,7 +702,7 @@ uorb_receive_start(void) /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 5); /* 200 Hz active updates */ + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ /* --- LOCAL POS VALUE --- */ mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); From 68a6a64213b5af66771a6a302bf06c4c588dc719 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 15 Jan 2013 18:25:08 -0500 Subject: [PATCH 23/47] Working on velocity errors. --- apps/examples/kalman_demo/KalmanNav.cpp | 86 ++++++++++++++----------- apps/examples/kalman_demo/KalmanNav.hpp | 1 + apps/examples/kalman_demo/params.c | 1 + apps/mavlink/mavlink_receiver.c | 7 +- 4 files changed, 57 insertions(+), 38 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 3966b4fcd1..41e948ae2d 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -45,7 +45,7 @@ // Titterton pg. 52 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s static const float R0 = 6378137.0f; // earth radius, m -static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated +static const float g0 = 9.806f; // standard gravitational accel. m/s^2 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), @@ -98,6 +98,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rAccel(this, "R_ACCEL"), _magDip(this, "MAG_DIP"), _magDec(this, "MAG_DEC"), + _g(this, "G"), _faultPos(this, "FAULT_POS"), _faultAtt(this, "FAULT_ATT"), _positionInitialized(false) @@ -119,8 +120,6 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - - // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -142,16 +141,12 @@ void KalmanNav::update() { using namespace math; - struct pollfd fds[3]; + struct pollfd fds[1]; fds[0].fd = _sensors.getHandle(); fds[0].events = POLLIN; - fds[1].fd = _param_update.getHandle(); - fds[1].events = POLLIN; - fds[2].fd = _gps.getHandle(); - fds[2].events = POLLIN; - // poll 20 milliseconds for new data - int ret = poll(fds, 2, 20); + // poll for new data + int ret = poll(fds, 1, 1000); // check return value if (ret < 0) { @@ -202,22 +197,25 @@ void KalmanNav::update() // note, using sensors timestamp so we can account // for packet lag float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; + printf("dtFast: %15.10f\n", double(dtFast)); _fastTimeStamp = _sensors.timestamp; - if (dtFast < 1.0f / 50) { + if (dtFast < 1.0f) { predictFast(dtFast); + predictSlow(dtFast); // count fast frames _navFrames += 1; + } else _missFast++; // slow prediction step - float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f; + //float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f; - if (dtSlow > 1.0f / 100) { // 100 Hz - _slowTimeStamp = _sensors.timestamp; - if (dtSlow < 1.0f / 50) predictSlow(dtSlow); - else _missSlow ++; - } + //if (dtSlow > 1.0f / 100) { // 100 Hz + //_slowTimeStamp = _sensors.timestamp; + //if (dtSlow < 1.0f / 50) predictSlow(dtSlow); + //else _missSlow ++; + //} // gps correction step if (gpsUpdate) { @@ -240,7 +238,7 @@ void KalmanNav::update() if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", - _navFrames/10, _missFast/10, _missSlow/10); + _navFrames / 10, _missFast / 10, _missSlow / 10); _navFrames = 0; _missFast = 0; _missSlow = 0; @@ -340,18 +338,18 @@ void KalmanNav::predictFast(float dt) float vNDot = fN - vE * rotRate * sinL + vD * LDot; float vDDot = fD - vE * rotRate * cosL - - vN * LDot + g; + vN * LDot + _g.get(); float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; - //printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", - //double(vNDot), double(vEDot), double(vDDot)); - //printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", - //double(LDot), double(lDot), double(rotRate)); + printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", + double(vNDot), double(vEDot), double(vDDot)); + printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", + double(LDot), double(lDot), double(rotRate)); // rectangular integration - //printf("dt: %8.4f\n", double(dt)); - //printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD)); - //printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD)); + printf("dt: %8.4f\n", double(dt)); + printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD)); vN += vNDot * dt; vE += vEDot * dt; vD += vDDot * dt; @@ -495,7 +493,7 @@ void KalmanNav::correctAtt() // ignore accel correction when accel mag not close to g Matrix RAttAdjust = RAtt; - bool ignoreAccel = fabsf(accelMag - g) > 1.1f; + bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; if (ignoreAccel) { RAttAdjust(3, 3) = 1.0e10; @@ -511,7 +509,7 @@ void KalmanNav::correctAtt() //Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi); // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit(); + Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get()) /*+ zCentrip*/).unit(); // combined measurement Vector zAtt(6); @@ -572,6 +570,7 @@ void KalmanNav::correctAtt() // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); + if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); @@ -586,11 +585,12 @@ void KalmanNav::correctAtt() phi += xCorrect(PHI); theta += xCorrect(THETA); } + psi += xCorrect(PSI); // attitude also affects nav velocities - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); + //vN += xCorrect(VN); + //vE += xCorrect(VE); + //vD += xCorrect(VD); // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -598,6 +598,7 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot(S.inverse() * y); + if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); @@ -631,6 +632,7 @@ void KalmanNav::correctPos() // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); + if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); @@ -661,14 +663,15 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); + if (beta > _faultPos.get()) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(y(0) / sqrtf(RPos(0,0))), - double(y(1) / sqrtf(RPos(1,1))), - double(y(2) / sqrtf(RPos(2,2))), - double(y(3) / sqrtf(RPos(3,3))), - double(y(4) / sqrtf(RPos(3,3)))); + double(y(0) / sqrtf(RPos(0, 0))), + double(y(1) / sqrtf(RPos(1, 1))), + double(y(2) / sqrtf(RPos(2, 2))), + double(y(3) / sqrtf(RPos(3, 3))), + double(y(4) / sqrtf(RPos(4, 4)))); } } @@ -691,22 +694,26 @@ void KalmanNav::updateParams() // magnetometer noise float noiseMin = 1e-6f; float noiseMagSq = _rMag.get() * _rMag.get(); + if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; + RAtt(0, 0) = noiseMagSq; // normalized direction RAtt(1, 1) = noiseMagSq; RAtt(2, 2) = noiseMagSq; // accelerometer noise float noiseAccelSq = _rAccel.get() * _rAccel.get(); + // bound noise to prevent singularities if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; + RAtt(3, 3) = noiseAccelSq; // normalized direction RAtt(4, 4) = noiseAccelSq; RAtt(5, 5) = noiseAccelSq; // gps noise - float cosLSing = cosf(lat); float R = R0 + float(alt); + float cosLSing = cosf(lat); // prevent singularity if (fabsf(cosLSing) < 0.01f) { @@ -718,11 +725,16 @@ void KalmanNav::updateParams() float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseAlt = _rGpsAlt.get(); + // bound noise to prevent singularities if (noiseVel < noiseMin) noiseVel = noiseMin; + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + if (noiseAlt < noiseMin) noiseAlt = noiseMin; + RPos(0, 0) = noiseVel * noiseVel; // vn RPos(1, 1) = noiseVel * noiseVel; // ve RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index 4af8bbf5c2..d7068beecf 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -159,6 +159,7 @@ protected: control::BlockParam _rAccel; /**< accelerometer measurement noise */ control::BlockParam _magDip; /**< magnetic inclination with level */ control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam _g; /**< gravitational constant */ control::BlockParam _faultPos; /**< fault detection threshold for position */ control::BlockParam _faultAtt; /**< fault detection threshold for attitude */ // status diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index b98dd8fd72..2cdbc1435f 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -12,3 +12,4 @@ PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_G, 9.806f); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 0b63c30a4a..9491ce7e3b 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -318,9 +318,11 @@ handle_message(mavlink_message_t *msg) static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; + /* sensors general */ + hil_sensors.timestamp = imu.time_usec; + /* hil gyro */ static const float mrad2rad = 1.0e-3f; - hil_sensors.timestamp = timestamp; hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro; hil_sensors.gyro_raw[1] = imu.ygyro; @@ -429,6 +431,9 @@ handle_message(mavlink_message_t *msg) static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; + /* sensors general */ + hil_sensors.timestamp = press.time_usec; + /* baro */ /* TODO, set ground_press/ temp during calib */ static const float ground_press = 1013.25f; // mbar From ce3f835c637338086a18307c61deb35ccddbee05 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Tue, 15 Jan 2013 23:36:01 -0500 Subject: [PATCH 24/47] Mag and velocity measurement fix. Fault detection working. --- apps/examples/kalman_demo/KalmanNav.cpp | 79 +++++++++++-------------- apps/examples/kalman_demo/KalmanNav.hpp | 29 +++++---- apps/examples/kalman_demo/params.c | 25 ++++---- 3 files changed, 62 insertions(+), 71 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 41e948ae2d..d93a7bdc6a 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -73,15 +73,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), - _fastTimeStamp(hrt_absolute_time()), - _slowTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), _attTimeStamp(hrt_absolute_time()), _outTimeStamp(hrt_absolute_time()), // frame count _navFrames(0), // miss counts - _missFast(0), - _missSlow(0), + _miss(0), // accelerations fN(0), fE(0), fD(0), // state @@ -96,9 +94,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), _rAccel(this, "R_ACCEL"), - _magDip(this, "MAG_DIP"), - _magDec(this, "MAG_DEC"), - _g(this, "G"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), _faultPos(this, "FAULT_POS"), _faultAtt(this, "FAULT_ATT"), _positionInitialized(false) @@ -193,29 +191,21 @@ void KalmanNav::update() lat, lon, alt); } - // fast prediciton step - // note, using sensors timestamp so we can account - // for packet lag - float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; - printf("dtFast: %15.10f\n", double(dtFast)); - _fastTimeStamp = _sensors.timestamp; - - if (dtFast < 1.0f) { - predictFast(dtFast); - predictSlow(dtFast); + // prediciton step + // using sensors timestamp so we can account for packet lag + float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dtFast)); + _predictTimeStamp = _sensors.timestamp; + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); // count fast frames _navFrames += 1; + } - } else _missFast++; - - // slow prediction step - //float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f; - - //if (dtSlow > 1.0f / 100) { // 100 Hz - //_slowTimeStamp = _sensors.timestamp; - //if (dtSlow < 1.0f / 50) predictSlow(dtSlow); - //else _missSlow ++; - //} + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; // gps correction step if (gpsUpdate) { @@ -237,11 +227,10 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", - _navFrames / 10, _missFast / 10, _missSlow / 10); + printf("nav: %4d Hz, miss #: %4d\n", + _navFrames / 10, _miss/ 10); _navFrames = 0; - _missFast = 0; - _missSlow = 0; + _miss= 0; } } @@ -288,7 +277,7 @@ void KalmanNav::updatePublications() SuperBlock::updatePublications(); } -void KalmanNav::predictFast(float dt) +void KalmanNav::predictState(float dt) { using namespace math; Vector3 w(_sensors.gyro_rad_s); @@ -336,20 +325,13 @@ void KalmanNav::predictFast(float dt) float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; float vNDot = fN - vE * rotRate * sinL + - vD * LDot; + vD * LDot; float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); + vN * LDot + _g.get(); float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", - double(vNDot), double(vEDot), double(vDDot)); - printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", - double(LDot), double(lDot), double(rotRate)); + vDDot * rotRate * cosL; // rectangular integration - printf("dt: %8.4f\n", double(dt)); - printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD)); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD)); vN += vNDot * dt; vE += vEDot * dt; vD += vDDot * dt; @@ -358,7 +340,7 @@ void KalmanNav::predictFast(float dt) alt += double(-vD * dt); } -void KalmanNav::predictSlow(float dt) +void KalmanNav::predictStateCovariance(float dt) { using namespace math; @@ -473,6 +455,8 @@ void KalmanNav::correctAtt() // mag measurement Vector3 zMag(_sensors.magnetometer_ga); + //float magNorm = zMag.norm(); + zMag = zMag.unit(); // mag predicted measurement // choosing some typical magnetic field properties, @@ -588,9 +572,9 @@ void KalmanNav::correctAtt() psi += xCorrect(PSI); // attitude also affects nav velocities - //vN += xCorrect(VN); - //vE += xCorrect(VE); - //vD += xCorrect(VD); + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -602,6 +586,9 @@ void KalmanNav::correctAtt() if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); + printf("zMagHat:\n"); zMagHat.print(); + printf("zMag:\n"); zMag.print(); + printf("bNav:\n"); bNav.print(); } // update quaternions from euler diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index d7068beecf..da1a7ee108 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -83,18 +83,23 @@ public: */ void update(); - /** - * Fast prediction - * Contains: state integration - */ - virtual void updatePublications(); - void predictFast(float dt); /** - * Slow prediction - * Contains: covariance integration + * Publication update */ - void predictSlow(float dt); + virtual void updatePublications(); + + /** + * State prediction + * Continuous, non-linear + */ + void predictState(float dt); + + /** + * State covariance prediction + * Continuous, linear + */ + void predictStateCovariance(float dt); /** * Attitude correction @@ -133,15 +138,13 @@ protected: control::UOrbPublication _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _fastTimeStamp; /**< fast prediction time stamp */ - uint64_t _slowTimeStamp; /**< slow prediction time stamp */ + uint64_t _predictTimeStamp; /**< prediction time stamp */ uint64_t _attTimeStamp; /**< attitude correction time stamp */ uint64_t _outTimeStamp; /**< output time stamp */ // frame count uint16_t _navFrames; /**< navigation frames completed in output cycle */ // miss counts - uint16_t _missFast; /**< number of times fast prediction loop missed */ - uint16_t _missSlow; /**< number of times slow prediction loop missed */ + uint16_t _miss; /**< number of times fast prediction loop missed */ // accelerations float fN, fE, fD; /**< navigation frame acceleration */ // states diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 2cdbc1435f..58ebeba3c5 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,15 +1,16 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); + From f8811649cb91fc88cef7b224b29c6c5f235f1d8d Mon Sep 17 00:00:00 2001 From: jgoppert Date: Wed, 16 Jan 2013 00:24:14 -0500 Subject: [PATCH 25/47] Controller/ EKF tuning. --- apps/examples/control_demo/params.c | 12 ++++++------ apps/examples/kalman_demo/KalmanNav.cpp | 17 +++++++++-------- apps/examples/kalman_demo/params.c | 16 ++++++++-------- 3 files changed, 23 insertions(+), 22 deletions(-) diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 422e9b90e6..3eed83b5cb 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -15,16 +15,16 @@ PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass // stabilization mode PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder // psi -> phi -> p PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass @@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f); // pitch angle to pitch-rate PID +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); @@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); // crosstrack PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f); // cross-track to yaw angle gain +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain // speed command PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity @@ -60,4 +60,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f); // trim throttle (0,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index d93a7bdc6a..6acf39be8c 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -66,7 +66,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), @@ -193,9 +193,10 @@ void KalmanNav::update() // prediciton step // using sensors timestamp so we can account for packet lag - float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; //printf("dt: %15.10f\n", double(dtFast)); _predictTimeStamp = _sensors.timestamp; + // don't predict if time greater than a second if (dt < 1.0f) { predictState(dt); @@ -213,7 +214,7 @@ void KalmanNav::update() } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz + if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -228,9 +229,9 @@ void KalmanNav::update() if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss/ 10); + _navFrames / 10, _miss / 10); _navFrames = 0; - _miss= 0; + _miss = 0; } } @@ -325,11 +326,11 @@ void KalmanNav::predictState(float dt) float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; float vNDot = fN - vE * rotRate * sinL + - vD * LDot; + vD * LDot; float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); + vN * LDot + _g.get(); float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; + vDDot * rotRate * cosL; // rectangular integration vN += vNDot * dt; diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 58ebeba3c5..77225b4c75 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,16 +1,16 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); From 41ac3fdef9e3b7210286b438f3dae50af06b814c Mon Sep 17 00:00:00 2001 From: jgoppert Date: Wed, 16 Jan 2013 00:25:53 -0500 Subject: [PATCH 26/47] Increased fault threshhold. --- apps/examples/kalman_demo/params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 77225b4c75..bb18d5b928 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -8,7 +8,7 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); From ded442fd194442134accf0080be3fe73098481c1 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 16 Jan 2013 13:27:04 -0500 Subject: [PATCH 27/47] Added position initialization. --- apps/examples/kalman_demo/KalmanNav.cpp | 169 +++++++++++++----------- apps/examples/kalman_demo/KalmanNav.hpp | 12 +- apps/examples/kalman_demo/params.c | 2 +- 3 files changed, 102 insertions(+), 81 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 6acf39be8c..15254f7c79 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -46,6 +46,8 @@ static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s static const float R0 = 6378137.0f; // earth radius, m static const float g0 = 9.806f; // standard gravitational accel. m/s^2 +static const int8_t ret_ok = 0; // no error in function +static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), @@ -99,7 +101,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _g(this, "ENV_G"), _faultPos(this, "FAULT_POS"), _faultAtt(this, "FAULT_ATT"), - _positionInitialized(false) + _attitudeInitialized(false), + _positionInitialized(false), + _attitudeInitCounter(0) { using namespace math; @@ -146,7 +150,6 @@ void KalmanNav::update() // poll for new data int ret = poll(fds, 1, 1000); - // check return value if (ret < 0) { // XXX this is seriously bad - should be an emergency return; @@ -168,11 +171,24 @@ void KalmanNav::update() // this clears update flag updateSubscriptions(); - // abort update if no new data - if (!(sensorsUpdate || gpsUpdate)) return; + // initialize attitude when sensors online + if (!_attitudeInitialized && sensorsUpdate && + _sensors.accelerometer_counter > 10 && + _sensors.gyro_counter > 10 && + _sensors.magnetometer_counter > 10) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; - // if received gps for first time, reset position to gps + if (_attitudeInitCounter > 100) { + printf("[kalman_demo] initialized EKF attitude\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + _attitudeInitialized = true; + } + } + + // initialize position when gps received if (!_positionInitialized && + _attitudeInitialized && // wait for attitude first gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 10) { @@ -183,9 +199,7 @@ void KalmanNav::update() setLonDegE7(_gps.lon); setAltE3(_gps.alt); _positionInitialized = true; - printf("[kalman_demo] initializing EKF state with GPS\n"); - printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", - double(phi), double(theta), double(psi)); + printf("[kalman_demo] initialized EKF state with GPS\n"); printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), lat, lon, alt); @@ -194,7 +208,7 @@ void KalmanNav::update() // prediciton step // using sensors timestamp so we can account for packet lag float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dtFast)); + //printf("dt: %15.10f\n", double(dt)); _predictTimeStamp = _sensors.timestamp; // don't predict if time greater than a second @@ -209,12 +223,15 @@ void KalmanNav::update() if (dt > 0.01f) _miss++; // gps correction step - if (gpsUpdate) { + if (_positionInitialized && gpsUpdate) { correctPos(); } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + ) { _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -278,34 +295,9 @@ void KalmanNav::updatePublications() SuperBlock::updatePublications(); } -void KalmanNav::predictState(float dt) +int KalmanNav::predictState(float dt) { using namespace math; - Vector3 w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); - } - - // C_nb update - C_nb = Dcm(q); - - // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); - - // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); // trig float sinL = sinf(lat); @@ -318,30 +310,63 @@ void KalmanNav::predictState(float dt) else cosLSing = -0.01; } - // position update - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; + // attitude prediction + if (_attitudeInitialized) { + Vector3 w(_sensors.gyro_rad_s); - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); + // attitude + q = q + q.derivative(w) * dt; + + // renormalize quaternion if needed + if (fabsf(q.norm() - 1.0f) > 1e-4f) { + q = q.unit(); + } + + // C_nb update + C_nb = Dcm(q); + + // euler update + EulerAngles euler(C_nb); + phi = euler.getPhi(); + theta = euler.getTheta(); + psi = euler.getPsi(); + + // specific acceleration in nav frame + Vector3 accelB(_sensors.accelerometer_m_s2); + Vector3 accelN = C_nb * accelB; + fN = accelN(0); + fE = accelN(1); + fD = accelN(2); + } + + // position prediction + if (_positionInitialized) { + // neglects angular deflections in local gravity + // see Titerton pg. 70 + float R = R0 + float(alt); + float LDot = vN / R; + float lDot = vE / (cosLSing * R); + float rotRate = 2 * omega + lDot; + float vNDot = fN - vE * rotRate * sinL + + vD * LDot; + float vDDot = fD - vE * rotRate * cosL - + vN * LDot + _g.get(); + float vEDot = fE + vN * rotRate * sinL + + vDDot * rotRate * cosL; + + // rectangular integration + vN += vNDot * dt; + vE += vEDot * dt; + vD += vDDot * dt; + lat += double(LDot * dt); + lon += double(lDot * dt); + alt += double(-vD * dt); + } + + return ret_ok; } -void KalmanNav::predictStateCovariance(float dt) +int KalmanNav::predictStateCovariance(float dt) { using namespace math; @@ -434,18 +459,14 @@ void KalmanNav::predictStateCovariance(float dt) // for discrte time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + + return ret_ok; } -void KalmanNav::correctAtt() +int KalmanNav::correctAtt() { using namespace math; - // check for valid data, return if not ready - if (_sensors.accelerometer_counter < 10 || - _sensors.gyro_counter < 10 || - _sensors.magnetometer_counter < 10) - return; - // trig float cosPhi = cosf(phi); float cosTheta = cosf(theta); @@ -489,12 +510,8 @@ void KalmanNav::correctAtt() //printf("correcting attitude with accel\n"); } - // account for banked turn - // this would only work for fixed wing, so try to avoid - //Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi); - // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get()) /*+ zCentrip*/).unit(); + Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); // combined measurement Vector zAtt(6); @@ -561,7 +578,7 @@ void KalmanNav::correctAtt() printf("[kalman_demo] numerical failure in att correction\n"); // reset P matrix to P0 P = P0; - return; + return ret_error; } } @@ -595,14 +612,14 @@ void KalmanNav::correctAtt() // update quaternions from euler // angle correction q = Quaternion(EulerAngles(phi, theta, psi)); + + return ret_ok; } -void KalmanNav::correctPos() +int KalmanNav::correctPos() { using namespace math; - if (!_positionInitialized) return; - // residual Vector y(5); y(0) = _gps.vel_n - vN; @@ -633,7 +650,7 @@ void KalmanNav::correctPos() setAltE3(_gps.alt); // reset P matrix to P0 P = P0; - return; + return ret_error; } } @@ -661,6 +678,8 @@ void KalmanNav::correctPos() double(y(3) / sqrtf(RPos(3, 3))), double(y(4) / sqrtf(RPos(4, 4)))); } + + return ret_ok; } void KalmanNav::updateParams() diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index da1a7ee108..7709ac697c 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -93,23 +93,23 @@ public: * State prediction * Continuous, non-linear */ - void predictState(float dt); + int predictState(float dt); /** * State covariance prediction * Continuous, linear */ - void predictStateCovariance(float dt); + int predictStateCovariance(float dt); /** * Attitude correction */ - void correctAtt(); + int correctAtt(); /** * Position correction */ - void correctPos(); + int correctPos(); /** * Overloaded update parameters @@ -166,7 +166,9 @@ protected: control::BlockParam _faultPos; /**< fault detection threshold for position */ control::BlockParam _faultAtt; /**< fault detection threshold for attitude */ // status - bool _positionInitialized; /**< status, if position has been init. */ + bool _attitudeInitialized; + bool _positionInitialized; + uint16_t _attitudeInitCounter; // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index bb18d5b928..77225b4c75 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -8,7 +8,7 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); From 34d70bea4bb2a1719e4d081cf59081b84037d423 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 16 Jan 2013 13:55:49 -0500 Subject: [PATCH 28/47] Control tuning. --- apps/examples/control_demo/params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 3eed83b5cb..6525b70f56 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -14,9 +14,9 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass // stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator -PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder // psi -> phi -> p PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll From c2c0baf843aa376681fcf7bf9dfcac480177b7fb Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 17 Jan 2013 12:16:21 -0500 Subject: [PATCH 29/47] Increased process noise. --- apps/examples/kalman_demo/params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 77225b4c75..3bfe012612 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,8 +1,8 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); From 13bb814f2015154cb5cadb814e37db1b6b2e634b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 17 Jan 2013 12:18:20 -0500 Subject: [PATCH 30/47] Prevented attitude correction from changing velocity when pos not init. --- apps/examples/kalman_demo/KalmanNav.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 15254f7c79..40166f3a14 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -590,9 +590,11 @@ int KalmanNav::correctAtt() psi += xCorrect(PSI); // attitude also affects nav velocities - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter From dc5ddb937039fd3481009caeb2a472955986e2e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jan 2013 08:41:11 +0100 Subject: [PATCH 31/47] Defaulting to full auto in auto mode --- apps/commander/commander.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f3568ee8d0..33f1832272 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1845,15 +1845,16 @@ int commander_thread_main(int argc, char *argv[]) update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - /* check auto mode switch for correct mode */ - if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - /* enable guided mode */ - update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } + // } } else { /* center stick position, set SAS for all vehicle types */ From c5ecf88bfba7e359717df1977c920d2d29a90b3f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Dec 2012 19:30:43 -0800 Subject: [PATCH 32/47] Added ubx configuration CFG-NAV5 to airborne with less than 2g acceleration (compiling, not tested) --- apps/gps/ubx.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index e15ed4e00a..01d098871d 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -76,6 +76,11 @@ uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; +// Set dynamic model to 7: Airborne with <2g Acceleration +uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x00, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + + + void ubx_decode_init(void) { ubx_state->ck_a = 0; @@ -587,7 +592,10 @@ int configure_gps_ubx(int *fd) //TODO: write this in a loop once it is tested //UBX_CFG_PRT_PART: write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd); + usleep(100000); + //CFG_NAV5 + write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5, sizeof(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5) / sizeof(uint8_t), *fd); usleep(100000); //NAV_POSLLH: @@ -606,7 +614,6 @@ int configure_gps_ubx(int *fd) write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd); usleep(100000); - //NAV_SVINFO: write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd); usleep(100000); @@ -615,7 +622,6 @@ int configure_gps_ubx(int *fd) write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd); usleep(100000); - //RXM_SVSI: write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd); usleep(100000); From 80eb66c7a3f70ccaa46b3d955808a10a222241df Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Dec 2012 19:46:03 -0800 Subject: [PATCH 33/47] The config message was wrong, corrected (not tested) --- apps/gps/ubx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 01d098871d..a17260329f 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -77,7 +77,7 @@ uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x0 uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; // Set dynamic model to 7: Airborne with <2g Acceleration -uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x00, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; From dce9b2d0459b8dea5aa9e072a6914f0768f1867f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Dec 2012 20:26:29 -0800 Subject: [PATCH 34/47] The CFG-NAV5 dynamic model is now checked as well --- apps/gps/ubx.c | 62 ++++++++++++++++++++++++++++++++++++++++++++++++-- apps/gps/ubx.h | 35 ++++++++++++++++++++++++++-- 2 files changed, 93 insertions(+), 4 deletions(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index a17260329f..85cdcafbf7 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -54,6 +54,9 @@ #define UBX_BUFFER_SIZE 1000 +// Set dynamic model to 7: Airborne with <2g Acceleration +#define DYN_MODEL_NO 0x07 + extern bool gps_mode_try_all; extern bool gps_mode_success; extern bool terminate_gps_thread; @@ -65,6 +68,7 @@ pthread_mutex_t *ubx_mutex; gps_bin_ubx_state_t *ubx_state; static struct vehicle_gps_position_s *ubx_gps; +bool gps_nav5_conf_success = false; //Definitions for ubx, last two bytes are checksum which is calculated below uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00}; @@ -76,8 +80,8 @@ uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -// Set dynamic model to 7: Airborne with <2g Acceleration -uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, DYN_MODEL_NO, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL[] = {0xB5, 0x62, 0x06, 0x00, 0x00}; @@ -137,6 +141,10 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) ubx_state->message_class = RXM; break; + case UBX_CLASS_CFG: + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = CFG; + break; default: //unknown class: reset state machine ubx_decode_init(); break; @@ -198,7 +206,19 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) ubx_decode_init(); break; } + break; + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } break; default: //should not happen @@ -546,6 +566,36 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) break; } + case CFG_NAV5: { + printf("GOT CFG_NAV5 MESSAGE\n"); + type_gps_bin_cfg_nav5_packet_t *packet = (type_gps_bin_cfg_nav5_packet_t *) gps_rx_buffer; + + //Check if checksum is valid + if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { + + // check if dynamic model number is correct + if (packet->dynModel == DYN_MODEL_NO) { + printf("[gps] ubx dynamic model set successful\n"); + gps_nav5_conf_success = true; + } + else { + printf("[gps] ubx dynamic model set failed\n"); + gps_nav5_conf_success = false; + } + ret = 1; + + } else { + if (gps_verbose) printf("[gps] CFG_NAV5: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } default: //something went wrong ubx_decode_init(); @@ -626,6 +676,10 @@ int configure_gps_ubx(int *fd) write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd); usleep(100000); + //send CFG_NAV5_POLL to check whether previous CFG_NAV5 has been successful + write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL, sizeof(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL) / sizeof(uint8_t) , *fd); + usleep(100000); + return 0; } @@ -757,6 +811,10 @@ void *ubx_watchdog_loop(void *args) } } + // check if CFG-NAV5 is correct + if (!gps_nav5_conf_success) + all_okay = false; + pthread_mutex_unlock(ubx_mutex); if (!all_okay) { diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index f4f01df9a7..c3f2031845 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -72,6 +72,8 @@ #define UBX_MESSAGE_RXM_SVSI 0x20 #define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 + // ************ @@ -234,6 +236,33 @@ typedef struct { typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t; +typedef struct { + uint8_t clsID; + uint8_t msgId; + + 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; + +typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t; + // END the structures of the binary packets // ************ @@ -242,7 +271,8 @@ enum UBX_MESSAGE_CLASSES { CLASS_UNKNOWN = 0, NAV = 1, RXM = 2, - ACK = 3 + ACK = 3, + CFG = 4 }; enum UBX_MESSAGE_IDS { @@ -254,7 +284,8 @@ enum UBX_MESSAGE_IDS { NAV_DOP = 4, NAV_SVINFO = 5, NAV_VELNED = 6, - RXM_SVSI = 7 + RXM_SVSI = 7, + CFG_NAV5 = 8 }; enum UBX_DECODE_STATES { From ebaa38ad1b5c1d625467fc867480e1969edd60aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 16 Jan 2013 19:54:05 -0800 Subject: [PATCH 35/47] ubx with 38400 working, all messages seem to arrive, configuration procedure is still funny (work in progress) --- apps/gps/gps.c | 2 +- apps/gps/ubx.c | 323 ++++++++++++++++++++++++++++++++++++------------- apps/gps/ubx.h | 79 +++++++++++- 3 files changed, 317 insertions(+), 87 deletions(-) diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 00f6ee9f89..cb9a77c511 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -189,7 +189,7 @@ int gps_thread_main(int argc, char *argv[]) { gps_mode_success = true; terminate_gps_thread = false; bool retry = false; - gps_verbose = false; + gps_verbose = true; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 85cdcafbf7..33f44287b5 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -66,12 +66,16 @@ extern int current_gps_speed; pthread_mutex_t *ubx_mutex; gps_bin_ubx_state_t *ubx_state; +enum UBX_CONFIG_STATE ubx_config_state; static struct vehicle_gps_position_s *ubx_gps; bool gps_nav5_conf_success = false; +unsigned int got_ack = 0; +unsigned int got_nak = 0; + //Definitions for ubx, last two bytes are checksum which is calculated below -uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00}; +uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; @@ -80,8 +84,8 @@ uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, DYN_MODEL_NO, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL[] = {0xB5, 0x62, 0x06, 0x00, 0x00}; +//uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, DYN_MODEL_NO, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +//uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL[] = {0xB5, 0x62, 0x06, 0x00, 0x00}; @@ -109,15 +113,17 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) int ubx_parse(uint8_t b, char *gps_rx_buffer) { -// printf("b=%x\n",b); + //printf("b=%x\n",b); if (ubx_state->decode_state == UBX_DECODE_UNINIT) { - if (b == 0xb5) { + if (b == UBX_SYNC_1) { +// printf("got sync1"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; } } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { - if (b == 0x62) { + if (b == UBX_SYNC_2) { +// printf("got sync2"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; } else { @@ -131,12 +137,18 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //check for known class switch (b) { - case UBX_CLASS_NAV: //NAV + case UBX_CLASS_ACK: + printf("class ack"); + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = ACK; + break; + + case UBX_CLASS_NAV: ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = NAV; break; - case UBX_CLASS_RXM: //RXM + case UBX_CLASS_RXM: ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = RXM; break; @@ -221,6 +233,21 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } break; + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } + break; default: //should not happen ubx_decode_init(); break; @@ -254,7 +281,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //convert to correct struct switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); + printf("GOT NAV_POSLLH MESSAGE\n"); gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -287,7 +314,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); + printf("GOT NAV_SOL MESSAGE\n"); gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -319,7 +346,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_DOP: { -// printf("GOT NAV_DOP MESSAGE\n"); + printf("GOT NAV_DOP MESSAGE\n"); gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -351,7 +378,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); + printf("GOT NAV_TIMEUTC MESSAGE\n"); gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -397,7 +424,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); + printf("GOT NAV_SVINFO MESSAGE\n"); //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; @@ -498,7 +525,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); + printf("GOT NAV_VELNED MESSAGE\n"); gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -534,7 +561,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); + printf("GOT RXM_SVSI MESSAGE\n"); const int length_part1 = 7; char gps_rx_buffer_part1[length_part1]; memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); @@ -566,26 +593,96 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) break; } - case CFG_NAV5: { - printf("GOT CFG_NAV5 MESSAGE\n"); - type_gps_bin_cfg_nav5_packet_t *packet = (type_gps_bin_cfg_nav5_packet_t *) gps_rx_buffer; +// case CFG_NAV5: { +// printf("GOT CFG_NAV5 MESSAGE\n"); +// type_gps_bin_cfg_nav5_packet_t *packet = (type_gps_bin_cfg_nav5_packet_t *) gps_rx_buffer; +// +// //Check if checksum is valid +// if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { +// +// // check if dynamic model number is correct +// if (packet->dynModel == DYN_MODEL_NO) { +// printf("[gps] ubx dynamic model set successful\n"); +// gps_nav5_conf_success = true; +// } +// else { +// printf("[gps] ubx dynamic model set failed\n"); +// gps_nav5_conf_success = false; +// } +// ret = 1; +// +// } else { +// if (gps_verbose) printf("[gps] CFG_NAV5: checksum invalid\n"); +// +// ret = 0; +// } +// +// // Reset state machine to decode next packet +// ubx_decode_init(); +// return ret; +// +// break; +// } + + case ACK_ACK: { + printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer; //Check if checksum is valid - if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - // check if dynamic model number is correct - if (packet->dynModel == DYN_MODEL_NO) { - printf("[gps] ubx dynamic model set successful\n"); - gps_nav5_conf_success = true; - } - else { - printf("[gps] ubx dynamic model set failed\n"); - gps_nav5_conf_success = false; + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: + printf("clsID: %d, msgID: %d\n", packet->clsID, packet->msgID); + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + ubx_config_state++; + break; + case UBX_CONFIG_STATE_NAV5: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) + ubx_config_state++; + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + case UBX_CONFIG_STATE_MSG_NAV_DOP: + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + case UBX_CONFIG_STATE_MSG_NAV_SOL: + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + ubx_config_state++; + break; + default: + break; } + + ret = 1; } else { - if (gps_verbose) printf("[gps] CFG_NAV5: checksum invalid\n"); + if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case ACK_NAK: { + printf("GOT ACK_NAK\n"); + gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer; + + //Check if checksum is valid + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + printf("[gps] the ubx gps returned: not acknowledged\n"); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n"); ret = 0; } @@ -637,50 +734,93 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length) int configure_gps_ubx(int *fd) { - //fflush(((FILE *)fd)); + // only needed once like this + const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_PRT, + .length = UBX_CFG_PRT_LENGTH, + .portID = UBX_CFG_PRT_PAYLOAD_PORTID, + .mode = UBX_CFG_PRT_PAYLOAD_MODE, + .baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE, + .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK, + .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK, + .ck_a = 0, + .ck_b = 0 + }; - //TODO: write this in a loop once it is tested - //UBX_CFG_PRT_PART: - write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd); - usleep(100000); + // only needed once like this + const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_NAV5, + .length = UBX_CFG_NAV5_LENGTH, + .mask = UBX_CFG_NAV5_PAYLOAD_MASK, + .dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL, + .fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE, + .ck_a = 0, + .ck_b = 0 + }; - //CFG_NAV5 - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5, sizeof(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5) / sizeof(uint8_t), *fd); - usleep(100000); + // this message is reusable for different configuration commands, so not const + type_gps_bin_cfg_msg_packet cfg_msg_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_MSG, + .length = UBX_CFG_MSG_LENGTH, + .rate = UBX_CFG_MSG_PAYLOAD_RATE + }; - //NAV_POSLLH: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd); - usleep(100000); + if (gps_verbose) printf("Config state: %d\n", ubx_config_state); - //NAV_TIMEUTC: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd); - usleep(100000); + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: + write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); + break; + case UBX_CONFIG_STATE_NAV5: + write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_CONFIGURED: + if (gps_verbose) printf("[gps] ubx configuration finished\n"); + return OK; + break; + default: + break; + } - //NAV_DOP: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_SOL: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_SVINFO: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_VELNED: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd); - usleep(100000); - - //RXM_SVSI: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd); - usleep(100000); - - //send CFG_NAV5_POLL to check whether previous CFG_NAV5 has been successful - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL, sizeof(UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL) / sizeof(uint8_t) , *fd); - usleep(100000); - - return 0; + return OK; } @@ -748,7 +888,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) return ret; } -int write_config_message_ubx(uint8_t *message, size_t length, int fd) +int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd) { //calculate and write checksum to the end uint8_t ck_a = 0; @@ -756,19 +896,38 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd) unsigned int i; - for (i = 2; i < length; i++) { + uint8_t buffer[2]; + ssize_t result_write = 0; + + for (i = 0; i < length-2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } -// printf("[%x,%x]\n", ck_a, ck_b); +// printf("chk: [%x,%x]\n", ck_a, ck_b); - unsigned int result_write = write(fd, message, length); - result_write += write(fd, &ck_a, 1); - result_write += write(fd, &ck_b, 1); - fsync(fd); + buffer[0] = UBX_SYNC_1; + buffer[1] = UBX_SYNC_2; - return (result_write != length + 2); //return 0 as success + result_write = write(*fd, buffer, sizeof(buffer)); + result_write += write(*fd, message, length-2); + +// for(i=0; iprint_errors = false; @@ -933,8 +1094,8 @@ void *ubx_loop(void *args) } } - if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); - close(gps_pub); +// if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); +// close(gps_pub); return NULL; } diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index c3f2031845..eb19d17f24 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -58,6 +58,9 @@ #define APPNAME "gps: ubx" +#define UBX_SYNC_1 0xB5 +#define UBX_SYNC_2 0x62 + //UBX Protocoll definitions (this is the subset of the messages that are parsed) #define UBX_CLASS_NAV 0x01 #define UBX_CLASS_RXM 0x02 @@ -72,8 +75,24 @@ #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_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#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_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_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */ // ************ @@ -218,7 +237,7 @@ typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t; typedef struct { uint8_t clsID; - uint8_t msgId; + uint8_t msgID; uint8_t ck_a; uint8_t ck_b; @@ -228,7 +247,7 @@ typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t; typedef struct { uint8_t clsID; - uint8_t msgId; + uint8_t msgID; uint8_t ck_a; uint8_t ck_b; @@ -238,8 +257,28 @@ typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t; typedef struct { uint8_t clsID; - uint8_t msgId; + 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; + +typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; uint16_t mask; uint8_t dynModel; uint8_t fixMode; @@ -263,10 +302,38 @@ typedef struct { typedef type_gps_bin_cfg_nav5_packet 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; + +typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t; + // END the structures of the binary packets // ************ +enum UBX_CONFIG_STATE { + UBX_CONFIG_STATE_NONE = 0, + UBX_CONFIG_STATE_PRT = 1, + UBX_CONFIG_STATE_NAV5 = 2, + UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4, + UBX_CONFIG_STATE_MSG_NAV_DOP = 5, + UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6, + UBX_CONFIG_STATE_MSG_NAV_SOL = 7, + UBX_CONFIG_STATE_MSG_NAV_VELNED = 8, + UBX_CONFIG_STATE_MSG_RXM_SVSI = 9, + UBX_CONFIG_STATE_CONFIGURED = 10 +}; + enum UBX_MESSAGE_CLASSES { CLASS_UNKNOWN = 0, NAV = 1, @@ -285,7 +352,9 @@ enum UBX_MESSAGE_IDS { NAV_SVINFO = 5, NAV_VELNED = 6, RXM_SVSI = 7, - CFG_NAV5 = 8 + CFG_NAV5 = 8, + ACK_ACK = 9, + ACK_NAK = 10 }; enum UBX_DECODE_STATES { @@ -335,7 +404,7 @@ int configure_gps_ubx(int *fd); int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size); -int write_config_message_ubx(uint8_t *message, size_t length, int fd); +int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd); void calculate_ubx_checksum(uint8_t *message, uint8_t length); From 9ca472bbc7727294c039dbced0dc7bea6925789e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 17 Jan 2013 16:53:32 -0800 Subject: [PATCH 36/47] Ubx configuration working again, gps app is still complicated and big but should be wrking better now --- apps/gps/gps.c | 7 +- apps/gps/ubx.c | 256 +++++++++++++++++-------------------------------- apps/gps/ubx.h | 7 +- 3 files changed, 93 insertions(+), 177 deletions(-) diff --git a/apps/gps/gps.c b/apps/gps/gps.c index cb9a77c511..8a95120547 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -107,8 +107,8 @@ enum GPS_MODES { #define AUTO_DETECTION_COUNT 8 -const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400}; -const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea +const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600}; +const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea /**************************************************************************** * Private functions @@ -189,7 +189,7 @@ int gps_thread_main(int argc, char *argv[]) { gps_mode_success = true; terminate_gps_thread = false; bool retry = false; - gps_verbose = true; + gps_verbose = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -368,7 +368,6 @@ int gps_thread_main(int argc, char *argv[]) { args.thread_should_exit_ptr = &thread_should_exit; pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args); - sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver pthread_attr_t ubx_wd_attr; pthread_attr_init(&ubx_wd_attr); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 33f44287b5..50bf579a08 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -52,10 +52,7 @@ #define UBX_HEALTH_FAIL_COUNTER_LIMIT 3 #define UBX_HEALTH_PROBE_COUNTER_LIMIT 4 -#define UBX_BUFFER_SIZE 1000 - -// Set dynamic model to 7: Airborne with <2g Acceleration -#define DYN_MODEL_NO 0x07 +#define UBX_BUFFER_SIZE 500 extern bool gps_mode_try_all; extern bool gps_mode_success; @@ -69,24 +66,6 @@ gps_bin_ubx_state_t *ubx_state; enum UBX_CONFIG_STATE ubx_config_state; static struct vehicle_gps_position_s *ubx_gps; -bool gps_nav5_conf_success = false; - -unsigned int got_ack = 0; -unsigned int got_nak = 0; - -//Definitions for ubx, last two bytes are checksum which is calculated below -uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x96, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; - -//uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5[] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x01, DYN_MODEL_NO, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; -//uint8_t UBX_CONFIG_MESSAGE_MSG_CFG_NAV5_POLL[] = {0xB5, 0x62, 0x06, 0x00, 0x00}; - void ubx_decode_init(void) @@ -117,13 +96,11 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) if (ubx_state->decode_state == UBX_DECODE_UNINIT) { if (b == UBX_SYNC_1) { -// printf("got sync1"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; } } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { if (b == UBX_SYNC_2) { -// printf("got sync2"); ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; } else { @@ -138,7 +115,6 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //check for known class switch (b) { case UBX_CLASS_ACK: - printf("class ack"); ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = ACK; break; @@ -281,7 +257,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //convert to correct struct switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class case NAV_POSLLH: { - printf("GOT NAV_POSLLH MESSAGE\n"); +// printf("GOT NAV_POSLLH MESSAGE\n"); gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -314,7 +290,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SOL: { - printf("GOT NAV_SOL MESSAGE\n"); +// printf("GOT NAV_SOL MESSAGE\n"); gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -346,7 +322,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_DOP: { - printf("GOT NAV_DOP MESSAGE\n"); +// printf("GOT NAV_DOP MESSAGE\n"); gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -378,7 +354,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_TIMEUTC: { - printf("GOT NAV_TIMEUTC MESSAGE\n"); +// printf("GOT NAV_TIMEUTC MESSAGE\n"); gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -424,7 +400,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_SVINFO: { - printf("GOT NAV_SVINFO MESSAGE\n"); +// printf("GOT NAV_SVINFO MESSAGE\n"); //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; @@ -525,7 +501,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case NAV_VELNED: { - printf("GOT NAV_VELNED MESSAGE\n"); +// printf("GOT NAV_VELNED MESSAGE\n"); gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer; //Check if checksum is valid and the store the gps information @@ -561,7 +537,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case RXM_SVSI: { - printf("GOT RXM_SVSI MESSAGE\n"); +// printf("GOT RXM_SVSI MESSAGE\n"); const int length_part1 = 7; char gps_rx_buffer_part1[length_part1]; memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); @@ -593,39 +569,9 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) break; } -// case CFG_NAV5: { -// printf("GOT CFG_NAV5 MESSAGE\n"); -// type_gps_bin_cfg_nav5_packet_t *packet = (type_gps_bin_cfg_nav5_packet_t *) gps_rx_buffer; -// -// //Check if checksum is valid -// if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { -// -// // check if dynamic model number is correct -// if (packet->dynModel == DYN_MODEL_NO) { -// printf("[gps] ubx dynamic model set successful\n"); -// gps_nav5_conf_success = true; -// } -// else { -// printf("[gps] ubx dynamic model set failed\n"); -// gps_nav5_conf_success = false; -// } -// ret = 1; -// -// } else { -// if (gps_verbose) printf("[gps] CFG_NAV5: checksum invalid\n"); -// -// ret = 0; -// } -// -// // Reset state machine to decode next packet -// ubx_decode_init(); -// return ret; -// -// break; -// } case ACK_ACK: { - printf("GOT ACK_ACK\n"); +// printf("GOT ACK_ACK\n"); gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer; //Check if checksum is valid @@ -633,7 +579,6 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) switch (ubx_config_state) { case UBX_CONFIG_STATE_PRT: - printf("clsID: %d, msgID: %d\n", packet->clsID, packet->msgID); if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) ubx_config_state++; break; @@ -672,13 +617,13 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) } case ACK_NAK: { - printf("GOT ACK_NAK\n"); +// printf("GOT ACK_NAK\n"); gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer; //Check if checksum is valid if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { - printf("[gps] the ubx gps returned: not acknowledged\n"); + if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n"); ret = 1; } else { @@ -726,10 +671,6 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length) message[length - 2] = ck_a; message[length - 1] = ck_b; - -// printf("[%x,%x]", ck_a, ck_b); - -// printf("[%x,%x]\n", message[length-2], message[length-1]); } int configure_gps_ubx(int *fd) @@ -741,7 +682,7 @@ int configure_gps_ubx(int *fd) .length = UBX_CFG_PRT_LENGTH, .portID = UBX_CFG_PRT_PAYLOAD_PORTID, .mode = UBX_CFG_PRT_PAYLOAD_MODE, - .baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE, + .baudRate = current_gps_speed, .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK, .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK, .ck_a = 0, @@ -768,59 +709,67 @@ int configure_gps_ubx(int *fd) .rate = UBX_CFG_MSG_PAYLOAD_RATE }; - if (gps_verbose) printf("Config state: %d\n", ubx_config_state); + uint64_t time_before_config = hrt_absolute_time(); - switch (ubx_config_state) { - case UBX_CONFIG_STATE_PRT: - write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); - break; - case UBX_CONFIG_STATE_NAV5: - write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_MSG_RXM_SVSI: - cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; - write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); - break; - case UBX_CONFIG_STATE_CONFIGURED: - if (gps_verbose) printf("[gps] ubx configuration finished\n"); - return OK; - break; - default: - break; + while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) { + +// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state); + + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: +// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate); + + write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); + break; + case UBX_CONFIG_STATE_NAV5: + write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_CONFIGURED: + if (gps_verbose) printf("[gps] ubx configuration finished\n"); + return OK; + break; + default: + break; + } + usleep(10000); } - - return OK; + if (gps_verbose) printf("[gps] ubx configuration timeout\n"); + return ERROR; } @@ -837,22 +786,17 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) fds.events = POLLIN; // UBX GPS mode - // This blocks the task until there is something on the buffer while (1) { //check if the thread should terminate if (terminate_gps_thread == true) { -// printf("terminate_gps_thread=%u ", terminate_gps_thread); -// printf("exiting mtk thread\n"); -// fflush(stdout); ret = 1; break; } - if (poll(&fds, 1, 1000) > 0) { if (read(*fd, &c, 1) > 0) { - // printf("Read %x\n",c); +// printf("Read %x\n",c); if (rx_count >= buffer_size) { // The buffer is already full and we haven't found a valid ubx sentence. // Flush the buffer and note the overflow event. @@ -871,7 +815,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) int msg_read = ubx_parse(c, gps_rx_buffer); if (msg_read > 0) { - // printf("Found sequence\n"); + //printf("Found sequence\n"); break; } @@ -890,7 +834,6 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd) { - //calculate and write checksum to the end uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -899,36 +842,31 @@ int write_config_message_ubx(const uint8_t *message, const size_t length, const uint8_t buffer[2]; ssize_t result_write = 0; + //calculate and write checksum to the end for (i = 0; i < length-2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } -// printf("chk: [%x,%x]\n", ck_a, ck_b); - + // write sync bytes first buffer[0] = UBX_SYNC_1; buffer[1] = UBX_SYNC_2; - result_write = write(*fd, buffer, sizeof(buffer)); - result_write += write(*fd, message, length-2); - -// for(i=0; ifd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; + /* first try to configure the GPS anyway */ + configure_gps_ubx(fd); + /* GPS watchdog error message skip counter */ bool ubx_healthy = false; @@ -970,10 +911,6 @@ void *ubx_watchdog_loop(void *args) } } -// // check if CFG-NAV5 is correct -// if (!gps_nav5_conf_success) -// all_okay = false; - pthread_mutex_unlock(ubx_mutex); if (!all_okay) { @@ -1026,7 +963,6 @@ void *ubx_watchdog_loop(void *args) if(gps_verbose) printf("[gps] ubx loop is going to terminate\n"); close(mavlink_fd); - return NULL; } @@ -1050,7 +986,6 @@ void *ubx_loop(void *args) //ubx state ubx_state = malloc(sizeof(gps_bin_ubx_state_t)); - //printf("gps: ubx_state created\n"); ubx_decode_init(); ubx_state->print_errors = false; @@ -1058,23 +993,6 @@ void *ubx_loop(void *args) /* set parameters for ubx */ - if (configure_gps_ubx(fd) != 0) { - printf("[gps] Configuration of gps module to ubx failed\n"); - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present); - - } else { - if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n"); - - // XXX Shouldn't the system status only change if the module is known to work ok? - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present_enabled); - } - struct vehicle_gps_position_s ubx_gps_d = {.counter = 0}; ubx_gps = &ubx_gps_d; @@ -1094,8 +1012,8 @@ void *ubx_loop(void *args) } } -// if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); -// close(gps_pub); + if(gps_verbose) printf("[gps] ubx read is going to terminate\n"); + close(gps_pub); return NULL; } diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index eb19d17f24..e700fe3880 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -49,12 +49,11 @@ //internal definitions (not depending on the ubx protocol -#define CONFIGURE_UBX_FINISHED 0 -#define CONFIGURE_UBX_MESSAGE_ACKNOWLEDGED 1 -#define CONFIGURE_UBX_MESSAGE_NOT_ACKNOWLEDGED 2 #define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */ -#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 500000 /**< Check for current state every 0.4 seconds */ +#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 1000000 /**< Check for current state every second */ + +#define UBX_CONFIG_TIMEOUT 500000 #define APPNAME "gps: ubx" From 4b2d1690d33c2ae9248abd9fb025e8a1a30fbe84 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 18 Jan 2013 10:21:20 -0500 Subject: [PATCH 37/47] Set kalman_demo to only publish when it has valid info. --- apps/examples/kalman_demo/KalmanNav.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 40166f3a14..a07280515e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -239,7 +239,10 @@ void KalmanNav::update() // publication if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz _pubTimeStamp = newTimeStamp; - updatePublications(); + + if (_positionInitialized) _pos.update(); + + if (_attitudeInitialized) _att.update(); } // output @@ -589,6 +592,7 @@ int KalmanNav::correctAtt() } psi += xCorrect(PSI); + // attitude also affects nav velocities if (_positionInitialized) { vN += xCorrect(VN); From 3128529c3b67e3352de6a483292b74c22dafd377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 01:31:05 +0100 Subject: [PATCH 38/47] Added logging improvements for microSD --- apps/sdlog/sdlog.c | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f8668a2e38..d79343cd91 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -319,8 +319,25 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf) int sdlog_thread_main(int argc, char *argv[]) { + /* log every 2nd value (skip one) */ + int skip_value = 1; + + if (argc > 1) { + if (!strcmp(argv[1], "-s") && argc > 2) { + int s = atoi(argv[2]); + + if (s > 0 && s < 250) { + skip_value = s; + } else { + warnx("Ignoring skip value of %d, out of range (1..250)\n", s); + } + } + } + warnx("starting\n"); + warnx("skipping %d sensor packets between logged packets.\n", skip_value); + if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } @@ -565,8 +582,6 @@ int sdlog_thread_main(int argc, char *argv[]) gyro_fd.fd = subs.sensor_sub; gyro_fd.events = POLLIN; - /* log every 2nd value (skip one) */ - int skip_value = 0; /* track skipping */ int skip_count = 0; @@ -717,7 +732,10 @@ int sdlog_thread_main(int argc, char *argv[]) pthread_mutex_lock(&sysvector_mutex); sdlog_logbuffer_write(&lb, &sysvect); /* signal the other thread new data, but not yet unlock */ - pthread_cond_signal(&sysvector_cond); + if (lb.count > lb.size / 3) { + /* only request write if several packets can be written at once */ + pthread_cond_signal(&sysvector_cond); + } /* unlock, now the writer thread may run */ pthread_mutex_unlock(&sysvector_mutex); } From d463c94ea1e96ab1052d10dbd7eee9521f0d4298 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 12:45:23 +0100 Subject: [PATCH 39/47] Enable / disable logging while running, enabled black box logging (ringbuffer needed), enabled GPS KML logging (does not yet write outputs) --- apps/sdlog/sdlog.c | 376 ++++++++++++++++++++++----------------------- 1 file changed, 183 insertions(+), 193 deletions(-) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index d79343cd91..175392afb2 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -73,6 +73,8 @@ #include +#include + #include "sdlog_ringbuffer.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -83,6 +85,7 @@ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ static const char *mountpoint = "/fs/microsd"; static const char *mfile_in = "/etc/logging/logconv.m"; int sysvector_file = -1; +int mavlink_fd = -1; struct sdlog_logbuffer lb; /* mutex / condition to synchronize threads */ @@ -118,6 +121,8 @@ static int file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); +static void handle_command(struct vehicle_command_s *cmd); + /** * Print the current status. */ @@ -134,7 +139,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog {start|stop|status} [-p ]\n\n"); + errx(1, "usage: sdlog {start|stop|status} [-s ]\n\n"); } // XXX turn this into a C++ class @@ -145,6 +150,9 @@ unsigned sysvector_bytes = 0; unsigned blackbox_file_bytes = 0; uint64_t starttime = 0; +/* logging on or off, default to true */ +bool logging_enabled = true; + /** * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the @@ -318,25 +326,44 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf) int sdlog_thread_main(int argc, char *argv[]) { + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* log every 2nd value (skip one) */ - int skip_value = 1; - - if (argc > 1) { - if (!strcmp(argv[1], "-s") && argc > 2) { - int s = atoi(argv[2]); - - if (s > 0 && s < 250) { - skip_value = s; - } else { - warnx("Ignoring skip value of %d, out of range (1..250)\n", s); - } - } + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); } - warnx("starting\n"); + /* log every n'th value (skip one per default) */ + int skip_value = 1; - warnx("skipping %d sensor packets between logged packets.\n", skip_value); + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + int ch; + + while ((ch = getopt(argc, argv, "sr")) != EOF) { + switch (ch) { + case 's': + { + /* log only every n'th (gyro clocked) value */ + unsigned s = strtoul(optarg, NULL, 10); + + if (s < 1 || s > 250) { + errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); + } else { + skip_value = s; + } + } + break; + + case 'r': + /* log only on request, disable logging per default */ + logging_enabled = false; + break; + + default: + usage("unrecognized flag"); + } + } if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); @@ -347,31 +374,15 @@ int sdlog_thread_main(int argc, char *argv[]) if (create_logfolder(folder_path)) errx(1, "unable to create logging folder, exiting."); - /* create sensorfile */ - int sensorfile = -1; - int actuator_outputs_file = -1; - int actuator_controls_file = -1; FILE *gpsfile; FILE *blackbox_file; - // FILE *vehiclefile; - char path_buf[64] = ""; // string to hold the path to the sensorfile + /* string to hold the path to the sensorfile */ + char path_buf[64] = ""; + /* only print logging path, important to find log file later */ warnx("logging to directory %s\n", folder_path); - /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); - - if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - - // /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */ - // sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0"); - // if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - // errx(1, "opening %s failed.\n", path_buf); - // } - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); @@ -379,13 +390,6 @@ int sdlog_thread_main(int argc, char *argv[]) errx(1, "opening %s failed.\n", path_buf); } - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0"); - - if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); @@ -402,6 +406,7 @@ int sdlog_thread_main(int argc, char *argv[]) errx(1, "opening %s failed.\n", path_buf); } + // XXX for fsync() calls int blackbox_file_no = fileno(blackbox_file); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ @@ -455,12 +460,18 @@ int sdlog_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GPS POSITION --- */ + /* subscribe to ORB for global position */ + subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + fds[fdsc_count].fd = subs.gps_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(subs.sensor_sub, 5); + /* do not rate limit, instead use skip counter (aliasing on rate limit) */ fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -513,13 +524,6 @@ int sdlog_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- VICON POSITION --- */ /* subscribe to ORB for vicon position */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); @@ -577,18 +581,13 @@ int sdlog_thread_main(int argc, char *argv[]) starttime = hrt_absolute_time(); - // XXX clock the log for now with the gyro output rate / 2 - struct pollfd gyro_fd; - gyro_fd.fd = subs.sensor_sub; - gyro_fd.events = POLLIN; - /* track skipping */ int skip_count = 0; while (!thread_should_exit) { // XXX only use gyro for now - int poll_ret = poll(&gyro_fd, 1, 1000); + int poll_ret = poll(fds, 2, 1000); // int poll_ret = poll(fds, fdsc_count, timeout); @@ -599,145 +598,107 @@ int sdlog_thread_main(int argc, char *argv[]) /* XXX this is seriously bad - should be an emergency */ } else { - /* always copy sensors raw data into local buffer, since poll flags won't clear else */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + int ifds; - if (skip_count < skip_value) { - skip_count++; - /* do not log data */ - continue; - } else { - /* log data, reset */ - skip_count = 0; + /* --- VEHICLE COMMAND VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy command into local buffer */ + orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + + /* always log to blackbox, even when logging disabled */ + blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, + buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, + (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); + + handle_command(&buf.cmd); } - // int ifds = 0; + /* --- VEHICLE GPS VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy gps position into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // if (poll_count % 5000 == 0) { - // fsync(sensorfile); - // fsync(actuator_outputs_file); - // fsync(actuator_controls_file); - // fsync(blackbox_file_no); - // } + /* if logging disabled, continue */ + if (logging_enabled) { - - - // /* --- VEHICLE COMMAND VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy command into local buffer */ - // orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - // blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, - // buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, - // (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); - // } - - // /* --- SENSORS RAW VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - // /* write out */ - // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); - // } - - // /* --- ATTITUDE VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - - // /* copy attitude data into local buffer */ - // orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - - - // } - - // /* --- VEHICLE ATTITUDE SETPOINT --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy local position data into local buffer */ - // orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - - // } - - // /* --- ACTUATOR OUTPUTS 0 --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy actuator data into local buffer */ - // orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - // /* write out */ - // // actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs)); - // } - - // /* --- ACTUATOR CONTROL --- */ - // if (fds[ifds++].revents & POLLIN) { - // orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); - // /* write out */ - // actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls)); - // } - - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); - orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); - - struct sdlog_sysvector sysvect = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = { - buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] - }, - .vbat = buf.batt.voltage_v, - .bat_current = buf.batt.current_a, - .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pressure.differential_pressure_mbar, - .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, - .true_airspeed = buf.diff_pressure.true_airspeed_m_s - }; - - /* put into buffer for later IO */ - pthread_mutex_lock(&sysvector_mutex); - sdlog_logbuffer_write(&lb, &sysvect); - /* signal the other thread new data, but not yet unlock */ - if (lb.count > lb.size / 3) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&sysvector_cond); + /* write KML line */ + } } - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&sysvector_mutex); + + /* --- SENSORS RAW VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + + // /* copy sensors raw data into local buffer */ + // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + // /* write out */ + // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); + + /* always copy sensors raw data into local buffer, since poll flags won't clear else */ + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); + orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); + orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); + + /* if skipping is on or logging is disabled, ignore */ + if (skip_count < skip_value || !logging_enabled) { + skip_count++; + /* do not log data */ + continue; + } else { + /* log data, reset */ + skip_count = 0; + } + + struct sdlog_sysvector sysvect = { + .timestamp = buf.raw.timestamp, + .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, + .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, + .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, + .baro = buf.raw.baro_pres_mbar, + .baro_alt = buf.raw.baro_alt_meter, + .baro_temp = buf.raw.baro_temp_celcius, + .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, + .actuators = { + buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], + buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] + }, + .vbat = buf.batt.voltage_v, + .bat_current = buf.batt.current_a, + .bat_discharged = buf.batt.discharged_mah, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, + .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, + .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, + .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, + .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, + .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, + .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, + .diff_pressure = buf.diff_pressure.differential_pressure_mbar, + .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, + .true_airspeed = buf.diff_pressure.true_airspeed_m_s + }; + + /* put into buffer for later IO */ + pthread_mutex_lock(&sysvector_mutex); + sdlog_logbuffer_write(&lb, &sysvect); + /* signal the other thread new data, but not yet unlock */ + if ((unsigned)lb.count > lb.size / 3) { + /* only request write if several packets can be written at once */ + pthread_cond_signal(&sysvector_cond); + } + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&sysvector_mutex); + } + } } @@ -752,9 +713,8 @@ int sdlog_thread_main(int argc, char *argv[]) warnx("exiting.\n"); - close(sensorfile); - close(actuator_outputs_file); - close(actuator_controls_file); + /* finish KML file */ + // XXX fclose(gpsfile); fclose(blackbox_file); @@ -821,4 +781,34 @@ int file_copy(const char *file_old, const char *file_new) return ret; } +void handle_command(struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* request to set different system mode */ + switch (cmd->command) { + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param3)) == 1) { + + /* enable logging */ + mavlink_log_info(mavlink_fd, "[log] file:"); + mavlink_log_info(mavlink_fd, "logdir"); + logging_enabled = true; + } + if (((int)(cmd->param3)) == 0) { + + /* disable logging */ + mavlink_log_info(mavlink_fd, "[log] stopped."); + logging_enabled = false; + } + break; + + default: + /* silently ignore */ + break; + } + +} From f119d9fbda4b942f58b47b3f1af8addd052f6d9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 14:46:26 +0100 Subject: [PATCH 40/47] Added home position concept, uORB struct and MAVLink announcement of home position --- apps/commander/commander.c | 123 ++++++++++++++++--------------- apps/mavlink/orb_listener.c | 16 ++++ apps/mavlink/orb_topics.h | 2 + apps/uORB/objects_common.cpp | 3 + apps/uORB/topics/home_position.h | 77 +++++++++++++++++++ 5 files changed, 160 insertions(+), 61 deletions(-) create mode 100644 apps/uORB/topics/home_position.h diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f3568ee8d0..72727867d5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -72,8 +72,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -1251,6 +1253,7 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; + bool home_position_set = false; /* set parameters */ failsafe_lowlevel_timeout_ms = 0; @@ -1302,6 +1305,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish current state machine */ state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + if (stat_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); @@ -1332,10 +1340,6 @@ int commander_thread_main(int argc, char *argv[]) uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; - float hdop = 65535.0f; - - int gps_quality_good_counter = 0; - /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1356,6 +1360,15 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1660,65 +1673,54 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } + if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { - /* Check if last transition deserved an audio event */ -// #warning This code depends on state that is no longer? maintained -// #if 0 -// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); -// #endif + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* only check gps fix if we are outdoor */ -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// -// hdop = (float)(gps.eph) / 100.0f; -// -// /* check if gps fix is ok */ -// if (gps.fix_type == GPS_FIX_TYPE_3D) { //TODO: is 2d-fix ok? //see http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29 -// -// if (gotfix_counter >= GPS_GOTFIX_COUNTER_REQUIRED) { //TODO: add also a required time? -// update_state_machine_got_position_fix(stat_pub, ¤t_status); -// gotfix_counter = 0; -// } else { -// gotfix_counter++; -// } -// nofix_counter = 0; -// -// if (hdop < 5.0f) { //TODO: this should be a parameter -// if (gps_quality_good_counter > GPS_QUALITY_GOOD_COUNTER_LIMIT) { -// current_status.gps_valid = true;//--> position estimator can use the gps measurements -// } -// -// gps_quality_good_counter++; -// -// -//// if(counter%10 == 0)//for testing only -//// warnx("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only -// -// } else { -// gps_quality_good_counter = 0; -// current_status.gps_valid = false;//--> position estimator can not use the gps measurements -// } -// -// } else { -// gps_quality_good_counter = 0; -// current_status.gps_valid = false;//--> position estimator can not use the gps measurements -// -// if (nofix_counter > GPS_NOFIX_COUNTER_LIMIT) { //TODO: add also a timer limit? -// update_state_machine_no_position_fix(stat_pub, ¤t_status); -// nofix_counter = 0; -// } else { -// nofix_counter++; -// } -// gotfix_counter = 0; -// } -// -// } -// -// -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests - //update_state_machine_got_position_fix(stat_pub, ¤t_status, mavlink_fd); - /* end: check gps */ + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop = gps_position.eph / 100.0f; + float vdop = gps_position.epv / 100.0f; + /* check if gps fix is ok */ + // XXX magic number + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop < 4.0f) + && (vdop < 4.0f) + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp < 2000000) + && !current_status.flag_system_armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph = gps_position.eph; + home.epv = gps_position.epv; + + home.s_variance = gps_position.s_variance; + home.p_variance = gps_position.p_variance; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + tune_confirm(); + } + } /* ignore RC signals if in offboard control mode */ if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { @@ -2041,4 +2043,3 @@ int commander_thread_main(int argc, char *argv[]) return 0; } - diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f5253ea358..8920714ef5 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -114,6 +114,7 @@ static void l_vehicle_attitude_controls(struct listener *l); static void l_debug_key_value(struct listener *l); static void l_optical_flow(struct listener *l); static void l_vehicle_rates_setpoint(struct listener *l); +static void l_home(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -137,6 +138,7 @@ struct listener listeners[] = { {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, + {l_home, &mavlink_subs.home_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -621,6 +623,16 @@ l_optical_flow(struct listener *l) flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } +void +l_home(struct listener *l) +{ + struct home_position_s home; + + orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); +} + static void * uorb_receive_thread(void *arg) { @@ -688,6 +700,10 @@ uorb_receive_start(void) mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + /* --- HOME POSITION --- */ + mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + /* --- SYSTEM STATE --- */ status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 4650c49916..d61cd43dce 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +82,7 @@ struct mavlink_subscriptions { int input_rc_sub; int optical_flow; int rates_setpoint_sub; + int home_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 82893b3e9a..3f3476f62a 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -68,6 +68,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/home_position.h" +ORB_DEFINE(home_position, struct home_position_s); + #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h new file mode 100644 index 0000000000..dec34b6ab4 --- /dev/null +++ b/apps/uORB/topics/home_position.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 home_position.h + * Definition of the GPS home position uORB topic. + * + * @author Lorenz Meier + */ + +#ifndef TOPIC_HOME_POSITION_H_ +#define TOPIC_HOME_POSITION_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GPS home position in WGS84 coordinates. + */ +struct home_position_s +{ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ + + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ + uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ + float s_variance; /**< speed accuracy estimate cm/s */ + float p_variance; /**< position accuracy estimate cm */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(home_position); + +#endif From d6378428253aea5b8d4cd953bcca556cb4641b69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 16:59:56 +0100 Subject: [PATCH 41/47] Fixed a number of smaller issues with log changes, ready to merge --- apps/sdlog/sdlog.c | 39 +++++++++++++++++++++++++++------------ 1 file changed, 27 insertions(+), 12 deletions(-) diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 175392afb2..4fd5fea1b9 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -180,7 +181,7 @@ int sdlog_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (const char **)argv); exit(0); } @@ -332,15 +333,15 @@ int sdlog_thread_main(int argc, char *argv[]) warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); } - /* log every n'th value (skip one per default) */ - int skip_value = 1; + /* log every n'th value (skip three per default) */ + int skip_value = 3; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "sr")) != EOF) { + while ((ch = getopt(argc, argv, "s:r")) != EOF) { switch (ch) { case 's': { @@ -360,8 +361,18 @@ int sdlog_thread_main(int argc, char *argv[]) logging_enabled = false; break; + case '?': + if (optopt == 'c') { + warnx("Option -%c requires an argument.\n", optopt); + } else if (isprint(optopt)) { + warnx("Unknown option `-%c'.\n", optopt); + } else { + warnx("Unknown option character `\\x%x'.\n", optopt); + } + default: usage("unrecognized flag"); + errx(1, "exiting."); } } @@ -454,7 +465,7 @@ int sdlog_thread_main(int argc, char *argv[]) } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for sensors raw */ + /* subscribe to ORB for vehicle command */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; fds[fdsc_count].events = POLLIN; @@ -586,10 +597,8 @@ int sdlog_thread_main(int argc, char *argv[]) while (!thread_should_exit) { - // XXX only use gyro for now - int poll_ret = poll(fds, 2, 1000); - - // int poll_ret = poll(fds, fdsc_count, timeout); + /* only poll for commands, gps and sensor_combined */ + int poll_ret = poll(fds, 3, 1000); /* handle the poll result */ if (poll_ret == 0) { @@ -598,7 +607,7 @@ int sdlog_thread_main(int argc, char *argv[]) /* XXX this is seriously bad - should be an emergency */ } else { - int ifds; + int ifds = 0; /* --- VEHICLE COMMAND VALUE --- */ if (fds[ifds++].revents & POLLIN) { @@ -691,7 +700,7 @@ int sdlog_thread_main(int argc, char *argv[]) pthread_mutex_lock(&sysvector_mutex); sdlog_logbuffer_write(&lb, &sysvect); /* signal the other thread new data, but not yet unlock */ - if ((unsigned)lb.count > lb.size / 3) { + if ((unsigned)lb.count > (lb.size / 2)) { /* only request write if several packets can be written at once */ pthread_cond_signal(&sysvector_cond); } @@ -705,13 +714,19 @@ int sdlog_thread_main(int argc, char *argv[]) print_sdlog_status(); + /* wake up write thread one last time */ + pthread_mutex_lock(&sysvector_mutex); + pthread_cond_signal(&sysvector_cond); + /* unlock, now the writer thread may return */ + pthread_mutex_unlock(&sysvector_mutex); + /* wait for write thread to return */ (void)pthread_join(sysvector_pthread, NULL); pthread_mutex_destroy(&sysvector_mutex); pthread_cond_destroy(&sysvector_cond); - warnx("exiting.\n"); + warnx("exiting.\n\n"); /* finish KML file */ // XXX From c9c64b3f25113eba565d0926b435f2b492a7468e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 17:03:35 +0100 Subject: [PATCH 42/47] Added missing flag, tested --- apps/commander/commander.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 72727867d5..cc382c2fd7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1718,6 +1718,8 @@ int commander_thread_main(int argc, char *argv[]) home_pub = orb_advertise(ORB_ID(home_position), &home); } + /* mark home position as set */ + home_position_set = true; tune_confirm(); } } From 2542722102ce0ab3520dafe9ae695cf06caae675 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 17:11:12 +0100 Subject: [PATCH 43/47] Fixed selective publication update --- apps/examples/kalman_demo/KalmanNav.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index a07280515e..7e89dffb20 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -240,9 +240,7 @@ void KalmanNav::update() if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz _pubTimeStamp = newTimeStamp; - if (_positionInitialized) _pos.update(); - - if (_attitudeInitialized) _att.update(); + updatePublications(); } // output @@ -294,8 +292,13 @@ void KalmanNav::updatePublications() _att.q_valid = true; _att.counter = _navFrames; - // update publications - SuperBlock::updatePublications(); + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) + _pos.update(); + + if (_attitudeInitialized) + _att.update(); } int KalmanNav::predictState(float dt) From 4b9916eded5330b8964edb87b3cc9f4815ac7af0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 19:32:44 +0100 Subject: [PATCH 44/47] Made threshold a bit nicer, still a magic number --- apps/commander/commander.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index cc382c2fd7..103c53dc69 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1678,11 +1678,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ - float hdop = gps_position.eph / 100.0f; - float vdop = gps_position.epv / 100.0f; + float hdop_m = gps_position.eph / 100.0f; + float vdop_m = gps_position.epv / 100.0f; /* check if gps fix is ok */ // XXX magic number + float dop_threshold_m = 2.0f; /* * If horizontal dilution of precision (hdop / eph) @@ -1693,8 +1694,8 @@ int commander_thread_main(int argc, char *argv[]) * the system is currently not armed, set home * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop < 4.0f) - && (vdop < 4.0f) + if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop < dop_threshold_m) + && (vdop_m < dop_threshold_m) && !home_position_set && (hrt_absolute_time() - gps_position.timestamp < 2000000) && !current_status.flag_system_armed) { From 15c85ba2cb051c542a07851a155afbad0ce76b4c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 19 Jan 2013 18:05:33 -0800 Subject: [PATCH 45/47] Strip some debugging --- apps/px4io/mixer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed7d684b6f..c24cb8e52c 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -100,7 +100,6 @@ mixer_tick(void) /* too many frames without FMU input, time to go to failsafe */ system_state.mixer_manual_override = true; system_state.mixer_fmu_available = false; - lib_lowprintf("RX timeout\n"); } /* From 48e497e4069a2f8773d90f2d1887967a81e487d8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 19 Jan 2013 18:05:53 -0800 Subject: [PATCH 46/47] Fix a leftover from the earlier merges; building should work now. --- apps/commander/commander.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7654b34261..3a6fecf746 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1694,7 +1694,7 @@ int commander_thread_main(int argc, char *argv[]) * the system is currently not armed, set home * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop < dop_threshold_m) + if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) && (vdop_m < dop_threshold_m) && !home_position_set && (hrt_absolute_time() - gps_position.timestamp < 2000000) From f14c90c2220fffc51bc6e6e89bac6f9e726ff505 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Jan 2013 17:42:08 -0800 Subject: [PATCH 47/47] Some timeout needed to be raised for now to make ubx with baudrate 9600 working --- apps/gps/ubx.c | 5 ++++- apps/gps/ubx.h | 4 ++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 50bf579a08..b9f8a28a07 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -880,6 +880,7 @@ void *ubx_watchdog_loop(void *args) int *fd = arguments->fd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; + ubx_config_state = UBX_CONFIG_STATE_PRT; /* first try to configure the GPS anyway */ configure_gps_ubx(fd); @@ -892,7 +893,7 @@ void *ubx_watchdog_loop(void *args) bool once_ok = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - ubx_config_state = UBX_CONFIG_STATE_PRT; + //int err_skip_counter = 0; while (!(*thread_should_exit)) { @@ -940,7 +941,9 @@ void *ubx_watchdog_loop(void *args) ubx_healthy = false; ubx_success_count = 0; } + /* trying to reconfigure the gps configuration */ + ubx_config_state = UBX_CONFIG_STATE_PRT; configure_gps_ubx(fd); fflush(stdout); sleep(1); diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index e700fe3880..f3313a3c6d 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -51,9 +51,9 @@ //internal definitions (not depending on the ubx protocol #define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */ -#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 1000000 /**< Check for current state every second */ +#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 2000000 /**< Check for current state every two seconds */ -#define UBX_CONFIG_TIMEOUT 500000 +#define UBX_CONFIG_TIMEOUT 1000000 #define APPNAME "gps: ubx"