aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/gps/gps.c5
-rw-r--r--apps/gps/ubx.c327
-rw-r--r--apps/gps/ubx.h117
-rw-r--r--apps/sdlog/sdlog.c399
4 files changed, 556 insertions, 292 deletions
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..50bf579a0 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,9 @@ void *ubx_watchdog_loop(void *args)
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
+ /* first try to configure the GPS anyway */
+ configure_gps_ubx(fd);
+
/* GPS watchdog error message skip counter */
bool ubx_healthy = false;
@@ -732,7 +892,7 @@ void *ubx_watchdog_loop(void *args)
bool once_ok = false;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
+ ubx_config_state = UBX_CONFIG_STATE_PRT;
//int err_skip_counter = 0;
while (!(*thread_should_exit)) {
@@ -833,23 +993,6 @@ void *ubx_loop(void *args)
/* set parameters for ubx */
- if (configure_gps_ubx(fd) != 0) {
- printf("[gps] Configuration of gps module to ubx failed\n");
-
- /* Write shared variable sys_status */
- // TODO enable this again
- //global_data_send_subsystem_info(&ubx_present);
-
- } else {
- if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n");
-
- // XXX Shouldn't the system status only change if the module is known to work ok?
-
- /* Write shared variable sys_status */
- // TODO enable this again
- //global_data_send_subsystem_info(&ubx_present_enabled);
- }
-
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
ubx_gps = &ubx_gps_d;
diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h
index f4f01df9a..e700fe388 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 1000000 /**< Check for current state every second */
+
+#define UBX_CONFIG_TIMEOUT 500000
#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/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;
+ }
+
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-13 04:59:49 +0000