aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/commander/commander.c139
-rw-r--r--apps/drivers/blinkm/blinkm.cpp443
-rw-r--r--apps/examples/control_demo/params.c44
-rw-r--r--apps/examples/kalman_demo/KalmanNav.cpp393
-rw-r--r--apps/examples/kalman_demo/KalmanNav.hpp134
-rw-r--r--apps/examples/kalman_demo/params.c14
-rw-r--r--apps/gps/gps.c5
-rw-r--r--apps/gps/ubx.c328
-rw-r--r--apps/gps/ubx.h117
-rw-r--r--apps/mavlink/mavlink_receiver.c19
-rw-r--r--apps/mavlink/orb_listener.c18
-rw-r--r--apps/mavlink/orb_topics.h2
-rw-r--r--apps/sdlog/sdlog.c399
-rw-r--r--apps/uORB/objects_common.cpp3
-rw-r--r--apps/uORB/topics/home_position.h77
15 files changed, 1511 insertions, 624 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f3568ee8d..3a6fecf74 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -72,8 +72,10 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
@@ -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, &current_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,57 @@ 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, &current_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, &current_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, &current_status, mavlink_fd);
- /* end: check gps */
+ /* check for first, long-term and valid GPS lock -> set home position */
+ 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)
+ * 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_m < dop_threshold_m)
+ && (vdop_m < dop_threshold_m)
+ && !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);
+ }
+ /* mark home position as set */
+ home_position_set = true;
+ tune_confirm();
+ }
+ }
/* ignore RC signals if in offboard control mode */
if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
@@ -1845,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[])
update_state_machine_mode_manual(stat_pub, &current_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, &current_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, &current_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, &current_status, mavlink_fd);
- }
+ // }
} else {
/* center stick position, set SAS for all vehicle types */
@@ -2041,4 +2047,3 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
-
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index d589025cc..aeee80905 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -35,9 +35,61 @@
* @file blinkm.cpp
*
* Driver for the BlinkM LED controller connected via I2C.
+ *
+ * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
+ * blinkm start
+ *
+ * To start the system monitor put in the next line after the blinkm start:
+ * blinkm systemmonitor
+ *
+ *
+ * Description:
+ * After startup, the Application checked how many lipo cells are connected to the System.
+ * The recognized number off cells, will be blinked 5 times in purple color.
+ * 2 Cells = 2 blinks
+ * ...
+ * 5 Cells = 5 blinks
+ * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
+ *
+ * System disarmed:
+ * The BlinkM should lit solid red.
+ *
+ * System armed:
+ * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
+ *
+ * X-X-X-X-_-_-_-_-
+ * -------------------------
+ * G G G M
+ * P P P O
+ * S S S D
+ * E
+ *
+ * (X = on, _=off)
+ *
+ * The first 3 blinks indicates the status of the GPS-Signal (red):
+ * 0-4 satellites = X-X-X-X-_-_-_-_-
+ * 5 satellites = X-X-_-X-_-_-_-_-
+ * 6 satellites = X-_-_-X-_-_-_-_-
+ * >=7 satellites = _-_-_-X-_-_-_-_-
+ * If no GPS is found the first 3 blinks are white
+ *
+ * The fourth Blink indicates the Flightmode:
+ * MANUAL : 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
+ *
+ * General Error (no uOrb Data)
+ * Continuously blinking in white X-X-X-X-X-X-X-X
+ *
*/
-
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -59,15 +111,28 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+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:
BlinkM(int bus);
~BlinkM();
+
virtual int init();
virtual int probe();
-
+ virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
static const char *script_names[];
@@ -95,11 +160,31 @@ private:
MORSE_CODE
};
+ enum ledColors {
+ LED_OFF,
+ LED_RED,
+ LED_YELLOW,
+ LED_PURPLE,
+ LED_GREEN,
+ LED_BLUE,
+ LED_WHITE
+ };
+
work_s _work;
- static const unsigned _monitor_interval = 250;
- static void monitor_trampoline(void *arg);
- void monitor();
+ 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;
+
+ bool systemstate_run;
+
+ void setLEDColor(int ledcolor);
+ static void led_trampoline(void *arg);
+ void led();
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
@@ -127,8 +212,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 +239,21 @@ const char *BlinkM::script_names[] = {
nullptr
};
+
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));
}
BlinkM::~BlinkM()
@@ -170,7 +264,6 @@ int
BlinkM::init()
{
int ret;
-
ret = I2C::init();
if (ret != OK) {
@@ -178,14 +271,25 @@ BlinkM::init()
return ret;
}
- /* set some sensible defaults */
- set_fade_speed(25);
+ stop_script();
+ set_rgb(0,0,0);
- /* turn off by default */
- play_script(BLACK);
+ return OK;
+}
- /* start the system monitor as a low-priority workqueue entry */
- work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
+int
+BlinkM::setMode(int mode)
+{
+ if(mode == 1) {
+ if(systemstate_run == false) {
+ stop_script();
+ set_rgb(0,0,0);
+ systemstate_run = true;
+ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
+ }
+ } else {
+ systemstate_run = false;
+ }
return OK;
}
@@ -249,21 +353,300 @@ 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()
+BlinkM::led()
{
- /* 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);
+ 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[6] = { 0, 0, 0, 0, 0, 0};
+ static int t_led_blink = 0;
+ static int led_thread_runcount=0;
+ static int led_interval = 1000;
+
+ static int no_data_vehicle_status = 0;
+ static int no_data_vehicle_gps_position = 0;
+
+ static bool topic_initialized = false;
+ static bool detected_cells_blinked = false;
+ static bool led_thread_ready = true;
+
+ int num_of_used_sats = 0;
+
+ if(!topic_initialized) {
+ vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
+ orb_set_interval(vehicle_status_sub_fd, 1000);
+
+ vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
+ orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+
+ topic_initialized = true;
+ }
+
+ if(led_thread_ready == true) {
+ if(!detected_cells_blinked) {
+ if(num_of_cells > 0) {
+ t_led_color[0] = LED_PURPLE;
+ }
+ if(num_of_cells > 1) {
+ t_led_color[1] = LED_PURPLE;
+ }
+ if(num_of_cells > 2) {
+ t_led_color[2] = LED_PURPLE;
+ }
+ if(num_of_cells > 3) {
+ t_led_color[3] = LED_PURPLE;
+ }
+ if(num_of_cells > 4) {
+ t_led_color[4] = LED_PURPLE;
+ }
+ t_led_color[5] = LED_OFF;
+ t_led_blink = LED_BLINK;
+ } else {
+ t_led_color[0] = led_color_1;
+ t_led_color[1] = led_color_2;
+ t_led_color[2] = led_color_3;
+ t_led_color[3] = led_color_4;
+ t_led_color[4] = led_color_5;
+ t_led_color[5] = led_color_6;
+ t_led_blink = led_blink;
+ }
+ led_thread_ready = false;
+ }
+
+ if (led_thread_runcount & 1) {
+ if (t_led_blink)
+ setLEDColor(LED_OFF);
+ led_interval = LED_OFFTIME;
+ } else {
+ setLEDColor(t_led_color[(led_thread_runcount / 2) % 6]);
+ //led_interval = (led_thread_runcount & 1) : LED_ONTIME;
+ led_interval = LED_ONTIME;
+ }
+
+ 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("<blinkm> 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("<blinkm> 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( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
+ vehicle_status_raw.voltage_battery,
+ vehicle_status_raw.flag_system_armed,
+ vehicle_status_raw.flight_mode,
+ num_of_cells,
+ no_data_vehicle_status,
+ no_data_vehicle_gps_position,
+ num_of_used_sats,
+ vehicle_gps_position_raw.fix_type,
+ vehicle_gps_position_raw.satellites_visible);
+ */
+
+ led_thread_runcount=0;
+ led_thread_ready = true;
+ led_interval = LED_OFFTIME;
+
+ if(detected_cells_runcount < 4){
+ detected_cells_runcount++;
+ } else {
+ detected_cells_blinked = true;
+ }
+
+ } else {
+ led_thread_runcount++;
+ }
+
+ if(systemstate_run == true) {
+ /* re-queue ourselves to run again later */
+ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
+ } else {
+ stop_script();
+ set_rgb(0,0,0);
+ }
+}
+
+void BlinkM::setLEDColor(int ledcolor) {
+ switch (ledcolor) {
+ case LED_OFF: // off
+ set_rgb(0,0,0);
+ break;
+ case LED_RED: // red
+ set_rgb(255,0,0);
+ break;
+ case LED_YELLOW: // yellow
+ set_rgb(255,70,0);
+ break;
+ case LED_PURPLE: // purple
+ set_rgb(255,0,255);
+ break;
+ case LED_GREEN: // green
+ set_rgb(0,255,0);
+ break;
+ case LED_BLUE: // blue
+ set_rgb(0,0,255);
+ break;
+ case LED_WHITE: // white
+ set_rgb(255,255,255);
+ break;
+ }
}
int
@@ -413,7 +796,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 +825,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 +852,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.");
+}
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 8f923f5a1..6525b70f5 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -14,50 +14,50 @@ 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.5f); // 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
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.2f);
-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, 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
+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, 1.0f);
+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);
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.005f); // 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.8f); // trim throttle (0,1)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 240564b56..7e89dffb2 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -45,9 +45,9 @@
// 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
+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),
@@ -55,6 +55,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),
@@ -66,7 +67,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
C_nb(),
q(),
// subscriptions
- _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
+ _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 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
@@ -74,17 +75,16 @@ 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),
- // state
+ _miss(0),
+ // 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),
@@ -95,42 +95,32 @@ 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, "ENV_MAG_DIP"),
+ _magDec(this, "ENV_MAG_DEC"),
+ _g(this, "ENV_G"),
+ _faultPos(this, "FAULT_POS"),
+ _faultAtt(this, "FAULT_ATT"),
+ _attitudeInitialized(false),
+ _positionInitialized(false),
+ _attitudeInitCounter(0)
{
using namespace math;
// initial state covariance matrix
- P = Matrix::identity(9) * 1.0f;
-
- // 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");
- }
- }
+ P0 = Matrix::identity(9) * 0.01f;
+ P = P0;
// 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;
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
@@ -141,8 +131,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
@@ -153,18 +143,13 @@ 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) {
// XXX this is seriously bad - should be an emergency
return;
@@ -186,39 +171,67 @@ 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 (_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) {
+ 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] 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);
+ }
- // fast prediciton step
- // note, using sensors timestamp so we can account
- // for packet lag
- float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
- _fastTimeStamp = _sensors.timestamp;
+ // 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(dt));
+ _predictTimeStamp = _sensors.timestamp;
- if (dtFast < 1.0f / 50) {
- predictFast(dtFast);
+ // 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) {
+ 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();
}
@@ -226,16 +239,17 @@ void KalmanNav::update()
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
+
updatePublications();
}
// 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, miss #: %4d\n",
+ _navFrames / 10, _miss / 10);
_navFrames = 0;
- _missFast = 0;
- _missSlow = 0;
+ _miss = 0;
}
}
@@ -278,73 +292,87 @@ 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();
}
-void KalmanNav::predictFast(float dt)
+int KalmanNav::predictState(float dt)
{
using namespace math;
- Vector3 w(_sensors.gyro_rad_s);
- // attitude
- q = q + q.derivative(w) * dt;
+ // trig
+ float sinL = sinf(lat);
+ float cosL = cosf(lat);
+ float cosLSing = cosf(lat);
- // renormalize quaternion if needed
- if (fabsf(q.norm() - 1.0f) > 1e-4f) {
- q = q.unit();
+ // prevent singularity
+ if (fabsf(cosLSing) < 0.01f) {
+ if (cosLSing > 0) cosLSing = 0.01;
+ else cosLSing = -0.01;
}
- // C_nb update
- C_nb = Dcm(q);
+ // attitude prediction
+ if (_attitudeInitialized) {
+ Vector3 w(_sensors.gyro_rad_s);
- // euler update
- EulerAngles euler(C_nb);
- phi = euler.getPhi();
- theta = euler.getTheta();
- psi = euler.getPsi();
+ // attitude
+ q = q + q.derivative(w) * dt;
- // 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);
+ // renormalize quaternion if needed
+ if (fabsf(q.norm() - 1.0f) > 1e-4f) {
+ q = q.unit();
+ }
- // trig
- float sinL = sinf(lat);
- float cosL = cosf(lat);
- float cosLSing = cosf(lat);
+ // 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);
+ }
- if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+ // 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);
+ }
- // 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;
- float vEDot = fE + vN * rotRate * sinL +
- 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;
- lat += double(LDot * dt);
- lon += double(lDot * dt);
- alt += double(-vD * dt);
+ return ret_ok;
}
-void KalmanNav::predictSlow(float dt)
+int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
@@ -356,6 +384,7 @@ void KalmanNav::predictSlow(float dt)
// prepare for matrix
float R = R0 + float(alt);
+ float RSq = R * R;
// F Matrix
// Titterton pg. 291
@@ -436,9 +465,11 @@ void KalmanNav::predictSlow(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;
@@ -452,12 +483,14 @@ 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,
// 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
+ 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);
@@ -472,7 +505,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;
@@ -483,12 +516,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) /*+ zCentrip*/).unit();
+ Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
// combined measurement
Vector zAtt(6);
@@ -553,9 +582,9 @@ 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
- P = Matrix::identity(9) * 1.0f;
- return;
+ // reset P matrix to P0
+ P = P0;
+ return ret_error;
}
}
@@ -568,9 +597,11 @@ void 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
@@ -579,38 +610,33 @@ 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();
+ printf("y:\n"); y.print();
+ printf("zMagHat:\n"); zMagHat.print();
+ printf("zMag:\n"); zMag.print();
+ printf("bNav:\n"); bNav.print();
}
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
+
+ return ret_ok;
}
-void KalmanNav::correctPos()
+int KalmanNav::correctPos()
{
using namespace math;
- Vector y(6);
+
+ // residual
+ 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(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);
- float R = R0 + float(alt);
-
- // prevent singularity
- if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
-
- float noiseLat = _rGpsPos.get() / R;
- float noiseLon = _rGpsPos.get() / (R * cosLSing);
- RPos(2, 2) = noiseLat * noiseLat;
- RPos(3, 3) = noiseLon * noiseLon;
-
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
@@ -631,9 +657,9 @@ void KalmanNav::correctPos()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
- // reset P matrix to identity
- P = Matrix::identity(9) * 1.0f;
- return;
+ // reset P matrix to P0
+ P = P0;
+ return ret_error;
}
}
@@ -652,10 +678,17 @@ 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"); y.print();
+ 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(4, 4))));
}
+
+ return ret_ok;
}
void KalmanNav::updateParams()
@@ -664,7 +697,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
@@ -676,31 +708,54 @@ 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;
// gps noise
- float cosLSing = cosf(lat);
float R = R0 + float(alt);
+ float cosLSing = cosf(lat);
// 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;
- float noiseLon = _rGpsPos.get() / (R * cosLSing);
+ 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) = 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
+ // XXX, note that RPos depends on lat, so updateParams should
+ // be called if lat changes significantly
}
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index 1ea46d40f..7709ac697 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -68,52 +68,108 @@
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();
+
+
+ /**
+ * Publication update
+ */
virtual void updatePublications();
- void predictFast(float dt);
- void predictSlow(float dt);
- void correctAtt();
- void correctPos();
+
+ /**
+ * State prediction
+ * Continuous, non-linear
+ */
+ int predictState(float dt);
+
+ /**
+ * State covariance prediction
+ * Continuous, linear
+ */
+ int predictStateCovariance(float dt);
+
+ /**
+ * Attitude correction
+ */
+ int correctAtt();
+
+ /**
+ * Position correction
+ */
+ int correctPos();
+
+ /**
+ * Overloaded update parameters
+ */
virtual void updateParams();
protected:
- math::Matrix F;
- math::Matrix G;
- math::Matrix P;
- math::Matrix V;
- math::Matrix HAtt;
- math::Matrix RAtt;
- math::Matrix HPos;
- math::Matrix RPos;
- math::Dcm C_nb;
- math::Quaternion q;
- control::UOrbSubscription<sensor_combined_s> _sensors;
- control::UOrbSubscription<vehicle_gps_position_s> _gps;
- control::UOrbSubscription<parameter_update_s> _param_update;
- control::UOrbPublication<vehicle_global_position_s> _pos;
- control::UOrbPublication<vehicle_attitude_s> _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<sensor_combined_s> _sensors; /**< sensors sub. */
+ control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
+ control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
+ // publications
+ control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
+ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
+ // time stamps
+ uint64_t _pubTimeStamp; /**< output data publication 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 _miss; /**< number of times fast 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<float> _vGyro;
- control::BlockParam<float> _vAccel;
- control::BlockParam<float> _rMag;
- control::BlockParam<float> _rGpsVel;
- control::BlockParam<float> _rGpsPos;
- control::BlockParam<float> _rGpsAlt;
- control::BlockParam<float> _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<float> _vGyro; /**< gyro process noise */
+ control::BlockParam<float> _vAccel; /**< accelerometer process noise */
+ control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
+ control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
+ control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
+ control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
+ control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
+ control::BlockParam<float> _magDip; /**< magnetic inclination with level */
+ control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
+ control::BlockParam<float> _g; /**< gravitational constant */
+ control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
+ control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
+ // status
+ 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; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 03cdca5ed..3bfe01261 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,10 +1,16 @@
#include <systemlib/param/param.h>
/*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);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 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_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);
+
diff --git a/apps/gps/gps.c b/apps/gps/gps.c
index 00f6ee9f8..8a9512054 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
@@ -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 e15ed4e00..b9f8a28a0 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -52,7 +52,7 @@
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
-#define UBX_BUFFER_SIZE 1000
+#define UBX_BUFFER_SIZE 500
extern bool gps_mode_try_all;
extern bool gps_mode_success;
@@ -63,18 +63,10 @@ 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;
-//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_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};
void ubx_decode_init(void)
{
@@ -100,15 +92,15 @@ 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) {
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) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
@@ -122,16 +114,25 @@ 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:
+ 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;
+ 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;
@@ -193,9 +194,36 @@ 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;
+ 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;
@@ -542,6 +570,75 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
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 == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
+
+ switch (ubx_config_state) {
+ case UBX_CONFIG_STATE_PRT:
+ 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] 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) {
+
+ if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n");
+ ret = 1;
+
+ } else {
+ if (gps_verbose) printf("[gps] ACK_NAK: 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();
@@ -574,53 +671,105 @@ 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)
{
- //fflush(((FILE *)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);
-
- //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);
-
- //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);
-
- //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);
-
- return 0;
+ // 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 = current_gps_speed,
+ .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK,
+ .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK,
+ .ck_a = 0,
+ .ck_b = 0
+ };
+
+ // 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
+ };
+
+ // 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
+ };
+
+ uint64_t time_before_config = hrt_absolute_time();
+
+ 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);
+ }
+ if (gps_verbose) printf("[gps] ubx configuration timeout\n");
+ return ERROR;
}
@@ -637,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.
@@ -671,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;
}
@@ -688,28 +832,41 @@ 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;
uint8_t ck_b = 0;
unsigned int i;
- for (i = 2; i < length; i++) {
+ 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("[%x,%x]\n", ck_a, ck_b);
+ // write sync bytes first
+ buffer[0] = UBX_SYNC_1;
+ buffer[1] = UBX_SYNC_2;
+
+ // write config message without the checksum
+ result_write = write(*fd, buffer, sizeof(buffer));
+ result_write += write(*fd, message, length-2);
+
+ buffer[0] = ck_a;
+ buffer[1] = 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);
+ // write the checksum
+ result_write += write(*fd, buffer, sizeof(buffer));
- return (result_write != length + 2); //return 0 as success
+ fsync(*fd);
+ if ((unsigned int)result_write != length + 2)
+ return ERROR;
+ return OK;
}
void *ubx_watchdog_loop(void *args)
@@ -723,6 +880,10 @@ 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);
+
/* GPS watchdog error message skip counter */
bool ubx_healthy = false;
@@ -780,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);
@@ -833,23 +996,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;
diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h
index f4f01df9a..f3313a3c6 100644
--- a/apps/gps/ubx.h
+++ b/apps/gps/ubx.h
@@ -49,15 +49,17 @@
//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 2000000 /**< Check for current state every two seconds */
+
+#define UBX_CONFIG_TIMEOUT 1000000
#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,6 +74,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 */
// ************
@@ -216,7 +236,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;
@@ -226,7 +246,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;
@@ -234,15 +254,91 @@ 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 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;
+ 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;
+
+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,
RXM = 2,
- ACK = 3
+ ACK = 3,
+ CFG = 4
};
enum UBX_MESSAGE_IDS {
@@ -254,7 +350,10 @@ enum UBX_MESSAGE_IDS {
NAV_DOP = 4,
NAV_SVINFO = 5,
NAV_VELNED = 6,
- RXM_SVSI = 7
+ RXM_SVSI = 7,
+ CFG_NAV5 = 8,
+ ACK_ACK = 9,
+ ACK_NAK = 10
};
enum UBX_DECODE_STATES {
@@ -304,7 +403,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);
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index fa63c419f..9491ce7e3 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;
@@ -367,8 +369,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 +414,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;
}
@@ -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
@@ -454,8 +459,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;
}
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index f5253ea35..ec5149745 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 */
@@ -702,7 +718,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, 100); /* 10 Hz active updates */
/* --- LOCAL POS VALUE --- */
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h
index 4650c4991..d61cd43dc 100644
--- a/apps/mavlink/orb_topics.h
+++ b/apps/mavlink/orb_topics.h
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@@ -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/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index f8668a2e3..4fd5fea1b 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -51,6 +51,7 @@
#include <poll.h>
#include <stdlib.h>
#include <string.h>
+#include <ctype.h>
#include <systemlib/err.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
@@ -73,6 +74,8 @@
#include <systemlib/systemlib.h>
+#include <mavlink/mavlink_log.h>
+
#include "sdlog_ringbuffer.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
@@ -83,6 +86,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 +122,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 +140,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
- errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
+ errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
}
// XXX turn this into a C++ class
@@ -145,6 +151,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
@@ -172,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);
}
@@ -318,8 +327,54 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf)
int sdlog_thread_main(int argc, char *argv[])
{
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+ if (mavlink_fd < 0) {
+ warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
+ }
+
+ /* 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, "s:r")) != 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;
- warnx("starting\n");
+ case 'r':
+ /* log only on request, disable logging per default */
+ 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.");
+ }
+ }
if (file_exist(mountpoint) != OK) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
@@ -330,31 +385,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");
@@ -362,13 +401,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");
@@ -385,6 +417,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 --- */
@@ -432,18 +465,24 @@ 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;
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++;
@@ -496,13 +535,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));
@@ -560,22 +592,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;
-
- /* log every 2nd value (skip one) */
- int skip_value = 0;
/* 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, 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) {
@@ -584,159 +607,129 @@ 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);
-
- if (skip_count < skip_value) {
- skip_count++;
- /* do not log data */
- continue;
- } else {
- /* log data, reset */
- skip_count = 0;
+ int ifds = 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);
+ }
+
+ /* --- 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 logging disabled, continue */
+ if (logging_enabled) {
+
+ /* write KML line */
+ }
+ }
+
+ /* --- 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 / 2)) {
+ /* 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);
}
- // int ifds = 0;
-
- // if (poll_count % 5000 == 0) {
- // fsync(sensorfile);
- // fsync(actuator_outputs_file);
- // fsync(actuator_controls_file);
- // fsync(blackbox_file_no);
- // }
-
-
-
- // /* --- 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 */
- pthread_cond_signal(&sysvector_cond);
- /* unlock, now the writer thread may run */
- pthread_mutex_unlock(&sysvector_mutex);
}
}
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");
- close(sensorfile);
- close(actuator_outputs_file);
- close(actuator_controls_file);
+ /* finish KML file */
+ // XXX
fclose(gpsfile);
fclose(blackbox_file);
@@ -803,4 +796,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;
+ }
+
+}
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 82893b3e9..3f3476f62 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 000000000..dec34b6ab
--- /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 <lm@inf.ethz.ch>
+ *
+ * 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 <lm@inf.ethz.ch>
+ */
+
+#ifndef TOPIC_HOME_POSITION_H_
+#define TOPIC_HOME_POSITION_H_
+
+#include <stdint.h>
+#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
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-16 09:38:35 +0000