diff options
-rw-r--r-- | apps/gps/gps.c | 5 | ||||
-rw-r--r-- | apps/gps/ubx.c | 327 | ||||
-rw-r--r-- | apps/gps/ubx.h | 117 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 399 | ||||
-rw-r--r-- | nuttx/arch/arm/src/stm32/stm32_i2c.c | 391 | ||||
-rwxr-xr-x | nuttx/configs/px4fmu/nsh/defconfig | 32 |
6 files changed, 774 insertions, 497 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; + } + +} diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 10e6cac65..a4b10b55c 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -107,23 +107,17 @@ #if !defined(CONFIG_STM32_I2CTIMEOSEC) && !defined(CONFIG_STM32_I2CTIMEOMS) # define CONFIG_STM32_I2CTIMEOSEC 0 -# define CONFIG_STM32_I2CTIMEOMS 500 /* Default is 500 milliseconds */ +# define CONFIG_STM32_I2CTIMEOMS 500 /* Default is 500 milliseconds */ #elif !defined(CONFIG_STM32_I2CTIMEOSEC) # define CONFIG_STM32_I2CTIMEOSEC 0 /* User provided milliseconds */ #elif !defined(CONFIG_STM32_I2CTIMEOMS) -# define CONFIG_STM32_I2CTIMEOMS 0 /* User provided seconds */ +# define CONFIG_STM32_I2CTIMEOMS 0 /* User provided seconds */ #endif /* Interrupt wait time timeout in system timer ticks */ -#ifndef CONFIG_STM32_I2CTIMEOTICKS -# define CONFIG_STM32_I2CTIMEOTICKS \ - (SEC2TICK(CONFIG_STM32_I2CTIMEOSEC) + MSEC2TICK(CONFIG_STM32_I2CTIMEOMS)) -#endif - -#ifndef CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP -# define CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP TICK2USEC(CONFIG_STM32_I2CTIMEOTICKS) -#endif +#define CONFIG_STM32_I2CTIMEOTICKS \ + (SEC2TICK(CONFIG_STM32_I2CTIMEOSEC) + MSEC2TICK(CONFIG_STM32_I2CTIMEOMS)) /* On the STM32F103ZE, there is an internal conflict between I2C1 and FSMC. In that * case, it is necessary to disable FSMC before each I2C1 access and re-enable FSMC @@ -135,18 +129,6 @@ # define I2C1_FSMC_CONFLICT #endif -/* Macros to convert a I2C pin to a GPIO output */ - -#if defined(CONFIG_STM32_STM32F10XX) -# define I2C_OUTPUT (GPIO_OUTPUT | GPIO_OUTPUT_SET | GPIO_CNF_OUTOD | \ - GPIO_MODE_50MHz) -#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) -# define I2C_OUTPUT (GPIO_OUTPUT | GPIO_FLOAT | GPIO_OPENDRAIN |\ - GPIO_SPEED_50MHz | GPIO_OUTPUT_SET) -#endif - -#define MKI2C_OUTPUT(p) (((p) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | I2C_OUTPUT) - /* Debug ****************************************************************************/ /* CONFIG_DEBUG_I2C + CONFIG_DEBUG enables general I2C debug output. */ @@ -218,16 +200,18 @@ struct stm32_trace_s struct stm32_i2c_config_s { - uint32_t base; /* I2C base address */ - uint32_t clk_bit; /* Clock enable bit */ - uint32_t reset_bit; /* Reset bit */ - uint32_t scl_pin; /* GPIO configuration for SCL as SCL */ - uint32_t sda_pin; /* GPIO configuration for SDA as SDA */ + uint32_t base; /* I2C base address */ #ifndef CONFIG_I2C_POLLED - int (*isr)(int, void *); /* Interrupt handler */ - uint32_t ev_irq; /* Event IRQ */ - uint32_t er_irq; /* Error IRQ */ + int ( *isr)(int, void *); /* Interrupt handler */ #endif + uint32_t clk_bit; /* Clock enable bit */ + uint32_t reset_bit; /* Reset bit */ + uint32_t scl_pin; /* GPIO configuration for SCL as SCL */ + uint32_t scl_gpio; /* GPIO configuration for SCL as a GPIO */ + uint32_t sda_pin; /* GPIO configuration for SDA as SDA */ + uint32_t sda_gpio; /* GPIO configuration for SDA as a GPIO */ + uint32_t ev_irq; /* Event IRQ */ + uint32_t er_irq; /* Error IRQ */ }; /* I2C Device Private Data */ @@ -235,31 +219,31 @@ struct stm32_i2c_config_s struct stm32_i2c_priv_s { const struct stm32_i2c_config_s *config; /* Port configuration */ - int refs; /* Referernce count */ - sem_t sem_excl; /* Mutual exclusion semaphore */ + int refs; /* Referernce count */ + sem_t sem_excl; /* Mutual exclusion semaphore */ #ifndef CONFIG_I2C_POLLED - sem_t sem_isr; /* Interrupt wait semaphore */ + sem_t sem_isr; /* Interrupt wait semaphore */ #endif - volatile uint8_t intstate; /* Interrupt handshake (see enum stm32_intstate_e) */ + volatile uint8_t intstate; /* Interrupt handshake (see enum stm32_intstate_e) */ - uint8_t msgc; /* Message count */ - struct i2c_msg_s *msgv; /* Message list */ - uint8_t *ptr; /* Current message buffer */ - int dcnt; /* Current message length */ - uint16_t flags; /* Current message flags */ + uint8_t msgc; /* Message count */ + struct i2c_msg_s *msgv; /* Message list */ + uint8_t *ptr; /* Current message buffer */ + int dcnt; /* Current message length */ + uint16_t flags; /* Current message flags */ /* I2C trace support */ #ifdef CONFIG_I2C_TRACE - int tndx; /* Trace array index */ - uint32_t start_time; /* Time when the trace was started */ + int tndx; /* Trace array index */ + uint32_t start_time; /* Time when the trace was started */ /* The actual trace data */ struct stm32_trace_s trace[CONFIG_I2C_NTRACE]; #endif - uint32_t status; /* End of transfer SR2|SR1 status */ + uint32_t status; /* End of transfer SR2|SR1 status */ }; /* I2C Device, Instance */ @@ -286,11 +270,8 @@ static inline void stm32_i2c_modifyreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset, uint16_t clearbits, uint16_t setbits); static inline void stm32_i2c_sem_wait(FAR struct i2c_dev_s *dev); -#ifdef CONFIG_STM32_I2C_DYNTIMEO -static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs); -#endif /* CONFIG_STM32_I2C_DYNTIMEO */ -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv); -static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv); +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us); +static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv, int timeout_us); static inline void stm32_i2c_sem_post(FAR struct i2c_dev_s *dev); static inline void stm32_i2c_sem_init(FAR struct i2c_dev_s *dev); static inline void stm32_i2c_sem_destroy(FAR struct i2c_dev_s *dev); @@ -300,7 +281,7 @@ static void stm32_i2c_tracenew(FAR struct stm32_i2c_priv_s *priv, uint32_t statu static void stm32_i2c_traceevent(FAR struct stm32_i2c_priv_s *priv, enum stm32_trace_e event, uint32_t parm); static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv); -#endif /* CONFIG_I2C_TRACE */ +#endif static void stm32_i2c_setclock(FAR struct stm32_i2c_priv_s *priv, uint32_t frequency); static inline void stm32_i2c_sendstart(FAR struct stm32_i2c_priv_s *priv); @@ -310,7 +291,7 @@ static inline uint32_t stm32_i2c_getstatus(FAR struct stm32_i2c_priv_s *priv); #ifdef I2C1_FSMC_CONFLICT static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv); static inline void stm32_i2c_enablefsmc(uint32_t ahbenr); -#endif /* I2C1_FSMC_CONFLICT */ +#endif static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv); #ifndef CONFIG_I2C_POLLED #ifdef CONFIG_STM32_I2C1 @@ -348,18 +329,27 @@ static int stm32_i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *m ************************************************************************************/ #ifdef CONFIG_STM32_I2C1 +# ifndef GPIO_I2C1_SCL_GPIO +# define GPIO_I2C1_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C1_SDA_GPIO +# define GPIO_I2C1_SDA_GPIO 0 +# endif + static const struct stm32_i2c_config_s stm32_i2c1_config = { .base = STM32_I2C1_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c1_isr, +#endif .clk_bit = RCC_APB1ENR_I2C1EN, .reset_bit = RCC_APB1RSTR_I2C1RST, .scl_pin = GPIO_I2C1_SCL, + .scl_gpio = GPIO_I2C1_SCL_GPIO, .sda_pin = GPIO_I2C1_SDA, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c1_isr, + .sda_gpio = GPIO_I2C1_SDA_GPIO, .ev_irq = STM32_IRQ_I2C1EV, .er_irq = STM32_IRQ_I2C1ER -#endif }; struct stm32_i2c_priv_s stm32_i2c1_priv = @@ -377,18 +367,27 @@ struct stm32_i2c_priv_s stm32_i2c1_priv = #endif #ifdef CONFIG_STM32_I2C2 +# ifndef GPIO_I2C2_SCL_GPIO +# define GPIO_I2C2_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C2_SDA_GPIO +# define GPIO_I2C2_SDA_GPIO 0 +# endif + static const struct stm32_i2c_config_s stm32_i2c2_config = { .base = STM32_I2C2_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c2_isr, +#endif .clk_bit = RCC_APB1ENR_I2C2EN, .reset_bit = RCC_APB1RSTR_I2C2RST, .scl_pin = GPIO_I2C2_SCL, + .scl_gpio = GPIO_I2C2_SCL_GPIO, .sda_pin = GPIO_I2C2_SDA, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c2_isr, + .sda_gpio = GPIO_I2C2_SDA_GPIO, .ev_irq = STM32_IRQ_I2C2EV, .er_irq = STM32_IRQ_I2C2ER -#endif }; struct stm32_i2c_priv_s stm32_i2c2_priv = @@ -406,18 +405,27 @@ struct stm32_i2c_priv_s stm32_i2c2_priv = #endif #ifdef CONFIG_STM32_I2C3 +# ifndef GPIO_I2C3_SCL_GPIO +# define GPIO_I2C3_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C3_SDA_GPIO +# define GPIO_I2C3_SDA_GPIO 0 +# endif + static const struct stm32_i2c_config_s stm32_i2c3_config = { .base = STM32_I2C3_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c3_isr, +#endif .clk_bit = RCC_APB1ENR_I2C3EN, .reset_bit = RCC_APB1RSTR_I2C3RST, .scl_pin = GPIO_I2C3_SCL, + .scl_gpio = GPIO_I2C3_SCL_GPIO, .sda_pin = GPIO_I2C3_SDA, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c3_isr, + .sda_gpio = GPIO_I2C3_SDA_GPIO, .ev_irq = STM32_IRQ_I2C3EV, .er_irq = STM32_IRQ_I2C3ER -#endif }; struct stm32_i2c_priv_s stm32_i2c3_priv = @@ -518,35 +526,6 @@ static inline void stm32_i2c_sem_wait(FAR struct i2c_dev_s *dev) } /************************************************************************************ - * Name: stm32_i2c_tousecs - * - * Description: - * Return a micro-second delay based on the number of bytes left to be processed. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO -static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs) -{ - size_t bytecount = 0; - int i; - - /* Count the number of bytes left to process */ - - for (i = 0; i < msgc; i++) - { - bytecount += msgs[i].length; - } - - /* Then return a number of microseconds based on a user provided scaling - * factor. - */ - - return (useconds_t)(CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE * bytecount); -} -#endif - -/************************************************************************************ * Name: stm32_i2c_sem_waitdone * * Description: @@ -555,7 +534,7 @@ static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs) ************************************************************************************/ #ifndef CONFIG_I2C_POLLED -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { struct timespec abstime; irqstate_t flags; @@ -587,24 +566,31 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) #if CONFIG_STM32_I2CTIMEOSEC > 0 abstime.tv_sec += CONFIG_STM32_I2CTIMEOSEC; #endif +#if CONFIG_STM32_I2CTIMEOUS_PER_BYTE > 0 - /* Add a value proportional to the number of bytes in the transfer */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - abstime.tv_nsec += 1000 * stm32_i2c_tousecs(priv->msgc, priv->msgv); - if (abstime.tv_nsec > 1000 * 1000 * 1000) + /* Count the number of bytes left to process */ + int i; + int bytecount = 0; + for (i = 0; i < priv->msgc; i++) { - abstime.tv_sec++; - abstime.tv_nsec -= 1000 * 1000 * 1000; + bytecount += priv->msgv[i].length; } -#elif CONFIG_STM32_I2CTIMEOMS > 0 - abstime.tv_nsec += CONFIG_STM32_I2CTIMEOMS * 1000 * 1000; + abstime.tv_nsec += (CONFIG_STM32_I2CTIMEOUS_PER_BYTE * bytecount) * 1000; if (abstime.tv_nsec > 1000 * 1000 * 1000) { abstime.tv_sec++; abstime.tv_nsec -= 1000 * 1000 * 1000; } +#else + #if CONFIG_STM32_I2CTIMEOMS > 0 + abstime.tv_nsec += CONFIG_STM32_I2CTIMEOMS * 1000 * 1000; + if (abstime.tv_nsec > 1000 * 1000 * 1000) + { + abstime.tv_sec++; + abstime.tv_nsec -= 1000 * 1000 * 1000; + } + #endif #endif /* Wait until either the transfer is complete or the timeout expires */ @@ -638,21 +624,12 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) return ret; } #else -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { - uint32_t timeout; uint32_t start; uint32_t elapsed; int ret; - /* Get the timeout value */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - timeout = USEC2TICK(stm32_i2c_tousecs(priv->msgc, priv->msgv)); -#else - timeout = CONFIG_STM32_I2CTIMEOTICKS; -#endif - /* Signal the interrupt handler that we are waiting. NOTE: Interrupts * are currently disabled but will be temporarily re-enabled below when * sem_timedwait() sleeps. @@ -675,11 +652,10 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) } /* Loop until the transfer is complete. */ - - while (priv->intstate != INTSTATE_DONE && elapsed < timeout); + while (priv->intstate != INTSTATE_DONE && elapsed < USEC2TICK(timeout_us)); i2cvdbg("intstate: %d elapsed: %d threshold: %d status: %08x\n", - priv->intstate, elapsed, timeout, priv->status); + priv->intstate, elapsed, USEC2TICK(timeout_us), priv->status); /* Set the interrupt state back to IDLE */ @@ -697,22 +673,13 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) * ************************************************************************************/ -static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv) +static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { uint32_t start; uint32_t elapsed; - uint32_t timeout; uint32_t cr1; uint32_t sr1; - /* Select a timeout */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - timeout = USEC2TICK(CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP); -#else - timeout = CONFIG_STM32_I2CTIMEOTICKS; -#endif - /* Wait as stop might still be in progress; but stop might also * be set because of a timeout error: "The [STOP] bit is set and * cleared by software, cleared by hardware when a Stop condition is @@ -745,7 +712,7 @@ static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv) /* Loop until the stop is complete or a timeout occurs. */ - while (elapsed < timeout); + while (elapsed < USEC2TICK(timeout_us)); /* If we get here then a timeout occurred with the STOP condition * still pending. @@ -965,7 +932,7 @@ static void stm32_i2c_setclock(FAR struct stm32_i2c_priv_s *priv, uint32_t frequ { /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ -#ifdef CONFIG_STM32_I2C_DUTY16_9 +#ifdef CONFIG_I2C_DUTY16_9 speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); /* Set DUTY and fast speed bits */ @@ -1104,7 +1071,7 @@ static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv) /* Is this I2C1 */ -#if defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) +#ifdef CONFIG_STM32_I2C2 if (priv->config->base == STM32_I2C1_BASE) #endif { @@ -1231,29 +1198,26 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv) { stm32_i2c_traceevent(priv, I2CEVENT_RCVBYTE, priv->dcnt); - /* No interrupts or context switches may occur in the following - * sequence. Otherwise, additional bytes may be sent by the - * device. - */ - #ifdef CONFIG_I2C_POLLED irqstate_t state = irqsave(); #endif + /* Receive a byte */ *priv->ptr++ = stm32_i2c_getreg(priv, STM32_I2C_DR_OFFSET); /* Disable acknowledge when last byte is to be received */ - priv->dcnt--; if (priv->dcnt == 1) { stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0); } + priv->dcnt--; #ifdef CONFIG_I2C_POLLED irqrestore(state); #endif + } } @@ -1444,6 +1408,7 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv) /* Enable power and reset the peripheral */ modifyreg32(STM32_RCC_APB1ENR, 0, priv->config->clk_bit); + modifyreg32(STM32_RCC_APB1RSTR, 0, priv->config->reset_bit); modifyreg32(STM32_RCC_APB1RSTR, priv->config->reset_bit, 0); @@ -1463,10 +1428,10 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv) /* Attach ISRs */ #ifndef CONFIG_I2C_POLLED - irq_attach(priv->config->ev_irq, priv->config->isr); - irq_attach(priv->config->er_irq, priv->config->isr); - up_enable_irq(priv->config->ev_irq); - up_enable_irq(priv->config->er_irq); + irq_attach(priv->config->ev_irq, priv->config->isr); + irq_attach(priv->config->er_irq, priv->config->isr); + up_enable_irq(priv->config->ev_irq); + up_enable_irq(priv->config->er_irq); #endif /* Set peripheral frequency, where it must be at least 2 MHz for 100 kHz @@ -1496,23 +1461,17 @@ static int stm32_i2c_deinit(FAR struct stm32_i2c_priv_s *priv) stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0); - /* Unconfigure GPIO pins */ - stm32_unconfiggpio(priv->config->scl_pin); stm32_unconfiggpio(priv->config->sda_pin); - /* Disable and detach interrupts */ - #ifndef CONFIG_I2C_POLLED up_disable_irq(priv->config->ev_irq); up_disable_irq(priv->config->er_irq); irq_detach(priv->config->ev_irq); irq_detach(priv->config->er_irq); #endif - - /* Disable clocking */ - modifyreg32(STM32_RCC_APB1ENR, priv->config->clk_bit, 0); + return OK; } @@ -1574,14 +1533,14 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms struct stm32_i2c_inst_s *inst = (struct stm32_i2c_inst_s *)dev; FAR struct stm32_i2c_priv_s *priv = inst->priv; uint32_t status = 0; - uint32_t ahbenr; + //uint32_t ahbenr; int errval = 0; ASSERT(count); /* Disable FSMC that shares a pin with I2C1 (LBAR) */ - ahbenr = stm32_i2c_disablefsmc(priv); + (void)stm32_i2c_disablefsmc(priv); /* Wait for any STOP in progress. NOTE: If we have to disable the FSMC * then we cannot do this at the top of the loop, unfortunately. The STOP @@ -1589,7 +1548,11 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms */ #ifndef I2C1_FSMC_CONFLICT - stm32_i2c_sem_waitstop(priv); + #if CONFIG_STM32_I2CTIMEOUS_START_STOP > 0 + stm32_i2c_sem_waitstop(priv, CONFIG_STM32_I2CTIMEOUS_START_STOP); + #else + stm32_i2c_sem_waitstop(priv, CONFIG_STM32_I2CTIMEOMS + CONFIG_STM32_I2CTIMEOSEC * 1000000); + #endif #endif /* Clear any pending error interrupts */ @@ -1610,6 +1573,22 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms priv->msgv = msgs; priv->msgc = count; + /* Calculate timeout values */ + int timeout_us = 0; + #if CONFIG_STM32_I2CTIMEOUS_PER_BYTE > 0 + /* Count the number of bytes left to process */ + int i; + int bytecount = 10; + for (i = 0; i < count; i++) + { + bytecount += msgs[i].length; + } + timeout_us = CONFIG_STM32_I2CTIMEOUS_PER_BYTE * bytecount; + //i2cvdbg("i2c wait: %d\n", timeout_us); + #else + timeout_us = CONFIG_STM32_I2CTIMEOMS + CONFIG_STM32_I2CTIMEOSEC * 1000000; + #endif + /* Reset I2C trace logic */ stm32_i2c_tracereset(priv); @@ -1629,7 +1608,7 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms * the BUSY flag. */ - if (stm32_i2c_sem_waitdone(priv) < 0) + if (stm32_i2c_sem_waitdone(priv, timeout_us) < 0) { status = stm32_i2c_getstatus(priv); errval = ETIMEDOUT; @@ -1644,9 +1623,7 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms */ stm32_i2c_clrstart(priv); - - /* Clear busy flag in case of timeout */ - + // XXX also clear busy flag in case of timeout status = priv->status & 0xffff; } else @@ -1976,15 +1953,13 @@ int up_i2cuninitialize(FAR struct i2c_dev_s * dev) * ************************************************************************************/ -#ifdef CONFIG_I2C_RESET int up_i2creset(FAR struct i2c_dev_s * dev) { struct stm32_i2c_priv_s * priv; - unsigned int clock_count; - unsigned int stretch_count; - uint32_t scl_gpio; - uint32_t sda_gpio; + unsigned clock_count; + unsigned stretch_count; int ret = ERROR; + irqstate_t state; ASSERT(dev); @@ -2004,72 +1979,82 @@ int up_i2creset(FAR struct i2c_dev_s * dev) stm32_i2c_deinit(priv); - /* Use GPIO configuration to un-wedge the bus */ - - scl_gpio = MKI2C_OUTPUT(priv->config->scl_pin); - sda_gpio = MKI2C_OUTPUT(priv->config->sda_pin); - - /* Let SDA go high */ - stm32_gpiowrite(sda_gpio, 1); - - /* Clock the bus until any slaves currently driving it let it go. */ + /* If possible, use GPIO configuration to un-wedge the bus */ - clock_count = 0; - while (!stm32_gpioread(sda_gpio)) + if ((priv->config->scl_gpio != 0) && (priv->config->sda_gpio != 0)) { - /* Give up if we have tried too hard */ + stm32_configgpio(priv->config->scl_gpio); + stm32_configgpio(priv->config->sda_gpio); - if (clock_count++ > 10) - { - goto out; - } - - /* Sniff to make sure that clock stretching has finished. - * - * If the bus never relaxes, the reset has failed. + /* + * Clock the bus until any slaves currently driving it let it go. */ - stretch_count = 0; - while (!stm32_gpioread(scl_gpio)) - { + clock_count = 0; + while (!stm32_gpioread(priv->config->sda_gpio)) + { + /* Give up if we have tried too hard */ - if (stretch_count++ > 10) + if (clock_count++ > 1000) { goto out; } - up_udelay(10); - } + /* + * Sniff to make sure that clock stretching has finished. + * + * If the bus never relaxes, the reset has failed. + */ - /* Drive SCL low */ + stretch_count = 0; + while (!stm32_gpioread(priv->config->scl_gpio)) + { - stm32_gpiowrite(scl_gpio, 0); - up_udelay(10); + /* Give up if we have tried too hard */ - /* Drive SCL high again */ + if (stretch_count++ > 1000) + { + goto out; + } - stm32_gpiowrite(scl_gpio, 1); - up_udelay(10); - } + up_udelay(10); - /* Generate a start followed by a stop to reset slave - * state machines. - */ + } + + /* Drive SCL low */ + + stm32_gpiowrite(priv->config->scl_gpio, 0); + up_udelay(10); - stm32_gpiowrite(sda_gpio, 0); - up_udelay(10); - stm32_gpiowrite(scl_gpio, 0); - up_udelay(10); - stm32_gpiowrite(scl_gpio, 1); - up_udelay(10); - stm32_gpiowrite(sda_gpio, 1); - up_udelay(10); + /* Drive SCL high again */ - /* Revert the GPIO configuration. */ - - stm32_unconfiggpio(sda_gpio); - stm32_unconfiggpio(scl_gpio); + stm32_gpiowrite(priv->config->scl_gpio, 1); + up_udelay(10); + + } + + /* + * Generate a start followed by a stop to reset slave + * state machines. + */ + + stm32_gpiowrite(priv->config->sda_gpio, 0); + up_udelay(10); + stm32_gpiowrite(priv->config->scl_gpio, 0); + up_udelay(10); + stm32_gpiowrite(priv->config->scl_gpio, 1); + up_udelay(10); + stm32_gpiowrite(priv->config->sda_gpio, 1); + up_udelay(10); + + /* + * Revert the GPIO configuration. + */ + stm32_unconfiggpio(priv->config->sda_gpio); + stm32_unconfiggpio(priv->config->scl_gpio); + + } /* Re-init the port */ @@ -2078,11 +2063,11 @@ int up_i2creset(FAR struct i2c_dev_s * dev) out: - /* Release the port for re-use by other clients */ + /* release the port for re-use by other clients */ stm32_i2c_sem_post(dev); + return ret; } -#endif /* CONFIG_I2C_RESET */ -#endif /* CONFIG_STM32_I2C1 || CONFIG_STM32_I2C2 || CONFIG_STM32_I2C3 */ +#endif /* defined(CONFIG_STM32_I2C1) || defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) */ diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 7bb4d1003..0b27b552e 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -348,6 +348,16 @@ CONFIG_CAN_LOOPBACK=n CONFIG_CAN1_BAUD=700000 CONFIG_CAN2_BAUD=700000 + +# XXX remove after integration testing +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + # # I2C configuration # @@ -357,14 +367,32 @@ CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y + + +# XXX re-enable after integration testing + +# +# I2C configuration +# +#CONFIG_I2C=y +#CONFIG_I2C_POLLED=y +#CONFIG_I2C_TRANSFER=y +#CONFIG_I2C_TRACE=n +#CONFIG_I2C_RESET=y + # Dynamic timeout #CONFIG_STM32_I2C_DYNTIMEO=y #CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 #CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 # Fixed per-transaction timeout -CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 +#CONFIG_STM32_I2CTIMEOSEC=0 +#CONFIG_STM32_I2CTIMEOMS=10 + + + + + # # General build options |