diff options
author | px4dev <px4@purgatory.org> | 2013-07-14 13:04:44 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-07-14 13:04:44 -0700 |
commit | dca9019f75ddf31a5477db935252eb009a0be655 (patch) | |
tree | 6fd0661f9e2c7035097bf2b044b33e95ae352caf /src/drivers | |
parent | 6cf120831289368015b7b4f51db4f99f418e7129 (diff) | |
parent | 60ce9759d9d5a9b5f2e9fd218852fa595cc7bebd (diff) | |
download | px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.tar.gz px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.tar.bz2 px4-firmware-dca9019f75ddf31a5477db935252eb009a0be655.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup
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 | 12 | ||||
-rw-r--r-- | src/drivers/drv_pwm_output.h | 9 | ||||
-rw-r--r-- | src/drivers/gps/ubx.cpp | 843 | ||||
-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/px4fmu/fmu.cpp | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 137 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_i2c.cpp | 2 |
16 files changed, 1176 insertions, 662 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 5b06f91a8..c7c25048a 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -63,7 +63,8 @@ * 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 @@ -92,22 +93,17 @@ * 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 /* no GPIO driver on the PX4IOv1 board */ -# define GPIO_DEVICE_PATH "/nonexistent" #endif #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 /* no GPIO driver on the PX4IOv2 board */ -# define GPIO_DEVICE_PATH "/nonexistent" -#endif - -#ifndef GPIO_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 762c257aa..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)); - 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, - UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); - // /* 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, - 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_VELNED, - 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_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, - UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - - _waiting_for_ack = false; - return 0; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_NAV5; + + 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; } - 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; + } + + 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 @@ -231,48 +261,47 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool position_updated = false; + bool handled = false; while (true) { - /* 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)); - /* pass received bytes to the packet decoder */ + 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])) { + 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()) - position_updated = true; + if (handle_message() > 0) + handled = true; } } } } - /* return success after receiving a packet */ - if (position_updated) - return 1; - - /* abort after timeout if no packet parsed successfully */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + /* abort after timeout if no useful packets received */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } } @@ -283,406 +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; + 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; + 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 */ + } + + 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_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; - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + _message_id = b; + _decode_state = UBX_DECODE_GOT_MESSAGEID; + break; -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// 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_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: - { + 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); - 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; - 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; - } - // Evaluate state machine - if the state changed, - // the state machine was reset via decode_init() - // and we want to tell the module to stop sending this message + _rx_buffer[_rx_count] = b; - // disable unknown message - //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - //configure_message_rate(_message_class, b, 0); + /* 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 + + } else { + warnx("ubx: checksum wrong"); + decode_init(); + return -1; } - 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"); - } - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + + break; + + default: + break; } - return 0; //XXX ? + + return 0; // message decoding in progress } + int UBX::handle_message() { int ret = 0; - 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; + 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->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; + _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->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rate_count_lat_lon++; - _rate_count_lat_lon++; + ret = 1; + break; + } - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; - } - break; - } + 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; - 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; + _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(); - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + ret = 1; + break; + } - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } + 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; -// 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"); + /* 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); - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_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->timestamp_time = hrt_absolute_time(); - //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; + ret = 1; + break; + } - time_t epoch = mktime(&timeinfo); + 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; - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + uint8_t satellites_used = 0; + int i; - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } + //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]); - case NAV_SVINFO: { - // printf("GOT NAV_SVINFO MESSAGE\n"); + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; - 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; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - - //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 - - uint8_t satellites_used = 0; - int i; - // printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* 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; - - /* Write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if ( sv_used ) { - // Count SVs used for NAV. - satellites_used++; + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* 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); + } + + 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 + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; } - - // 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; - } - 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; + _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 @@ -692,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 @@ -705,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]; @@ -732,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 @@ -761,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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7928752d5..70147d56a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -195,7 +195,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 9f84c0950..e9f26b0d3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -132,6 +132,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: device::Device *_interface; @@ -179,6 +189,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[]); @@ -319,7 +335,7 @@ PX4IO *g_dev; } PX4IO::PX4IO(device::Device *interface) : - CDev("px4io", "/dev/px4io"), + CDev("px4io", PX4IO_DEVICE_PATH), _interface(interface), _hardware(0), _max_actuators(0), @@ -346,7 +362,8 @@ PX4IO::PX4IO(device::Device *interface) : _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; @@ -1264,13 +1281,14 @@ 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_ARMED) ? " ARMED" : ""), ((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" : ""), @@ -1373,7 +1391,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; @@ -1425,6 +1444,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 */ @@ -1467,18 +1506,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: @@ -1646,10 +1698,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; @@ -1658,7 +1765,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"); @@ -1895,7 +2002,6 @@ px4io_main(int argc, char *argv[]) * doesn't reference filp in ioctl() */ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); - exit(0); } @@ -1928,7 +2034,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); @@ -1950,6 +2056,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); -out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + 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', 'bind', or 'update'"); } diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index f561b4359..e0c3e5608 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -78,7 +78,7 @@ device::Device } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - I2C("PX4IO_i2c", nullptr, bus, address, 400000) + I2C("PX4IO_i2c", nullptr, bus, address, 320000) { _retries = 3; } |