From 54a6e66adafb7cca710bab3dc9e9c78c8b2390e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 19:46:35 +0200 Subject: [PATCH 01/18] mavlink: in HIL_CONTROLS send 0 for disabled channels --- src/modules/mavlink/mavlink_messages.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..f292605c9b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1333,7 +1333,7 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (act.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); @@ -1344,7 +1344,7 @@ protected: } } else { - /* send 0 when disarmed */ + /* send 0 when disarmed and for disabled channels */ out[i] = 0.0f; } } @@ -1353,15 +1353,20 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + /* set 0 for disabled channels */ + out[i] = 0.0f; } - } } From 73b4c6eba16d57550c4e745f99428b3580cbe4aa Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Aug 2014 20:09:57 +0200 Subject: [PATCH 02/18] mavlink: stream names updated in configuration --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index f1c21f295d..ee040a706d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -23,7 +23,7 @@ mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 -mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 +mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 # Exit shell to make it available to MAVLink diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a39d3bed2..4086059398 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; From e03c4d4b65d956a90a9e95b35718ebc323ff7539 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 Aug 2014 12:34:47 +0200 Subject: [PATCH 03/18] Fixed copy & paste error on RC output --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b52..0d0896353e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1637,10 +1637,10 @@ protected: msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); From da0e3169f2796d3223b4f7857da067e9aac2fea4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 18:32:51 +0200 Subject: [PATCH 04/18] fw att control: change control surface deflection scaling --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 ++++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6b..926a8db2ac 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d75..94bd26f03d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } From 5b1e4db396425ea6dc559734b785753c54449235 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Sat, 16 Aug 2014 21:43:07 +0200 Subject: [PATCH 05/18] px4io driver: fix actuator output format --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09b..97919538ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status) printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); printf("servos"); From dd0916bd7d537f831838925cbe86e7ff3d933e9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 10:31:56 +0200 Subject: [PATCH 06/18] code style fix for mavlink app --- src/modules/mavlink/mavlink_main.cpp | 74 ++++++++++++++-------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4086059398..2faf8ab769 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,44 +134,44 @@ Mavlink::Mavlink() : _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(1000), - _datarate_events(500), - _rate_mult(1.0f), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _last_write_success_time(0), - _last_write_try_time(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _send_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; From 1913ae19f166d61038e436eff73efa42ac4d8f13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 10:45:30 +0200 Subject: [PATCH 07/18] add param for qnh --- src/drivers/ms5611/ms5611.cpp | 4 +-- src/modules/sensors/sensor_params.c | 10 ++++++++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++++------ 3 files changed, 44 insertions(+), 10 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 873fa62c44..889643d0df 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -130,7 +130,7 @@ protected: float _T; /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ + unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; @@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + irqrestore(flags); return OK; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7ce6ef5ef6..229bfe3ce1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensor Calibration + * @unit hPa + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + /** * Board rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4e8a8c01d3..d8e1fe9c8c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -235,7 +235,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -258,7 +258,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +301,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +356,11 @@ private: param_t board_rotation; param_t external_mag_rotation; - + param_t board_offset[3]; + param_t baro_qnh; + } _parameter_handles; /**< handles for interesting parameters */ @@ -611,12 +615,15 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); - + /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + /* fetch initial parameter values */ parameters_update(); } @@ -828,19 +835,36 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); - + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - + /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; + math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + _board_rotation = _board_rotation * board_rotation_offset; + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int fd; + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + + } else { + warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); + int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (ret) { + errx(ret, "qnh could not be set"); + } + } + close(fd); + return OK; } From c3522f85928c16d55e54dedc73eb4ed1420d527f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 10:58:28 +0200 Subject: [PATCH 08/18] Publish telemetry status on telemetry update and on heartbeat update events to avoid inducing heartbeat update latencies resulting in spurious telemetry link dropped detections. Makes overall state handling simpler --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 30 +++++++----------------- src/modules/mavlink/mavlink_receiver.h | 1 - 3 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2faf8ab769..c27716f74f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -217,6 +217,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0fae0a2f5..e9a9ff1332 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -430,9 +429,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -474,25 +470,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { - - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 014193100b..3999b8b01b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -151,7 +151,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; From 91d8cb2edcda1fde3c1765a659b2445592ed03f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 13:45:51 +0200 Subject: [PATCH 09/18] sensors: move close to correct position --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d8e1fe9c8c..0d51667d05 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -862,8 +862,8 @@ Sensors::parameters_update() if (ret) { errx(ret, "qnh could not be set"); } + close(fd); } - close(fd); return OK; } From 86ae2e489fdd3656faa38202f44e4b53342e450b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 15:21:27 +0200 Subject: [PATCH 10/18] sensors: do not exit task on error --- src/modules/sensors/sensors.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0d51667d05..aac297ef8d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -143,6 +143,12 @@ #define STICK_ON_OFF_LIMIT 0.75f +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * Sensor app start / stop handling function * @@ -466,12 +472,6 @@ private: namespace sensors { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Sensors *g_sensors = nullptr; } @@ -860,7 +860,9 @@ Sensors::parameters_update() warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { - errx(ret, "qnh could not be set"); + warnx("qnh could not be set"); + close(fd); + return ERROR; } close(fd); } From 01f1c90c2673754c10d031796b0dbba5e7a6fd55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:48:38 +0200 Subject: [PATCH 11/18] Make some space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f3..97eddfdd2b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) From 16694051c927b35a853c6ee41a00ad69c81f0bd0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 13:16:58 +0200 Subject: [PATCH 12/18] Require a digit ahead of the dot for a valid number for the SF0x output --- src/drivers/sf0x/sf0x.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bca1715fa1..80ecab2ee1 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -598,7 +598,8 @@ SF0X::collect() memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } - if (_linebuf[i] == '.') { + /* we need a digit before the dot and a dot for a valid number */ + if (i > 0 && _linebuf[i] == '.') { valid = true; } } From 024c6d572706b3ae7755b5c022e198d7db7c5d34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:49:12 +0200 Subject: [PATCH 13/18] Airspeed fix attempt --- src/drivers/airspeed/airspeed.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 41942aacd5..29110373f1 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -364,6 +364,7 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; + warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); @@ -381,7 +382,10 @@ Airspeed::cycle_trampoline(void *arg) Airspeed *dev = (Airspeed *)arg; dev->cycle(); - dev->update_status(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); } void From 0d87dc992312526bb96269d706e2869d540926f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:50:13 +0200 Subject: [PATCH 14/18] mavlink: Handle command int packets --- src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 3 files changed, 63 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c27716f74f..976b6d4d5f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1655,6 +1655,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e9a9ff1332..a602344fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -136,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -275,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3999b8b01b..91125736c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); From 9e82f14ad805dab3be7fcba701430db54c4b339f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 18:53:37 +0200 Subject: [PATCH 15/18] Mag scale check: only test if the scale roughly makes sense, do not judge the environment --- src/systemcmds/tests/test_sensors.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index a4f17eebde..e005bf9c12 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -331,7 +331,7 @@ mag(int argc, char *argv[]) float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); - if (len < 1.0f || len > 3.0f) { + if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } From 8d7a12218ca4305e3ca36320584113773e550091 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:06:30 +0200 Subject: [PATCH 16/18] Time out after a reasonable interval (10 seconds, as e.g. OBC rules prescribe). Experiments show the SiK radios to time out ~4-7 seconds if they loose sync --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c2c030702..74deda8cca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -129,7 +129,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 +#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 7eb521b6d4643fbf36d20e923faadca4e8f61dc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 20:07:55 +0200 Subject: [PATCH 17/18] Do not perform retries in airspeed driver - this is too much load for the HRT queue --- src/drivers/airspeed/airspeed.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 29110373f1..293690d274 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -165,7 +165,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 2; + _retries = 0; return ret; } @@ -364,7 +364,6 @@ Airspeed::update_status() _sensor_ok, SUBSYSTEM_TYPE_DIFFPRESSURE }; - warnx("airspeed ok state changed"); if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); From 690843c53aa03283688e04fbe2894fc986f31b6b Mon Sep 17 00:00:00 2001 From: muharred Date: Wed, 20 Aug 2014 13:06:39 +0300 Subject: [PATCH 18/18] Reset excitement mode for hmc5883 sensor before applying a new one to avoid getting reserved 11 bits set in case of a set_excitement(1) call following a set_excitement(-1) call. --- src/drivers/hmc5883/hmc5883.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0e9a961ac7..e4ecfa6b5d 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); + _conf_reg &= ~0x03; // reset previous excitement mode if (((int)enable) < 0) { _conf_reg |= 0x01; } else if (enable > 0) { _conf_reg |= 0x02; - } else { - _conf_reg &= ~0x03; } // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);