diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/ardrone_interface/ardrone_motor_control.c | 2 | ||||
| -rw-r--r-- | src/drivers/drv_gpio.h | 37 | ||||
| -rw-r--r-- | src/drivers/drv_pwm_output.h | 9 | ||||
| -rw-r--r-- | src/drivers/gps/ubx.cpp | 842 | ||||
| -rw-r--r-- | src/drivers/gps/ubx.h | 61 | ||||
| -rw-r--r-- | src/drivers/hott/comms.cpp | 92 | ||||
| -rw-r--r-- | src/drivers/hott/comms.h | 46 | ||||
| -rw-r--r-- | src/drivers/hott/hott_sensors/hott_sensors.cpp | 238 | ||||
| -rw-r--r-- | src/drivers/hott/hott_sensors/module.mk | 42 | ||||
| -rw-r--r-- | src/drivers/hott/hott_telemetry/hott_telemetry.cpp (renamed from src/drivers/hott_telemetry/hott_telemetry_main.c) | 86 | ||||
| -rw-r--r-- | src/drivers/hott/hott_telemetry/module.mk (renamed from src/drivers/hott_telemetry/module.mk) | 7 | ||||
| -rw-r--r-- | src/drivers/hott/messages.cpp (renamed from src/drivers/hott_telemetry/messages.c) | 152 | ||||
| -rw-r--r-- | src/drivers/hott/messages.h (renamed from src/drivers/hott_telemetry/messages.h) | 107 | ||||
| -rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 125 | ||||
| -rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 30 | ||||
| -rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 2 | ||||
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 138 |
17 files changed, 1291 insertions, 725 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073..be8968a4e 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -109,7 +109,7 @@ int ar_multiplexing_init() { int fd; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { warn("GPIO: open fail"); diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 7e3998fca..510983d4b 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -63,43 +63,12 @@ * they also export GPIO-like things. This is always the GPIOs on the * main board. */ -# define GPIO_DEVICE_PATH "/dev/px4fmu" +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" - -#endif - -#ifndef GPIO_DEVICE_PATH +#ifndef PX4IO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 56af71059..52a667403 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm); /** clear the 'ARM ok' bit, which deactivates the safety switch */ #define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +/** start DSM bind */ +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) + +/** stop DSM bind */ +#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) + +/** Power up DSM receiver */ +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b579db715 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,13 +56,16 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 100 +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_waiting_for_ack(false), -_disable_cmd_counter(0) + _fd(fd), + _gps_position(gps_position), + _configured(false), + _waiting_for_ack(false), + _disable_cmd_counter(0) { decode_init(); } @@ -73,12 +77,13 @@ UBX::~UBX() int UBX::configure(unsigned &baudrate) { - _waiting_for_ack = true; - + _configured = false; /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < 5; baud_i++) { + int baud_i; + + for (baud_i = 0; baud_i < 5; baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); @@ -89,8 +94,8 @@ UBX::configure(unsigned &baudrate) /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_PRT; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_PRT; /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; @@ -102,9 +107,9 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -120,100 +125,125 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - receive(UBX_CONFIG_TIMEOUT); - + wait_for_ack(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + /* at this point we have correct baudrate on both ends */ + break; + } - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_RATE; + if (baud_i >= 5) { + return 1; + } - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + /* send a CFG-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_RATE; - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_NAV5; + send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: RATE"); + return 1; + } - send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_NAV5; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 1); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - 1); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - 1); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, - // 0); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - - _waiting_for_ack = false; - return 0; + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: NAV5"); + return 1; + } + + /* configure message rates */ + /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + return 1; } - return -1; + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SOL\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + return 1; + } + + _configured = true; + return 0; } int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - int ret = receive(timeout); - _waiting_for_ack = false; - return ret; + uint64_t time_started = hrt_absolute_time(); + + while (hrt_absolute_time() < time_started + timeout * 1000) { + if (receive(timeout) > 0) { + if (!_waiting_for_ack) { + return 1; + } + + } else { + return -1; // timeout or error receiving, or NAK + } + } + + return -1; // timeout } int @@ -224,56 +254,56 @@ UBX::receive(unsigned timeout) fds[0].fd = _fd; fds[0].events = POLLIN; - uint8_t buf[32]; + uint8_t buf[128]; /* timeout additional to poll */ uint64_t time_started = hrt_absolute_time(); - int j = 0; ssize_t count = 0; - while (true) { + bool handled = false; - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message() > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { - return -1; - } - j++; - } - - /* everything is read */ - j = count = 0; + while (true) { - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ return -1; } else if (ret == 0) { - /* Timeout */ - return -1; + /* return success after short delay after receiving a packet or timeout after long delay */ + return handled ? 1 : -1; } else if (ret > 0) { /* if we have new data from GPS, go handle it */ if (fds[0].revents & POLLIN) { /* * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If more bytes are available, we'll go back to poll() again. */ - count = ::read(_fd, buf, sizeof(buf)); + usleep(UBX_WAIT_BEFORE_READ * 1000); + count = read(_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ + for (int i = 0; i < count; i++) { + if (parse_char(buf[i]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + handled = true; + } + } } } + + /* abort after timeout if no useful packets received */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { + return -1; + } } } @@ -282,410 +312,292 @@ UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } - break; - /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - } else { - /* Second start symbol was wrong, reset state machine */ - decode_init(); - } - break; - /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ - add_byte_to_checksum(b); - /* check for known class */ - switch (b) { - case UBX_CLASS_ACK: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = ACK; - break; + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; + break; -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// break; + /* Second, look for sync2 */ + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + /* don't return error, it can be just false sync1 */ + } - case UBX_CLASS_CFG: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = CFG; - break; - default: //unknown class: reset state machine - decode_init(); - break; - } - break; - case UBX_DECODE_GOT_CLASS: - add_byte_to_checksum(b); - switch (_message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_POSLLH; - break; - - case UBX_MESSAGE_NAV_SOL: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SOL; - break; - - case UBX_MESSAGE_NAV_TIMEUTC: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_TIMEUTC; - break; - -// case UBX_MESSAGE_NAV_DOP: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = NAV_DOP; -// break; - - case UBX_MESSAGE_NAV_SVINFO: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; -// case RXM: -// switch (b) { -// case UBX_MESSAGE_RXM_SVSI: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = RXM_SVSI; -// break; -// -// default: //unknown class: reset state machine, should not happen -// decode_init(); -// break; -// } -// break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; + break; - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know"); - decode_init(); -// config_needed = true; - break; - } - break; - case UBX_DECODE_GOT_MESSAGEID: - add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - break; - case UBX_DECODE_GOT_LENGTH1: - add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - break; - case UBX_DECODE_GOT_LENGTH2: - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); - _rx_buffer[_rx_count] = b; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { - return 1; - } else { - decode_init(); - return -1; - warnx("ubx: Checksum wrong"); - } + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + _message_class = b; + _decode_state = UBX_DECODE_GOT_CLASS; + break; - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; - } - return 0; //XXX ? -} + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + _message_id = b; + _decode_state = UBX_DECODE_GOT_MESSAGEID; + break; + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; -int -UBX::handle_message() -{ - int ret = 0; + case UBX_DECODE_GOT_LENGTH1: + add_byte_to_checksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + case UBX_DECODE_GOT_LENGTH2: - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) + add_byte_to_checksum(b); - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rx_buffer[_rx_count] = b; - _rate_count_lat_lon++; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + decode_init(); + return 1; // message received successfully - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; + } else { + warnx("ubx: checksum wrong"); + decode_init(); + return -1; } - break; + + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; } - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + break; - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + default: + break; + } - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } + return 0; // message decoding in progress +} -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// _gps_position->eph_m = packet->hDOP; -// _gps_position->epv = packet->vDOP; -// -// _gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; +int +UBX::handle_message() +{ + int ret = 0; - time_t epoch = mktime(&timeinfo); + if (_configured) { + /* handle only info messages when configured */ + switch (_message_class) { + case UBX_CLASS_NAV: + switch (_message_id) { + case UBX_MESSAGE_NAV_POSLLH: { + // printf("GOT NAV_POSLLH\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->timestamp_position = hrt_absolute_time(); - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } + _rate_count_lat_lon++; - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); + ret = 1; + break; + } - if (!_waiting_for_ack) { - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + case UBX_MESSAGE_NAV_SOL: { + // printf("GOT NAV_SOL\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + _gps_position->timestamp_variance = hrt_absolute_time(); - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage + ret = 1; + break; + } - uint8_t satellites_used = 0; - int i; + case UBX_MESSAGE_NAV_TIMEUTC: { + // printf("GOT NAV_TIMEUTC\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - for (i = 0; i < packet_part1->numCh; i++) { //for each channel + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + _gps_position->timestamp_time = hrt_absolute_time(); + + ret = 1; + break; + } - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + uint8_t satellites_used = 0; + int i; - /* Write satellite information in the global storage */ - _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - _gps_position->satellite_used[i] = 1; + if (sv_used) { + /* count SVs used for NAV */ satellites_used++; - - } else { - _gps_position->satellite_used[i] = 0; } + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; _gps_position->satellite_snr[i] = packet_part2->cno; _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); + } - } else { + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; _gps_position->satellite_used[i] = 0; _gps_position->satellite_snr[i] = 0; _gps_position->satellite_elevation[i] = 0; _gps_position->satellite_azimuth[i] = 0; } - } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - } else { - _gps_position->satellite_info_available = false; + case UBX_MESSAGE_NAV_VELNED: { + // printf("GOT NAV_VELNED\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; + + ret = 1; + break; } - _gps_position->timestamp_satellites = hrt_absolute_time(); + + default: + break; } break; - } - case NAV_VELNED: { - - if (!_waiting_for_ack) { - /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); - - _rate_count_vel++; + case UBX_CLASS_ACK: { + /* ignore ACK when already configured */ + ret = 1; + break; } + default: break; } -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; -// -// _gps_position->satellites_visible = packet->numVis; -// _gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// -// break; -// } - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { - ret = 1; - } + if (ret == 0) { + /* message not handled */ + warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + + if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + /* don't attempt for every message to disable, some might not be disabled */ + warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + configure_message_rate(_message_class, _message_id, 0); } } - break; - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - ret = -1; - break; - } + } else { + /* handle only ACK while configuring */ + if (_message_class == UBX_CLASS_ACK) { + switch (_message_id) { + case UBX_MESSAGE_ACK_ACK: { + // printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { + _waiting_for_ack = false; + ret = 1; + } + } - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - if (_disable_cmd_counter++ == 0) { - // Don't attempt for every message to disable, some might not be disabled */ - warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); + break; + } + + case UBX_MESSAGE_ACK_NAK: { + // printf("GOT ACK_NAK\n"); + warnx("ubx: not acknowledged"); + /* configuration obviously not successful */ + _waiting_for_ack = false; + ret = -1; + break; + } + + default: + break; } - return ret; - ret = -1; - break; } - // end if _rx_count high enough + } + decode_init(); - return ret; //XXX? + return ret; } void @@ -695,9 +607,8 @@ UBX::decode_init(void) _rx_ck_b = 0; _rx_count = 0; _decode_state = UBX_DECODE_UNINIT; - _message_class = CLASS_UNKNOWN; - _message_id = ID_UNKNOWN; _payload_size = 0; + /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ } void @@ -708,23 +619,24 @@ UBX::add_byte_to_checksum(uint8_t b) } void -UBX::add_checksum_to_message(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t *message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; unsigned i; - for (i = 0; i < length-2; i++) { + for (i = 0; i < length - 2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ - message[length-2] = ck_a; - message[length-1] = ck_b; + message[length - 2] = ck_a; + message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) { for (unsigned i = 0; i < length; i++) { ck_a = ck_a + message[i]; @@ -735,11 +647,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_ void UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void @@ -764,19 +676,19 @@ void UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) { struct ubx_header header; - uint8_t ck_a=0, ck_b=0; + uint8_t ck_a = 0, ck_b = 0; header.sync1 = UBX_SYNC1; header.sync2 = UBX_SYNC2; header.msg_class = msg_class; header.msg_id = msg_id; header.length = size; - add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); // Configure receive check - _clsID_needed = msg_class; - _msgID_needed = msg_id; + _message_class_needed = msg_class; + _message_id_needed = msg_id; write(_fd, (const char *)&header, sizeof(header)); write(_fd, (const char *)msg, size); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5a433642c..4fc276975 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -3,6 +3,7 @@ * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,23 +52,23 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 //#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 +#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ @@ -299,44 +300,6 @@ struct ubx_cfg_msg_rate { // ************ typedef enum { - UBX_CONFIG_STATE_PRT = 0, - UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, - UBX_CONFIG_STATE_RATE, - UBX_CONFIG_STATE_NAV5, - UBX_CONFIG_STATE_MSG_NAV_POSLLH, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, - UBX_CONFIG_STATE_MSG_NAV_DOP, - UBX_CONFIG_STATE_MSG_NAV_SVINFO, - UBX_CONFIG_STATE_MSG_NAV_SOL, - UBX_CONFIG_STATE_MSG_NAV_VELNED, -// UBX_CONFIG_STATE_MSG_RXM_SVSI, - UBX_CONFIG_STATE_CONFIGURED -} ubx_config_state_t; - -typedef enum { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -} ubx_message_class_t; - -typedef enum { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH, - NAV_SOL, - NAV_TIMEUTC, -// NAV_DOP, - NAV_SVINFO, - NAV_VELNED, -// RXM_SVSI, - CFG_NAV5, - ACK_ACK, - ACK_NAK, -} ubx_message_id_t; - -typedef enum { UBX_DECODE_UNINIT = 0, UBX_DECODE_GOT_SYNC1, UBX_DECODE_GOT_SYNC2, @@ -401,17 +364,17 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; - ubx_config_state_t _config_state; + bool _configured; bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + uint8_t _message_class_needed; + uint8_t _message_id_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - ubx_message_class_t _message_class; - ubx_message_id_t _message_id; + uint8_t _message_class; + uint8_t _message_id; unsigned _payload_size; uint8_t _disable_cmd_counter; }; diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp new file mode 100644 index 000000000..cb8bbba37 --- /dev/null +++ b/src/drivers/hott/comms.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file comms.c + * @author Simon Wilks <sjwilks@gmail.com> + * + */ + +#include "comms.h" + +#include <fcntl.h> +#include <stdio.h> +#include <sys/ioctl.h> +#include <systemlib/err.h> +#include <termios.h> + +int +open_uart(const char *device) +{ + /* baud rate */ + static const speed_t speed = B19200; + + /* open uart */ + const int uart = open(device, O_RDWR | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", device); + } + + /* Back up the original uart configuration to restore it after exit */ + int termios_state; + struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { + close(uart); + err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + device, termios_state); + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + } + + /* Activate single wire mode */ + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + + return uart; +} diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h new file mode 100644 index 000000000..f5608122f --- /dev/null +++ b/src/drivers/hott/comms.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file comms.h + * @author Simon Wilks <sjwilks@gmail.com> + * + */ + + +#ifndef COMMS_H_ +#define COMMS_H + +int open_uart(const char *device); + +#endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp new file mode 100644 index 000000000..e322c6349 --- /dev/null +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hott_sensors.c + * @author Simon Wilks <sjwilks@gmail.com> + * + * Graupner HoTT sensor driver implementation. + * + * Poll any sensors connected to the PX4 via the telemetry wire. + */ + +#include <fcntl.h> +#include <nuttx/config.h> +#include <poll.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <sys/ioctl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include "../comms.h" +#include "../messages.h" + +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static const char daemon_name[] = "hott_sensors"; +static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]"; + +/** + * Deamon management function. + */ +extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int hott_sensors_thread_main(int argc, char *argv[]); + +static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id); +static int send_poll(int uart, uint8_t *buffer, size_t size); + +int +send_poll(int uart, uint8_t *buffer, size_t size) +{ + for (size_t i = 0; i < size; i++) { + write(uart, &buffer[i], sizeof(buffer[i])); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) +{ + static const int timeout_ms = 1000; + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; + + // XXX should this poll be inside the while loop??? + if (poll(&fds, 1, timeout_ms) > 0) { + int i = 0; + bool stop_byte_read = false; + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + + if (stop_byte_read) { + // XXX process checksum + *size = ++i; + return OK; + } + // XXX can some other field not have the STOP BYTE value? + if (buffer[i] == STOP_BYTE) { + *id = buffer[1]; + stop_byte_read = true; + } + i++; + } + } + return ERROR; +} + +int +hott_sensors_thread_main(int argc, char *argv[]) +{ + warnx("starting"); + + thread_running = true; + + const char *device = DEFAULT_UART; + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -d\n%s", commandline_usage); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + const int uart = open_uart(device); + if (uart < 0) { + errx(1, "Failed opening HoTT UART, exiting."); + thread_running = false; + } + + init_pub_messages(); + + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; + size_t size = 0; + uint8_t id = 0; + while (!thread_should_exit) { + // Currently we only support a General Air Module sensor. + build_gam_request(&buffer[0], &size); + send_poll(uart, buffer, size); + + // The sensor will need a little time before it starts sending. + usleep(5000); + + recv_data(uart, &buffer[0], &size, &id); + + // Determine which moduel sent it and process accordingly. + if (id == GAM_SENSOR_ID) { + publish_gam_message(buffer); + } else { + warnx("Unknown sensor ID: %d", id); + } + } + + warnx("exiting"); + close(uart); + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and start the daemon. + */ +int +hott_sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 1024, + hott_sensors_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("daemon is running"); + + } else { + warnx("daemon not started"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk new file mode 100644 index 000000000..b5f5762ba --- /dev/null +++ b/src/drivers/hott/hott_sensors/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Graupner HoTT Sensors application. +# + +MODULE_COMMAND = hott_sensors + +SRCS = hott_sensors.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 4699ce5bf..042d9f816 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -41,7 +41,6 @@ * The HoTT receiver polls each device at a regular interval at which point * a data packet can be returned if necessary. * - * TODO: Add support for at least the vario and GPS sensor data. */ #include <fcntl.h> @@ -50,13 +49,21 @@ #include <stdlib.h> #include <stdio.h> #include <string.h> -#include <termios.h> #include <sys/ioctl.h> #include <unistd.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> -#include "messages.h" +#include "../comms.h" +#include "../messages.h" + +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ @@ -67,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop /** * Deamon management function. */ -__EXPORT int hott_telemetry_main(int argc, char *argv[]); +extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]); /** * Mainloop of daemon. @@ -77,60 +84,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]); static int recv_req_id(int uart, uint8_t *id); static int send_data(int uart, uint8_t *buffer, size_t size); -static int -open_uart(const char *device, struct termios *uart_config_original) -{ - /* baud rate */ - static const speed_t speed = B19200; - - /* open uart */ - const int uart = open(device, O_RDWR | O_NOCTTY); - - if (uart < 0) { - err(1, "Error opening port: %s", device); - } - - /* Back up the original uart configuration to restore it after exit */ - int termios_state; - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); - } - - /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); - } - - /* Activate single wire mode */ - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - - return uart; -} - int recv_req_id(int uart, uint8_t *id) { static const int timeout_ms = 1000; // TODO make it a define - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; uint8_t mode; + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; - if (poll(fds, 1, timeout_ms) > 0) { + if (poll(&fds, 1, timeout_ms) > 0) { /* Get the mode: binary or text */ read(uart, &mode, sizeof(mode)); @@ -155,7 +120,6 @@ send_data(int uart, uint8_t *buffer, size_t size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; - for (size_t i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -186,7 +150,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) thread_running = true; - const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ + const char *device = DEFAULT_UART; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { @@ -202,22 +166,20 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } - messages_init(); + init_sub_messages(); - uint8_t buffer[MESSAGE_BUFFER_SIZE]; + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; bool connected = true; - while (!thread_should_exit) { + // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { if (!connected) { connected = true; @@ -228,7 +190,9 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; - + case GAM_SENSOR_ID: + build_gam_response(buffer, &size); + break; case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; @@ -254,7 +218,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /** - * Process command line arguments and tart the daemon. + * Process command line arguments and start the daemon. */ int hott_telemetry_main(int argc, char *argv[]) diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index def1d59e9..b19cbd14c 100644 --- a/src/drivers/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -32,10 +32,11 @@ ############################################################################ # -# Graupner HoTT Telemetry application. +# Graupner HoTT Telemetry applications. # MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry_main.c \ - messages.c +SRCS = hott_telemetry.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott/messages.cpp index 0ce56acef..57c256339 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott/messages.cpp @@ -46,6 +46,7 @@ #include <unistd.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/esc_status.h> #include <uORB/topics/home_position.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> @@ -53,24 +54,75 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -static int battery_sub = -1; -static int gps_sub = -1; -static int home_sub = -1; -static int sensor_sub = -1; -static int airspeed_sub = -1; +static int _battery_sub = -1; +static int _gps_sub = -1; +static int _home_sub = -1; +static int _sensor_sub = -1; +static int _airspeed_sub = -1; +static int _esc_sub = -1; -static bool home_position_set = false; -static double home_lat = 0.0d; -static double home_lon = 0.0d; +static orb_advert_t _esc_pub; +struct esc_status_s _esc; + +static bool _home_position_set = false; +static double _home_lat = 0.0d; +static double _home_lon = 0.0d; void -messages_init(void) +init_sub_messages(void) { - battery_sub = orb_subscribe(ORB_ID(battery_status)); - gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - home_sub = orb_subscribe(ORB_ID(home_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _home_sub = orb_subscribe(ORB_ID(home_position)); + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _esc_sub = orb_subscribe(ORB_ID(esc_status)); +} + +void +init_pub_messages(void) +{ + memset(&_esc, 0, sizeof(_esc)); + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); +} + +void +build_gam_request(uint8_t *buffer, size_t *size) +{ + struct gam_module_poll_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.mode = BINARY_MODE_REQUEST_ID; + msg.id = GAM_SENSOR_ID; + + memcpy(buffer, &msg, *size); +} + +void +publish_gam_message(const uint8_t *buffer) +{ + struct gam_module_msg msg; + size_t size = sizeof(msg); + memset(&msg, 0, size); + memcpy(&msg, buffer, size); + + /* announce the esc if needed, just publish else */ + if (_esc_pub > 0) { + orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + } else { + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + } + + // Publish it. + _esc.esc_count = 1; + _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + _esc.esc[0].esc_temperature = msg.temperature1 - 20; + _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); } void @@ -79,12 +131,12 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID(battery_status), _battery_sub, &battery); struct eam_module_msg msg; *size = sizeof(msg); @@ -92,7 +144,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; - msg.sensor_id = EAM_SENSOR_TEXT_ID; + msg.sensor_text_id = EAM_SENSOR_TEXT_ID; msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -106,12 +158,46 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the airspeed data */ struct airspeed_s airspeed; memset(&airspeed, 0, sizeof(airspeed)); - orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; - + + msg.stop = STOP_BYTE; + memcpy(buffer, &msg, *size); +} + +void +build_gam_response(uint8_t *buffer, size_t *size) +{ + /* get a local copy of the ESC Status values */ + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), _esc_sub, &esc); + + struct gam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.gam_sensor_id = GAM_SENSOR_ID; + msg.sensor_text_id = GAM_SENSOR_TEXT_ID; + + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature2 = 20; // 0 deg. C. + + uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + msg.main_voltage_L = (uint8_t)voltage & 0xff; + msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; + + uint16_t current = (uint16_t)(esc.esc[0].esc_current); + msg.current_L = (uint8_t)current & 0xff; + msg.current_H = (uint8_t)(current >> 8) & 0xff; + + uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + msg.rpm_L = (uint8_t)rpm & 0xff; + msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); @@ -123,14 +209,14 @@ build_gps_response(uint8_t *buffer, size_t *size) /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); - struct gps_module_msg msg = { 0 }; + struct gps_module_msg msg; *size = sizeof(msg); memset(&msg, 0, *size); @@ -150,7 +236,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); /* GPS speed */ - uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); + uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; @@ -196,33 +282,33 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); + uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - /* Get any (and probably only ever one) home_sub postion report */ + /* Get any (and probably only ever one) _home_sub postion report */ bool updated; - orb_check(home_sub, &updated); + orb_check(_home_sub, &updated); if (updated) { /* get a local copy of the home position data */ struct home_position_s home; memset(&home, 0, sizeof(home)); - orb_copy(ORB_ID(home_position), home_sub, &home); + orb_copy(ORB_ID(home_position), _home_sub, &home); - home_lat = ((double)(home.lat))*1e-7d; - home_lon = ((double)(home.lon))*1e-7d; - home_position_set = true; + _home_lat = ((double)(home.lat))*1e-7d; + _home_lon = ((double)(home.lon))*1e-7d; + _home_position_set = true; } /* Distance from home */ - if (home_position_set) { - uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + if (_home_position_set) { + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon); msg.distance_L = (uint8_t)dist & 0xff; msg.distance_H = (uint8_t)(dist >> 8) & 0xff; /* Direction back to home */ - uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); + uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F); msg.home_direction = (uint8_t)bearing >> 1; } } diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott/messages.h index e6d5cc723..451bee91c 100644 --- a/src/drivers/hott_telemetry/messages.h +++ b/src/drivers/hott/messages.h @@ -60,19 +60,25 @@ #define STOP_BYTE 0x7d #define TEMP_ZERO_CELSIUS 0x14 +/* The GAM Module poll message. */ +struct gam_module_poll_msg { + uint8_t mode; + uint8_t id; +}; + /* Electric Air Module (EAM) constants. */ #define EAM_SENSOR_ID 0x8e #define EAM_SENSOR_TEXT_ID 0xe0 /* The Electric Air Module message. */ struct eam_module_msg { - uint8_t start; /**< Start byte */ + uint8_t start; /**< Start byte */ uint8_t eam_sensor_id; /**< EAM sensor */ uint8_t warning; - uint8_t sensor_id; /**< Sensor ID, why different? */ + uint8_t sensor_text_id; uint8_t alarm_inverse1; uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ uint8_t cell2_L; uint8_t cell3_L; uint8_t cell4_L; @@ -92,9 +98,9 @@ struct eam_module_msg { uint8_t batt2_voltage_H; uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ uint8_t current_H; uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ uint8_t main_voltage_H; @@ -103,35 +109,82 @@ struct eam_module_msg { uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ uint8_t climbrate_H; uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ uint8_t rpm_H; uint8_t electric_min; /**< Flight time in minutes. */ uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ }; -/** - * The maximum buffer size required to store a HoTT message. - */ -#define MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ -}) + +/* General Air Module (GAM) constants. */ +#define GAM_SENSOR_ID 0x8d +#define GAM_SENSOR_TEXT_ID 0xd0 + +struct gam_module_msg { + uint8_t start; // start byte constant value 0x7c + uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d + uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm + uint8_t sensor_text_id; // constant value 0xd0 + uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted + uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted + uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; // battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V + uint8_t batt1_H; + uint8_t batt2_L; // battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V + uint8_t batt2_H; + uint8_t temperature1; // temperature 1. offset of 20. a value of 20 = 0°C + uint8_t temperature2; // temperature 2. offset of 20. a value of 20 = 0°C + uint8_t fuel_procent; // Fuel capacity in %. Values 0--100 + // graphical display ranges: 0-25% 50% 75% 100% + uint8_t fuel_ml_L; // Fuel in ml scale. Full = 65535! + uint8_t fuel_ml_H; // + uint8_t rpm_L; // RPM in 10 RPM steps. 300 = 3000rpm + uint8_t rpm_H; // + uint8_t altitude_L; // altitude in meters. offset of 500, 500 = 0m + uint8_t altitude_H; // + uint8_t climbrate_L; // climb rate in 0.01m/s. Value of 30000 = 0.00 m/s + uint8_t climbrate_H; // + uint8_t climbrate3s; // climb rate in m/3sec. Value of 120 = 0m/3sec + uint8_t current_L; // current in 0.1A steps + uint8_t current_H; // + uint8_t main_voltage_L; // Main power voltage using 0.1V steps + uint8_t main_voltage_H; // + uint8_t batt_cap_L; // used battery capacity in 10mAh steps + uint8_t batt_cap_H; // + uint8_t speed_L; // (air?) speed in km/h(?) we are using ground speed here per default + uint8_t speed_H; // + uint8_t min_cell_volt; // minimum cell voltage in 2mV steps. 124 = 2,48V + uint8_t min_cell_volt_num; // number of the cell with the lowest voltage + uint8_t rpm2_L; // RPM in 10 RPM steps. 300 = 3000rpm + uint8_t rpm2_H; // + uint8_t general_error_number; // Voice error == 12. TODO: more docu + uint8_t pressure; // Pressure up to 16bar. 0,1bar scale. 20 = 2bar + uint8_t version; // version number TODO: more info? + uint8_t stop; // stop byte + uint8_t checksum; // checksum +}; /* GPS sensor constants. */ -#define GPS_SENSOR_ID 0x8A -#define GPS_SENSOR_TEXT_ID 0xA0 +#define GPS_SENSOR_ID 0x8a +#define GPS_SENSOR_TEXT_ID 0xa0 /** * The GPS sensor message * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ struct gps_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; /**< GPS sensor ID*/ - uint8_t warning; /**< Byte 3: 0…= warning beeps */ + uint8_t start; /**< Start byte */ + uint8_t sensor_id; /**< GPS sensor ID*/ + uint8_t warning; /**< Byte 3: 0…= warning beeps */ uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ @@ -179,15 +232,15 @@ struct gps_module_msg { uint8_t checksum; /**< Byte 45: Parity Byte */ }; -/** - * The maximum buffer size required to store a HoTT message. - */ -#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ -}) +// The maximum size of a message. +#define MAX_MESSAGE_BUFFER_SIZE 45 -void messages_init(void); +void init_sub_messages(void); +void init_pub_messages(void); +void build_gam_request(uint8_t *buffer, size_t *size); +void publish_gam_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); +void build_gam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index f187f22a6..3fc1054e6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -75,6 +75,7 @@ #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_safety.h> +#include <uORB/topics/esc_status.h> #include <systemlib/err.h> @@ -88,7 +89,7 @@ #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 2500 - +#define ESC_UORB_PUBLISH_DELAY 200 class MK : public device::I2C { @@ -120,6 +121,7 @@ public: int set_pwm_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); + int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); @@ -137,11 +139,15 @@ private: unsigned int _motor; int _px4mode; int _frametype; + orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; + orb_advert_t _t_esc_status; + unsigned int _num_outputs; bool _primary_pwm_device; bool _motortest; + bool _overrideSecurityChecks; volatile bool _task_should_exit; bool _armed; @@ -215,6 +221,7 @@ struct MotorData_t { unsigned int Version; // the version of the BL (0 = old) unsigned int SetPoint; // written by attitude controller unsigned int SetPointLowerBits; // for higher Resolution of new BLs + float SetPoint_PX4; // Values from PX4 unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present unsigned int ReadMode; // select data to read unsigned short RawPwmValue; // length of PWM pulse @@ -244,8 +251,10 @@ MK::MK(int bus) : _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), + _t_esc_status(0), _num_outputs(0), _motortest(false), + _overrideSecurityChecks(false), _motor(-1), _px4mode(MAPPING_MK), _frametype(FRAME_PLUS), @@ -465,6 +474,13 @@ MK::set_motor_test(bool motortest) return OK; } +int +MK::set_overrideSecurityChecks(bool overrideSecurityChecks) +{ + _overrideSecurityChecks = overrideSecurityChecks; + return OK; +} + short MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) { @@ -507,8 +523,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - - /* advertise the effective control inputs */ actuator_controls_effective_s controls_effective; memset(&controls_effective, 0, sizeof(controls_effective)); @@ -516,6 +530,12 @@ MK::task_main() _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), &controls_effective); + /* advertise the blctrl status */ + esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + _t_esc_status = orb_advertise(ORB_ID(esc_status), &esc); + + pollfd fds[2]; fds[0].fd = _t_actuators; @@ -603,9 +623,11 @@ MK::task_main() } } - /* don't go under BLCTRL_MIN_VALUE */ - if (outputs.output[i] < BLCTRL_MIN_VALUE) { - outputs.output[i] = BLCTRL_MIN_VALUE; + if(!_overrideSecurityChecks) { + /* don't go under BLCTRL_MIN_VALUE */ + if (outputs.output[i] < BLCTRL_MIN_VALUE) { + outputs.output[i] = BLCTRL_MIN_VALUE; + } } /* output to BLCtrl's */ @@ -613,7 +635,10 @@ MK::task_main() mk_servo_test(i); } else { - mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine + //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine + // 11 Bit + Motor[i].SetPoint_PX4 = outputs.output[i]; + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine } } @@ -636,8 +661,43 @@ MK::task_main() } + + /* + * Only update esc topic every half second. + */ + + if (hrt_absolute_time() - esc.timestamp > 500000) { + esc.counter++; + esc.timestamp = hrt_absolute_time(); + esc.esc_count = (uint8_t) _num_outputs; + esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C; + + for (unsigned int i = 0; i < _num_outputs; i++) { + esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; + esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; + esc.esc[i].esc_version = (uint16_t) Motor[i].Version; + esc.esc[i].esc_voltage = (uint16_t) 0; + esc.esc[i].esc_current = (uint16_t) Motor[i].Current; + esc.esc[i].esc_rpm = (uint16_t) 0; + esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; + if (Motor[i].Version == 1) { + // BLCtrl 2.0 (11Bit) + esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits; + } else { + // BLCtrl < 2.0 (8Bit) + esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; + } + esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; + esc.esc[i].esc_state = (uint16_t) Motor[i].State; + esc.esc[i].esc_errorcount = (uint16_t) 0; + } + + orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } + } + //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); ::close(_t_actuator_safety); @@ -716,17 +776,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput) fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { - _task_should_exit = true; + + if(!_overrideSecurityChecks) { + if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { + _task_should_exit = true; + } } } return foundMotorCount; } - - - int MK::mk_servo_set(unsigned int chan, short val) { @@ -739,17 +799,15 @@ MK::mk_servo_set(unsigned int chan, short val) tmpVal = val; - if (tmpVal > 1024) { - tmpVal = 1024; + if (tmpVal > 2047) { + tmpVal = 2047; } else if (tmpVal < 0) { tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - //Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4; - - Motor[chan].SetPointLowerBits = 0; + Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff; + Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07; if (_armed == false) { Motor[chan].SetPoint = 0; @@ -1015,8 +1073,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; - mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024)); - + mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); } else { ret = -EINVAL; } @@ -1113,25 +1170,19 @@ ssize_t MK::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - // loeschen uint16_t values[4]; uint16_t values[8]; - // loeschen if (count > 4) { - // loeschen // we only have 4 PWM outputs on the FMU - // loeschen count = 4; - // loeschen } if (count > _num_outputs) { // we only have 8 I2C outputs in the driver count = _num_outputs; } - // allow for misaligned values memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { Motor[i].RawPwmValue = (unsigned short)values[i]; - mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024)); + mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047)); } return count * 2; @@ -1283,7 +1334,7 @@ enum FrameType { PortMode g_port_mode; int -mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) +mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { uint32_t gpio_bits; int shouldStop = 0; @@ -1342,6 +1393,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* motortest if enabled */ g_mk->set_motor_test(motortest); + /* ovveride security checks if enabled */ + g_mk->set_overrideSecurityChecks(overrideSecurityChecks); + /* count used motors */ do { @@ -1407,6 +1461,7 @@ mkblctrl_main(int argc, char *argv[]) int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; + bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; @@ -1462,11 +1517,21 @@ mkblctrl_main(int argc, char *argv[]) showHelp = true; } + /* look for the optional --override-security-checks parameter */ + if (strcmp(argv[i], "--override-security-checks") == 0) { + overrideSecurityChecks = true; + newMode = true; + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); + fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); exit(1); } @@ -1484,7 +1549,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype); + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); } /* test, etc. here g*/ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index df1958186..220842536 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -272,6 +272,11 @@ private: */ void _set_dlpf_filter(uint16_t frequency_hz); + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(uint16_t desired_sample_rate_hz); + }; /** @@ -378,7 +383,8 @@ MPU6000::init() up_udelay(1000); // SAMPLE RATE - write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + _set_sample_rate(200); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) @@ -494,6 +500,18 @@ MPU6000::probe() } /* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro +*/ +void +MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +{ + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_reg(MPUREG_SMPLRT_DIV, div-1); +} + +/* set the DLPF filter frequency. This affects both accel and gyro. */ void @@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: @@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 05951f74c..d0499d2fd 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -167,7 +167,7 @@ PX4FMU *g_fmu; } // namespace PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), + CDev("fmuservo", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), _pwm_alt_rate(50), diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df2f18270..94f231821 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -145,6 +145,16 @@ public: */ void print_status(); + inline void set_dsm_vcc_ctl(bool enable) + { + _dsm_vcc_ctl = enable; + }; + + inline bool get_dsm_vcc_ctl() + { + return _dsm_vcc_ctl; + }; + private: // XXX unsigned _max_actuators; @@ -190,6 +200,12 @@ private: uint64_t _battery_last_timestamp; /** + * Relay1 is dedicated to controlling DSM receiver power + */ + + bool _dsm_vcc_ctl; + + /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); @@ -330,7 +346,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -356,7 +372,8 @@ PX4IO::PX4IO() : _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _dsm_vcc_ctl(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -762,8 +779,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1399,14 +1414,15 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), @@ -1506,7 +1522,8 @@ PX4IO::print_status() } int -PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) +/* Make it obvious that file * isn't used here */ { int ret = OK; @@ -1558,6 +1575,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case DSM_BIND_START: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + usleep(1000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(100000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + break; + + case DSM_BIND_STOP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + usleep(500000); + break; + + case DSM_BIND_POWER_UP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { /* TODO: we could go lower for e.g. TurboPWM */ @@ -1600,18 +1637,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case GPIO_RESET: - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); + case GPIO_RESET: { + uint32_t bits = (1 << _max_relays) - 1; + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_RELAY1; + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; + } case GPIO_SET: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: @@ -1748,10 +1798,65 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + int dsm_vcc_ctl; + + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { + if (dsm_vcc_ctl) { + g_dev->set_dsm_vcc_ctl(true); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); + } + } exit(0); } void +bind(int argc, char *argv[]) +{ + int pulses; + + if (g_dev == nullptr) + errx(1, "px4io must be started first"); + + if (!g_dev->get_dsm_vcc_ctl()) + errx(1, "DSM bind feature not enabled"); + + if (argc < 3) + errx(0, "needs argument, use dsm2 or dsmx"); + + if (!strcmp(argv[2], "dsm2")) + pulses = 3; + else if (!strcmp(argv[2], "dsmx")) + pulses = 7; + else + errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + errx(1, "failed opening console"); + + warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); + warnx("Press CTRL-C or 'c' when done."); + + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); + + for (;;) { + usleep(500000L); + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("Done\n"); + g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); + close(console); + exit(0); + } + } + } +} + +void test(void) { int fd; @@ -1760,7 +1865,7 @@ test(void) int direction = 1; int ret; - fd = open("/dev/px4io", O_WRONLY); + fd = open(PX4IO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); @@ -2044,7 +2149,7 @@ px4io_main(int argc, char *argv[]) * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1); } else { errx(1, "not loaded"); } @@ -2088,7 +2193,7 @@ px4io_main(int argc, char *argv[]) /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); + int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); @@ -2162,6 +2267,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); + if (!strcmp(argv[1], "bind")) + bind(argc, argv); + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); } |