From 496127ca459c603e5de3f8bc83c6113bcf9cbead Mon Sep 17 00:00:00 2001 From: sergeil Date: Fri, 31 May 2013 11:44:20 +0200 Subject: mpu6000 driver support for setting rate --- src/drivers/mpu6000/mpu6000.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) (limited to 'src') 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) @@ -493,6 +499,18 @@ MPU6000::probe() return -EIO; } +/* + 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. */ @@ -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: -- cgit v1.2.3 From 209dc7100e03c257a88d3c71221f136295d61929 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 2 Jul 2013 19:46:15 +0200 Subject: mkclctrl 8/11Bit support, uOrb Topic added, ESC Topic to Sdlog2 added --- src/drivers/mkblctrl/mkblctrl.cpp | 125 ++++++++++++++++++++++++++--------- src/modules/sdlog2/sdlog2.c | 33 ++++++++- src/modules/sdlog2/sdlog2_messages.h | 18 +++++ src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/esc_status.h | 114 ++++++++++++++++++++++++++++++++ 5 files changed, 262 insertions(+), 31 deletions(-) create mode 100644 src/modules/uORB/topics/esc_status.h (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..d0a0829b0 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -87,7 +88,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 { @@ -119,6 +120,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); @@ -136,11 +138,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; @@ -214,6 +220,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 @@ -243,8 +250,10 @@ MK::MK(int bus) : _t_armed(-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), @@ -464,6 +473,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) { @@ -506,8 +522,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)); @@ -515,6 +529,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; @@ -602,9 +622,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 */ @@ -612,7 +634,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 } } @@ -635,8 +660,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_armed); @@ -715,17 +775,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) { @@ -738,17 +798,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; @@ -1014,8 +1072,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; } @@ -1112,25 +1169,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; @@ -1282,7 +1333,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; @@ -1341,6 +1392,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 { @@ -1406,6 +1460,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; @@ -1461,11 +1516,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); } @@ -1483,7 +1548,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/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4cca46b9c..5cc80ead6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -79,6 +79,7 @@ #include #include #include +#include #include @@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 17; + const ssize_t fdsc = 18; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct rc_channels_s rc; struct differential_pressure_s diff_pres; struct airspeed_s airspeed; + struct esc_status_s esc; } buf; memset(&buf, 0, sizeof(buf)); @@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int flow_sub; int rc_sub; int airspeed_sub; + int esc_sub; } subs; /* log message buffer: header + body */ @@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; + struct log_ESC_s log_ESC; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- ESCs --- */ + subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); + fds[fdsc_count].fd = subs.esc_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1105,6 +1115,27 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(AIRS); } + /* --- ESCs --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); + for (uint8_t i=0; i + * + * 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 esc_status.h + * Definition of the esc_status uORB topic. + * + * Published the state machine and the system status bitfields + * (see SYS_STATUS mavlink message), published only by commander app. + * + * All apps should write to subsystem_info: + * + * (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app) + */ + +#ifndef ESC_STATUS_H_ +#define ESC_STATUS_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + +/** + * The number of ESCs supported. + * Current (Q2/2013) we support 8 ESCs, + */ +#define CONNECTED_ESC_MAX 8 + +enum ESC_VENDOR { + ESC_VENDOR_GENERIC = 0, /**< generic ESC */ + ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */ +}; + +enum ESC_CONNECTION_TYPE { + ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */ + ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */ + ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */ + ESC_CONNECTION_TYPE_I2C, /**< I2C */ + ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */ +}; + +/** + * + */ +struct esc_status_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + uint8_t esc_count; /**< number of connected ESCs */ + enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ + + struct { + uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ + enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ + uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ + uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ + uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ + uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + } esc[CONNECTED_ESC_MAX]; + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +//ORB_DECLARE(esc_status); +ORB_DECLARE_OPTIONAL(esc_status); + +#endif -- cgit v1.2.3 From 3f9f2018e20f4be23e7d62571ec0a3678d960108 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 5 Jul 2013 20:51:29 -0400 Subject: Support binding DSM2 and DSMX satellite receivers The px4io bind command allows you to put a DSM satellite receiver into bind mode. Since this feature requires that the dsm VCC line (red wire) be cut and routed through relay one, it is not enabled by default in order not to affect those not using a DSM satellite receiver or wising to use relay one for other purposes. NOTE: Binding DSM2 satellites in 11-bit mode is not supported due to potential bug in some DSM2 receiver streams when in 11-bit mode. Furthermore the px4io software folds 11 bit data down to 10 bits so there is no resolution advantage to to 11-bit mode. To enable the feature the RC_RL1_DSM_VCC parameter must be set to a non zero value from the console, or using QGroundControl: param set RC_RL1_DSM_VCC 1 From the console you can initiate DSM bind mode with: uorb start param set RC_RL1_DSM_VCC 1 px4io start px4io bind dsm2 For binding a DSMX satellite to a DSMX transmitter you would instead use: px4io bind dsmx Your receiver module should start a rapid flash and you can follow the normal binding sequence of your transmitter. Note: The value of parameter RC_RL1_DSM_VCC defaults to 0, so none of this will have any effect on an unmodified DSM receiver connection. For this feature to work, the power wire (red) must be cut and each side connected to a terminal on relay1 of the px4io board. This has been tested using Spektrum as well as Hobby King 'Orange' DSM satellite receivers. Both px4fmu and px4io images are updated. --- src/drivers/drv_pwm_output.h | 9 ++++ src/drivers/px4io/px4io.cpp | 96 +++++++++++++++++++++++++++++++++-- src/modules/px4iofirmware/controls.c | 13 +++-- src/modules/px4iofirmware/dsm.c | 43 +++++++++++++++- src/modules/px4iofirmware/protocol.h | 9 ++++ src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++ src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 12 +++++ 9 files changed, 178 insertions(+), 10 deletions(-) (limited to 'src') 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/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..54b9d50e4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,6 +89,8 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +static int dsm_vcc_ctl; + class PX4IO : public device::I2C { public: @@ -700,8 +702,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; @@ -1271,13 +1271,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" : ""), @@ -1424,6 +1425,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 */ @@ -1614,9 +1635,71 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { + if (dsm_vcc_ctl) { + int fd = open("/dev/px4io", O_WRONLY); + if (fd < 0) + errx(1, "failed to open device"); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + } + } exit(0); } +void +bind(int argc, char *argv[]) +{ + int fd, pulses; + + if (g_dev == nullptr) + errx(1, "px4io must be started first"); + + if (dsm_vcc_ctl == 0) + 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]); + + fd = open("/dev/px4io", O_WRONLY); + + if (fd < 0) + errx(1, "failed to open device"); + + ioctl(fd, DSM_BIND_START, pulses); + + /* 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."); + + 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"); + ioctl(fd, DSM_BIND_STOP, 0); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + close(console); + exit(0); + } + } + } +} + void test(void) { @@ -1918,6 +2001,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' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'"); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..9561c9b1e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -95,9 +95,16 @@ controls_tick() { */ perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + else + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -138,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..79e892503 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include @@ -101,6 +102,41 @@ dsm_init(const char *device) return dsm_fd; } +void +dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + case dsm_bind_power_down: + // power down DSM satellite + POWER_RELAY1(0); + break; + case dsm_bind_power_up: + POWER_RELAY1(1); + dsm_guess_format(true); + break; + case dsm_bind_set_rx_out: + stm32_configgpio(usart1RxAsOutp); + break; + case dsm_bind_send_pulses: + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(50); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(50); + } + break; + case dsm_bind_reinit_uart: + // Restore USART rx pin + stm32_configgpio(GPIO_USART1_RX); + break; + } +} + bool dsm_input(uint16_t *values, uint16_t *num_values) { @@ -218,7 +254,7 @@ dsm_guess_format(bool reset) /* * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. + * decoding in 10 or 11-bit mode has yielded anything we recognize. * * XXX Note that due to what seem to be bugs in the DSM2 high-resolution * stream, we may want to sniff for longer in some cases when we think we @@ -303,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; + uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) @@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + if (channel_shift == 11) + *num_values |= 0x8000; + /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..6ee5c2834 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ @@ -157,6 +158,14 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..83feeb9b6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..805eb7ecc 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 7); + break; + default: return -1; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 230060148..a11c13568 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -151,6 +151,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ +PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee..626cbade4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,6 +229,8 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + + int rc_rl1_DSM_VCC_control; } _parameters; /**< local copies of interesting parameters */ struct { @@ -276,6 +278,8 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + + param_t rc_rl1_DSM_VCC_control; } _parameter_handles; /**< handles for interesting parameters */ @@ -509,6 +513,9 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* DSM VCC relay control */ + _parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC"); + /* fetch initial parameter values */ parameters_update(); } @@ -722,6 +729,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* relay 1 DSM VCC control */ + if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) { + warnx("Failed updating relay 1 DSM VCC control"); + } + return OK; } -- cgit v1.2.3 From 8d0784af61536302cc6a2a1d65f847528658ba09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 6 Jul 2013 18:30:09 +0400 Subject: gpio_led: PX4IO RELAY and ACC outputs support, some fixes --- src/modules/gpio_led/gpio_led.c | 110 ++++++++++++++++++++++++++++++++-------- 1 file changed, 89 insertions(+), 21 deletions(-) (limited to 'src') diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c7..632630b54 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -54,9 +54,15 @@ #include #include +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + struct gpio_led_s { struct work_s work; int gpio_fd; + bool use_io; int pin; struct vehicle_status_s status; int vehicle_status_sub; @@ -75,51 +81,97 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { - int pin = GPIO_EXT_1; - if (argc < 2) { - errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]"); + errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" + "\t-p\tUse pin:\n" + "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" + "\t\t2\tPX4FMU GPIO_EXT2\n" + "\t\ta1\tPX4IO GPIO_ACC1\n" + "\t\ta2\tPX4IO GPIO_ACC2\n" + "\t\tr1\tPX4IO GPIO_RELAY1\n" + "\t\tr2\tPX4IO GPIO_RELAY2"); } else { - /* START COMMAND HANDLING */ if (!strcmp(argv[1], "start")) { + if (gpio_led_started) { + errx(1, "already running"); + } + + bool use_io = false; + int pin = GPIO_EXT_1; if (argc > 2) { - if (!strcmp(argv[1], "-p")) { - if (!strcmp(argv[2], "1")) { + if (!strcmp(argv[2], "-p")) { + if (!strcmp(argv[3], "1")) { + use_io = false; pin = GPIO_EXT_1; - } else if (!strcmp(argv[2], "2")) { + } else if (!strcmp(argv[3], "2")) { + use_io = false; pin = GPIO_EXT_2; + } else if (!strcmp(argv[3], "a1")) { + use_io = true; + pin = PX4IO_ACC1; + + } else if (!strcmp(argv[3], "a2")) { + use_io = true; + pin = PX4IO_ACC2; + + } else if (!strcmp(argv[3], "r1")) { + use_io = true; + pin = PX4IO_RELAY1; + + } else if (!strcmp(argv[3], "r2")) { + use_io = true; + pin = PX4IO_RELAY2; + } else { - warnx("[gpio_led] Unsupported pin: %s\n", argv[2]); - exit(1); + errx(1, "unsupported pin: %s", argv[3]); } } } memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.use_io = use_io; gpio_led_data.pin = pin; int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); if (ret != 0) { - warnx("[gpio_led] Failed to queue work: %d\n", ret); - exit(1); + errx(1, "failed to queue work: %d", ret); } else { gpio_led_started = true; + char pin_name[24]; + + if (use_io) { + if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); + + } else { + sprintf(pin_name, "PX4IO RELAY%i", pin); + } + + } else { + sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); + + } + + warnx("start, using pin: %s", pin_name); } exit(0); - /* STOP COMMAND HANDLING */ } else if (!strcmp(argv[1], "stop")) { - gpio_led_started = false; + if (gpio_led_started) { + gpio_led_started = false; + warnx("stop"); - /* INVALID COMMAND */ + } else { + errx(1, "not running"); + } } else { errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]); @@ -131,11 +183,22 @@ void gpio_led_start(FAR void *arg) { FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + char *gpio_dev; + + if (priv->use_io) { + gpio_dev = "/dev/px4io"; + + } else { + gpio_dev = "/dev/px4fmu"; + } + /* open GPIO device */ - priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + priv->gpio_fd = open(gpio_dev, 0); if (priv->gpio_fd < 0) { - warnx("[gpio_led] GPIO: open fail\n"); + // TODO find way to print errors + //printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev); + gpio_led_started = false; return; } @@ -150,11 +213,11 @@ void gpio_led_start(FAR void *arg) int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); if (ret != 0) { - warnx("[gpio_led] Failed to queue work: %d\n", ret); + // TODO find way to print errors + //printf("gpio_led: failed to queue work: %d\n", ret); + gpio_led_started = false; return; } - - warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); } void gpio_led_cycle(FAR void *arg) @@ -211,7 +274,12 @@ void gpio_led_cycle(FAR void *arg) if (priv->counter > 5) priv->counter = 0; - /* repeat cycle at 5 Hz*/ - if (gpio_led_started) + /* repeat cycle at 5 Hz */ + if (gpio_led_started) { work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); + + } else { + /* switch off LED on stop */ + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); + } } -- cgit v1.2.3 From 369e6d1eeab8478f3ce81a7803e55047c221d15b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 6 Jul 2013 18:37:02 +0400 Subject: gpio_led: minor usage fix --- src/modules/gpio_led/gpio_led.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 632630b54..8b4c0cb30 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -86,10 +86,10 @@ int gpio_led_main(int argc, char *argv[]) "\t-p\tUse pin:\n" "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" "\t\t2\tPX4FMU GPIO_EXT2\n" - "\t\ta1\tPX4IO GPIO_ACC1\n" - "\t\ta2\tPX4IO GPIO_ACC2\n" - "\t\tr1\tPX4IO GPIO_RELAY1\n" - "\t\tr2\tPX4IO GPIO_RELAY2"); + "\t\ta1\tPX4IO ACC1\n" + "\t\ta2\tPX4IO ACC2\n" + "\t\tr1\tPX4IO RELAY1\n" + "\t\tr2\tPX4IO RELAY2"); } else { -- cgit v1.2.3 From c4dfc345a127a4b2318f1c6b441b9308a7ad5645 Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 7 Jul 2013 18:27:08 +0200 Subject: Version from esc_status topic added to sdlog2 --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5cc80ead6..92dcfc612 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,6 +1126,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; log_msg.body.log_ESC.esc_num = i; log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; + log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9afae6000..abc882d23 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -219,6 +219,7 @@ struct log_ESC_s { uint8_t esc_num; uint16_t esc_address; + uint16_t esc_version; uint16_t esc_voltage; uint16_t esc_current; uint16_t esc_rpm; @@ -247,7 +248,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(ESC, "HBBBHHHHHfH", "Counter,NumESC,Conn,No,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From dab652faf68931a2b1fa07609d63518237c9c8b7 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 7 Jul 2013 19:04:30 -0400 Subject: Prevent RELAY1 control via IOCTL if DSM bind feature is enabled --- src/drivers/px4io/px4io.cpp | 59 +++++++++++++++++++++++++++--------- src/modules/px4iofirmware/controls.c | 2 +- src/modules/px4iofirmware/dsm.c | 2 +- 3 files changed, 47 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 54b9d50e4..ad0112b9b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,8 +89,6 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -static int dsm_vcc_ctl; - class PX4IO : public device::I2C { public: @@ -131,6 +129,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; @@ -174,6 +182,12 @@ private: float _battery_mamphour_total; uint64_t _battery_last_timestamp; + /** + * Relay1 is dedicated to controlling DSM receiver power + */ + + bool _dsm_vcc_ctl; + /** * Trampoline to the worker task */ @@ -315,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -340,7 +354,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; @@ -1487,18 +1502,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 &= ~1; + 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 & 1)) + 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 & 1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: @@ -1635,9 +1663,12 @@ 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) { - int fd = open("/dev/px4io", O_WRONLY); + g_dev->set_dsm_vcc_ctl(true); + int fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); ioctl(fd, DSM_BIND_POWER_UP, 0); @@ -1655,7 +1686,7 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); - if (dsm_vcc_ctl == 0) + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); if (argc < 3) @@ -1668,7 +1699,7 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); @@ -1694,8 +1725,8 @@ bind(int argc, char *argv[]) ioctl(fd, DSM_BIND_POWER_UP, 0); close(fd); close(console); - exit(0); - } + exit(0); + } } } } @@ -1709,7 +1740,7 @@ test(void) int direction = 1; int ret; - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 9561c9b1e..43d96fb06 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -145,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { + for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 79e892503..ab6e3fec4 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -339,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; + uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) -- cgit v1.2.3 From 20103f572fcf1451b6625209f02b7fde70dd3f04 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 7 Jul 2013 20:19:27 -0400 Subject: Minor px4io optimization Since this module creates the PX4IO object and that the IOCTL function doesn't use the file descriptor parameter, there is no need to invoke IOCTL via the filesystem since we can call it directly. --- src/drivers/px4io/px4io.cpp | 27 +++++++++------------------ 1 file changed, 9 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ad0112b9b..1adefdea5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1388,7 +1388,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; @@ -1668,11 +1669,7 @@ start(int argc, char *argv[]) if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { if (dsm_vcc_ctl) { g_dev->set_dsm_vcc_ctl(true); - int fd = open(GPIO_DEVICE_PATH, O_WRONLY); - if (fd < 0) - errx(1, "failed to open device"); - ioctl(fd, DSM_BIND_POWER_UP, 0); - close(fd); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } exit(0); @@ -1681,7 +1678,7 @@ start(int argc, char *argv[]) void bind(int argc, char *argv[]) { - int fd, pulses; + int pulses; if (g_dev == nullptr) errx(1, "px4io must be started first"); @@ -1699,12 +1696,7 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - fd = open(GPIO_DEVICE_PATH, O_WRONLY); - - if (fd < 0) - errx(1, "failed to open device"); - - ioctl(fd, DSM_BIND_START, pulses); + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); /* Open console directly to grab CTRL-C signal */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); @@ -1721,9 +1713,8 @@ bind(int argc, char *argv[]) if (read(console, &c, 1) == 1) { if (c == 0x03 || c == 0x63) { warnx("Done\n"); - ioctl(fd, DSM_BIND_STOP, 0); - ioctl(fd, DSM_BIND_POWER_UP, 0); - close(fd); + g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); close(console); exit(0); } @@ -1914,7 +1905,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"); } @@ -1958,7 +1949,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); -- cgit v1.2.3 From 040b8f3802afdd59fcf669e54c6f12d33048e1d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jul 2013 14:16:46 +0200 Subject: Cleaned up MAVLink include hierarchy --- src/modules/mavlink/mavlink.c | 7 +++---- src/modules/mavlink/mavlink_bridge_header.h | 8 +++++--- src/modules/mavlink/mavlink_parameters.c | 1 - src/modules/mavlink/mavlink_receiver.cpp | 2 -- src/modules/mavlink/missionlib.c | 1 - src/modules/mavlink/missionlib.h | 2 +- src/modules/mavlink/orb_listener.c | 1 - src/modules/mavlink/waypoints.c | 1 + src/modules/mavlink/waypoints.h | 10 +++++----- 9 files changed, 15 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5b8345e7e..7c1c4b175 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -49,7 +49,6 @@ #include #include #include "mavlink_bridge_header.h" -#include #include #include #include @@ -471,7 +470,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf } void -mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { write(uart, ch, (size_t)(sizeof(uint8_t) * length)); } @@ -479,7 +478,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t *mavlink_get_channel_status(uint8_t channel) +extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_status[channel]; @@ -488,7 +487,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel) /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) +extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_buffer[channel]; diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0010bb341..149efda60 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system; * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 * @param ch Character to send */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); -mavlink_status_t *mavlink_get_channel_status(uint8_t chan); -mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); +extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); +extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); + +#include #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c index 819f3441b..18ca7a854 100644 --- a/src/modules/mavlink/mavlink_parameters.c +++ b/src/modules/mavlink/mavlink_parameters.c @@ -40,7 +40,6 @@ */ #include "mavlink_bridge_header.h" -#include #include "mavlink_parameters.h" #include #include "math.h" /* isinf / isnan checks */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33ac14860..01bbabd46 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg) } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 4b010dd59..be88b8794 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -48,7 +48,6 @@ #include #include #include "mavlink_bridge_header.h" -#include #include #include #include diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h index c2ca735b3..c7d8f90c5 100644 --- a/src/modules/mavlink/missionlib.h +++ b/src/modules/mavlink/missionlib.h @@ -39,7 +39,7 @@ #pragma once -#include +#include "mavlink_bridge_header.h" //extern void mavlink_wpm_send_message(mavlink_message_t *msg); //extern void mavlink_wpm_send_gcs_string(const char *string); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0597555ab..edb8761b8 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -47,7 +47,6 @@ #include #include #include "mavlink_bridge_header.h" -#include #include #include #include diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index cefcca468..405046750 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -45,6 +45,7 @@ #include #include +#include "mavlink_bridge_header.h" #include "missionlib.h" #include "waypoints.h" #include "util.h" diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index c32ab32e5..96a0ecd30 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -47,11 +47,11 @@ #include -#ifndef MAVLINK_SEND_UART_BYTES -#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) -#endif -extern mavlink_system_t mavlink_system; -#include +// #ifndef MAVLINK_SEND_UART_BYTES +// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) +// #endif +//extern mavlink_system_t mavlink_system; +#include "mavlink_bridge_header.h" #include #include #include -- cgit v1.2.3 From 6436e2e3501ce79ee8d7355fbd79235bca402213 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jul 2013 14:43:27 +0200 Subject: Updated mavlink_onboard as well (Hotfix) --- src/modules/mavlink_onboard/mavlink.c | 1 - src/modules/mavlink_onboard/mavlink_bridge_header.h | 2 ++ src/modules/mavlink_onboard/mavlink_receiver.c | 1 - 3 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index cb6d6b16a..20fb11b2c 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -49,7 +49,6 @@ #include #include #include "mavlink_bridge_header.h" -#include #include #include #include diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h index 3ad3bb617..b72bbb2b1 100644 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len mavlink_status_t* mavlink_get_channel_status(uint8_t chan); mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); +#include + #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0acbea675..68d49c24b 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -50,7 +50,6 @@ #include #include #include "mavlink_bridge_header.h" -#include #include #include #include -- cgit v1.2.3 From 2a1bf3018b82c58955139f65845cc59cea1f38bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jul 2013 15:26:25 +0200 Subject: Hotfix: Changed all left-over task_spawn() to task_spawn_cmd() --- src/examples/fixedwing_control/main.c | 2 +- src/examples/flow_position_control/flow_position_control_main.c | 4 ++-- src/examples/flow_position_estimator/flow_position_estimator_main.c | 4 ++-- src/examples/flow_speed_control/flow_speed_control_main.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 27888523b..d13ffe414 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -512,7 +512,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int ex_fixedwing_control_main(int argc, char *argv[]) { diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c177c8fd2..c96f73155 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -101,7 +101,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int flow_position_control_main(int argc, char *argv[]) { @@ -118,7 +118,7 @@ int flow_position_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("flow_position_control", + deamon_task = task_spawn_cmd("flow_position_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 6, 4096, diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c0b16d2e7..e40c9081d 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -90,7 +90,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int flow_position_estimator_main(int argc, char *argv[]) { @@ -107,7 +107,7 @@ int flow_position_estimator_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn("flow_position_estimator", + daemon_task = task_spawn_cmd("flow_position_estimator", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 9648728c8..8b3881c43 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -99,7 +99,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int flow_speed_control_main(int argc, char *argv[]) { @@ -116,7 +116,7 @@ int flow_speed_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("flow_speed_control", + deamon_task = task_spawn_cmd("flow_speed_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 6, 4096, -- cgit v1.2.3 From d878d4756c6defe087b6c2125e74555f02cc63f9 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 9 Jul 2013 14:25:47 +0800 Subject: Ammended UBlox driver to record SV Info, satelites_visible == satelites used. Info is recorded for all SVs, used or not. Might be useful for GPS debugging. --- src/drivers/gps/ubx.cpp | 41 ++++++++++++++++++----------------------- 1 file changed, 18 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b136dfc0a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + 1); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -539,7 +539,7 @@ UBX::handle_message() } case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); + // printf("GOT NAV_SVINFO MESSAGE\n"); 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 @@ -560,7 +560,7 @@ UBX::handle_message() 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 */ @@ -572,27 +572,22 @@ UBX::handle_message() _gps_position->satellite_prn[i] = packet_part2->svid; //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - _gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - _gps_position->satellite_used[i] = 0; - } - - _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); - - } else { - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + //DT uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + //DT Above is broken due to operator precedence. should be ... & (1<<4) or ... & 0x10. + //DT If an SV is unhealthy then it won't be used. + + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + // Update count of SVs used for NAV. + satellites_used++; } + + // Record info for all used 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); } -- cgit v1.2.3 From dc2ef7b3c6e1ee90ba2130ed0574987d5c99a35b Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 9 Jul 2013 17:07:11 +0800 Subject: Some cleanup of NAV_SVINFO message handler --- src/drivers/gps/ubx.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b136dfc0a..ff8945da1 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -567,28 +567,20 @@ UBX::handle_message() 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; - /* Write satellite information in the global storage */ - _gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - //DT uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - //DT Above is broken due to operator precedence. should be ... & (1<<4) or ... & 0x10. - //DT If an SV is unhealthy then it won't be used. - - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - // Update count of SVs used for NAV. + if ( sv_used ) { + // Count SVs used for NAV. satellites_used++; } - // Record info for all used channels, whether or not the SV is used for NAV, + // 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 -- cgit v1.2.3 From e3bb9e87e2af2f0f70e486405e2c6df5d3e23667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 17:36:24 +0200 Subject: Hotfix for GPS: Disable unknown message classes --- src/drivers/gps/ubx.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b46089073 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,7 +176,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 1); + 0); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -327,6 +327,7 @@ UBX::parse_char(uint8_t b) } break; case UBX_DECODE_GOT_CLASS: + { add_byte_to_checksum(b); switch (_message_class) { case NAV: @@ -413,6 +414,14 @@ UBX::parse_char(uint8_t b) // 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 + + // disable unknown message + warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + configure_message_rate(_message_class, b, 0); + } break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); -- cgit v1.2.3 From 6cbbb9b99fbeddc2da00f67bb209d33971a30041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:46:24 +0200 Subject: Hotfix for GPS driver --- src/drivers/gps/ubx.cpp | 51 +++++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 25 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b46089073..3e790499b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 0); + 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, - 1); + 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, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -224,35 +224,18 @@ 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) { - - /* 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++; - } + bool message_found = false; - /* everything is read */ - j = count = 0; + while (true) { - /* then poll for new data */ + /* poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { @@ -272,8 +255,26 @@ UBX::receive(unsigned timeout) * available, we'll go back to poll() again... */ 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])) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + handle_message(); + message_found = true; + } + } } } + + /* return success after receiving a packet */ + if (message_found) + return 1; + + /* abort after timeout if no packet parsed successfully */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } } } -- cgit v1.2.3 From c3d07030dd1882c626ed027cfc5870f42b1cd33e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:56:02 +0200 Subject: Minor additions to fix, pushing --- src/drivers/gps/ubx.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 3e790499b..ad219cd25 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -231,7 +231,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool message_found = false; + bool position_updated = false; while (true) { @@ -260,15 +260,15 @@ UBX::receive(unsigned timeout) if (parse_char(buf[i])) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - handle_message(); - message_found = true; + if (handle_message()) + position_updated = true; } } } } /* return success after receiving a packet */ - if (message_found) + if (position_updated) return 1; /* abort after timeout if no packet parsed successfully */ @@ -420,8 +420,8 @@ UBX::parse_char(uint8_t b) // and we want to tell the module to stop sending this message // disable unknown message - warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - configure_message_rate(_message_class, b, 0); + //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + //configure_message_rate(_message_class, b, 0); } break; case UBX_DECODE_GOT_MESSAGEID: -- cgit v1.2.3 From 897b541b12d5782af51ce0a78658cc153bd13544 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Tue, 9 Jul 2013 20:37:00 -0400 Subject: General cleanup of /dev/px4io and /dev/px4fmu - Use distinct common symbols for px4io and px4fmu device files, and use instead of hardcoded filenames - Use common symbols defining px4io bits consistently between px4fmu and px4io builds. --- .../ardrone_interface/ardrone_motor_control.c | 2 +- src/drivers/drv_gpio.h | 37 ++-------------------- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 10 +++--- src/modules/gpio_led/gpio_led.c | 13 +++----- src/modules/px4iofirmware/protocol.h | 7 ++++ src/modules/px4iofirmware/registers.c | 8 ++--- src/systemcmds/tests/test_gpio.c | 2 +- 8 files changed, 26 insertions(+), 55 deletions(-) (limited to 'src') 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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5147ac500..a98b527b1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -166,7 +166,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 1adefdea5..08aef7069 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -329,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", GPIO_DEVICE_PATH, 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), @@ -1507,7 +1507,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~1; + bits &= ~PX4IO_RELAY1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1515,7 +1515,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1524,7 +1524,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); @@ -1731,7 +1731,7 @@ test(void) int direction = 1; int ret; - fd = open(GPIO_DEVICE_PATH, O_WRONLY); + fd = open(PX4IO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 8b4c0cb30..1aef739c7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,11 +53,7 @@ #include #include #include - -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#include struct gpio_led_s { struct work_s work; @@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; if (priv->use_io) { - gpio_dev = "/dev/px4io"; - + gpio_dev = PX4IO_DEVICE_PATH; } else { - gpio_dev = "/dev/px4fmu"; + gpio_dev = PX4FMU_DEVICE_PATH; } /* open GPIO device */ @@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ + /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ @@ -263,7 +259,6 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); - } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6ee5c2834..88d8cc87c 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -157,6 +157,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ + +/* px4io relay bit definitions */ +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ enum { /* DSM bind states */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 805eb7ecc..a922362b6 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & (1 << 0) ? 1 : 0); - POWER_RELAY2(value & (1 << 1) ? 1 : 0); - POWER_ACC1(value & (1 << 2) ? 1 : 0); - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); + POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); + POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); + POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); break; case PX4IO_P_SETUP_SET_DEBUG: diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index ab536d956..62a873270 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[]) int fd; int ret = 0; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); -- cgit v1.2.3 From 4685871c83e5175e58da08a68b49642daf79267d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 15:28:56 +0400 Subject: Major ubx driver cleanup: few pages of code removed, send update only when full navigation solution received --- src/drivers/gps/ubx.cpp | 841 ++++++++++++++++++++++-------------------------- src/drivers/gps/ubx.h | 61 +--- 2 files changed, 392 insertions(+), 510 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 762c257aa..895d16437 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 * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,12 +57,14 @@ #include "ubx.h" #define UBX_CONFIG_TIMEOUT 100 +#define UBX_PACKET_TIMEOUT 2 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 +76,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 +93,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 +106,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 +124,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, 10); + + 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,20 +260,20 @@ 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 */ @@ -254,25 +283,22 @@ UBX::receive(unsigned timeout) * won't block even on a blocking device. 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 */ + 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 +309,299 @@ 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(); + + ret = 1; + break; + } - //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; + case UBX_MESSAGE_NAV_SVINFO: { + // printf("GOT NAV_SVINFO\n"); + // TODO avoid memcpy + 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; - time_t epoch = mktime(&timeinfo); + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; // for temporal storage - _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++) { //for each channel - case NAV_SVINFO: { - // printf("GOT NAV_SVINFO MESSAGE\n"); + /* 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; - 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++; + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + 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; } - - // 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; + 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->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; + } + + _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 +611,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 +623,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 +651,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 +680,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 * Julian Oes + * Anton Babushkin * * 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 */ @@ -298,44 +299,6 @@ struct ubx_cfg_msg_rate { // END the structures of the binary packets // ************ -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, @@ -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; }; -- cgit v1.2.3 From 0ccf50bca367d39f75a7a4f76f98dec7352ef3d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 18:17:36 +0400 Subject: ubx: SVINFO parsing optimized and message rate increased, CPU consumption reduced in 6 times, ~0.3% now. --- src/drivers/gps/ubx.cpp | 42 +++++++++++++++++++----------------------- 1 file changed, 19 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 895d16437..b579db715 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -56,8 +56,9 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 100 -#define UBX_PACKET_TIMEOUT 2 +#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), @@ -214,7 +215,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); + 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"); @@ -280,9 +281,11 @@ UBX::receive(unsigned timeout) 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. */ + usleep(UBX_WAIT_BEFORE_READ * 1000); count = read(_fd, buf, sizeof(buf)); /* pass received bytes to the packet decoder */ @@ -459,45 +462,39 @@ UBX::handle_message() } case UBX_MESSAGE_NAV_SVINFO: { - // printf("GOT NAV_SVINFO\n"); - // TODO avoid memcpy + //printf("GOT NAV_SVINFO\n"); 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; - + 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; - 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; + //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]); /* write satellite information to global storage */ uint8_t sv_used = packet_part2->flags & 0x01; if (sv_used) { - // Count SVs used for NAV. + /* count SVs used for NAV */ satellites_used++; } - // Record info for all channels, whether or not the SV is used for NAV. + /* 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++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ + 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; @@ -507,7 +504,6 @@ UBX::handle_message() _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; -- cgit v1.2.3 From 39b3fc8d32ebaa0a9a01e73399884b007e97b378 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 11 Jul 2013 10:36:17 -0400 Subject: Don't leave RX in bind mode on console open fail Don't leave RX in bind mode in the unlikely eventuality that console open fails --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08aef7069..ae56b70b3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1696,8 +1696,6 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - /* Open console directly to grab CTRL-C signal */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); if (!console) @@ -1706,6 +1704,8 @@ bind(int argc, char *argv[]) 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 */ -- cgit v1.2.3