dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into mpc_rc

This commit is contained in:
Anton Babushkin 2014-04-25 21:39:59 +02:00
parent 22aaae197b f0298e005a
commit 0c1de81785
18 changed files with 117 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -34,5 +34,6 @@ mavlink/include/mavlink/v0.9/
/Documentation/html/
/Documentation/doxygen*objdb*tmp
.tags
tags
.tags_sorted_by_file
.pydevproject

44
src/drivers/airspeed/airspeed.cpp
View File

@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(-1),
_subsys_pub(-1),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@ -149,8 +151,7 @@ Airspeed::init()
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
@ -344,22 +345,6 @@ Airspeed::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_DIFFPRESSURE
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
@ -368,12 +353,35 @@ Airspeed::stop()
work_cancel(HPWORK, &_work);
}
void
Airspeed::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_sensor_ok,
SUBSYSTEM_TYPE_DIFFPRESSURE
};
if (_subsys_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
_last_published_sensor_ok = _sensor_ok;
}
}
void
Airspeed::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
dev->update_status();
}
void

7
src/drivers/airspeed/airspeed.h
View File

@ -118,14 +118,21 @@ protected:
virtual int measure() = 0;
virtual int collect() = 0;
/**
* Update the subsystem status
*/
void update_status();
work_s _work;
float _max_differential_pressure_pa;
bool _sensor_ok;
bool _last_published_sensor_ok;
int _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
orb_advert_t _airspeed_pub;
orb_advert_t _subsys_pub;
int _class_instance;

14
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -207,14 +207,18 @@ ETSAirspeed::collect()
void
ETSAirspeed::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
ret = collect();
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
@ -238,8 +242,12 @@ ETSAirspeed::cycle()
}
/* measurement phase */
if (OK != measure())
log("measure error");
ret = measure();
if (OK != ret) {
debug("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;

4
src/drivers/hmc5883/hmc5883.cpp
View File

@ -715,7 +715,7 @@ HMC5883::cycle()
/* perform collection */
if (OK != collect()) {
log("collection error");
debug("collection error");
/* restart the measurement state machine */
start();
return;
@ -742,7 +742,7 @@ HMC5883::cycle()
/* measurement phase */
if (OK != measure())
log("measure error");
debug("measure error");
/* next phase is collection */
_collect_phase = true;

13
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -288,13 +288,17 @@ MEASAirspeed::collect()
void
MEASAirspeed::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
ret = collect();
if (OK != ret) {
/* restart the measurement state machine */
start();
_sensor_ok = false;
return;
}
@ -318,10 +322,13 @@ MEASAirspeed::cycle()
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
ret = measure();
if (OK != ret) {
debug("measure error");
}
_sensor_ok = (ret == OK);
/* next phase is collection */
_collect_phase = true;

11
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013, 2014 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
@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@ -33,9 +33,10 @@
/**
* @file CatapultLaunchMethod.cpp
* Catpult Launch detection
* Catapult Launch detection
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"

6
src/lib/launchdetection/CatapultLaunchMethod.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013, 2014 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
@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*

7
src/lib/launchdetection/LaunchDetector.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013, 2014 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
@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*
@ -30,12 +30,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult)
*
* Authors and acknowledgements in header.
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "LaunchDetector.h"

6
src/lib/launchdetection/LaunchDetector.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013, 2014 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
@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*

6
src/lib/launchdetection/LaunchMethod.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2013, 2014 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
@ -10,9 +10,9 @@
* 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 documentation4 and/or other materials provided with the
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 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.
*

5
src/lib/launchdetection/launchdetection_params.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 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
@ -18,7 +17,7 @@
* 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
* "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,

2
src/lib/launchdetection/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
# Copyright (c) 2012, 2013, 2014 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

6
src/modules/mavlink/mavlink.c
View File

@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
* If set to 1 incomming HIL GPS messages are parsed.
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
mavlink_system_t mavlink_system = {
100,

8
src/modules/mavlink/mavlink_main.cpp
View File

@ -208,6 +208,7 @@ Mavlink::Mavlink() :
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
@ -487,11 +488,13 @@ void Mavlink::mavlink_update_system(void)
static param_t param_system_id;
static param_t param_component_id;
static param_t param_system_type;
static param_t param_use_hil_gps;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
param_use_hil_gps = param_find("MAV_USEHILGPS");
initialized = true;
}
@ -516,6 +519,11 @@ void Mavlink::mavlink_update_system(void)
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)

3
src/modules/mavlink/mavlink_main.h
View File

@ -157,6 +157,8 @@ public:
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
@ -223,6 +225,7 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */

22
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -160,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*
* Accept HIL GPS messages if use_hil_gps flag is true.
* This allows to provide fake gps measurements to the system.
*/
if (_mavlink->get_hil_enabled()) {
switch (msg->msgid) {
@ -167,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_hil_sensor(msg);
break;
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
@ -180,7 +179,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
}
}
/* If we've received a valid message, mark the flag indicating so.
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
default:
break;
}
}
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
}

12
src/modules/sensors/sensors.cpp
View File

@ -1242,7 +1242,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
@ -1527,12 +1527,10 @@ Sensors::task_main()
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* wait for up to 50ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@ -1571,7 +1569,7 @@ Sensors::task_main()
perf_end(_loop_perf);
}
printf("[sensors] exiting.\n");
warnx("[sensors] exiting.");
_sensors_task = -1;
_exit(0);