fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 00:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into mpc_local_pos

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-22 11:13:11 +02:00
parent dfd9601b57 f0e28a60ca
commit 302233a34f
250 changed files with 11421 additions and 526 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/blinkm/blinkm.cpp
+95 -46
View File
@@ -48,11 +48,14 @@
* The recognized number off cells, will be blinked 5 times in purple color.
* 2 Cells = 2 blinks
* ...
* 5 Cells = 5 blinks
* 6 Cells = 6 blinks
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
*
* System disarmed:
* The BlinkM should lit solid red.
* System disarmed and safe:
* The BlinkM should light solid cyan.
*
* System safety off but not armed:
* The BlinkM should light flashing orange
*
* System armed:
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
@@ -67,10 +70,10 @@
* (X = on, _=off)
*
* The first 3 blinks indicates the status of the GPS-Signal (red):
* 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
* 5 satellites = X-X-_-X-_-_-_-_-_-_-
* 6 satellites = X-_-_-X-_-_-_-_-_-_-
* >=7 satellites = _-_-_-X-_-_-_-_-_-_-
* 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
* 5 satellites = X-X-_-X-X-_-_-_-_-_-
* 6 satellites = X-_-_-X-X-_-_-_-_-_-
* >=7 satellites = _-_-_-X-X-_-_-_-_-_-
* If no GPS is found the first 3 blinks are white
*
* The fourth Blink indicates the Flightmode:
@@ -119,6 +122,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/safety.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
static const int LED_ONTIME = 120;
@@ -166,10 +170,12 @@ private:
enum ledColors {
LED_OFF,
LED_RED,
LED_ORANGE,
LED_YELLOW,
LED_PURPLE,
LED_GREEN,
LED_BLUE,
LED_CYAN,
LED_WHITE,
LED_AMBER
};
@@ -380,6 +386,7 @@ BlinkM::led()
static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
static int actuator_armed_sub_fd;
static int safety_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
@@ -402,16 +409,20 @@ BlinkM::led()
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
orb_set_interval(vehicle_status_sub_fd, 250);
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 1000);
orb_set_interval(vehicle_control_mode_sub_fd, 250);
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(actuator_armed_sub_fd, 1000);
orb_set_interval(actuator_armed_sub_fd, 250);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
orb_set_interval(vehicle_gps_position_sub_fd, 250);
/* Subscribe to safety topic */
safety_sub_fd = orb_subscribe(ORB_ID(safety));
orb_set_interval(safety_sub_fd, 250);
topic_initialized = true;
}
@@ -433,7 +444,9 @@ BlinkM::led()
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
if(num_of_cells > 5) {
t_led_color[5] = LED_PURPLE;
}
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
@@ -467,14 +480,17 @@ BlinkM::led()
struct vehicle_control_mode_s vehicle_control_mode;
struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
struct safety_s safety;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
memset(&safety, 0, sizeof(safety));
bool new_data_vehicle_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
bool new_data_safety;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -520,7 +536,12 @@ BlinkM::led()
no_data_vehicle_gps_position = 3;
}
/* update safety topic */
orb_check(safety_sub_fd, &new_data_safety);
if (new_data_safety) {
orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
@@ -541,19 +562,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -565,21 +574,56 @@ BlinkM::led()
led_color_8 = LED_RED;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.rc_signal_lost) {
/* LED Pattern for FAILSAFE */
led_color_1 = LED_BLUE;
led_color_2 = LED_BLUE;
led_color_3 = LED_BLUE;
led_color_4 = LED_BLUE;
led_color_5 = LED_BLUE;
led_color_6 = LED_BLUE;
led_color_7 = LED_BLUE;
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(actuator_armed.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_NOBLINK;
if(safety.safety_off){
led_color_1 = LED_ORANGE;
led_color_2 = LED_ORANGE;
led_color_3 = LED_ORANGE;
led_color_4 = LED_ORANGE;
led_color_5 = LED_ORANGE;
led_color_6 = LED_ORANGE;
led_color_7 = LED_ORANGE;
led_color_8 = LED_ORANGE;
led_blink = LED_BLINK;
}else{
led_color_1 = LED_CYAN;
led_color_2 = LED_CYAN;
led_color_3 = LED_CYAN;
led_color_4 = LED_CYAN;
led_color_5 = LED_CYAN;
led_color_6 = LED_CYAN;
led_color_7 = LED_CYAN;
led_color_8 = LED_CYAN;
led_blink = LED_NOBLINK;
}
} else {
/* armed system - initial led pattern */
led_color_1 = LED_RED;
@@ -593,23 +637,22 @@ BlinkM::led()
led_blink = LED_BLINK;
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
//XXX please check
if (vehicle_control_mode.flag_control_position_enabled)
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
led_color_4 = LED_GREEN;
else if (vehicle_control_mode.flag_control_velocity_enabled)
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_control_mode.flag_control_attitude_enabled)
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
led_color_4 = LED_YELLOW;
else if (vehicle_control_mode.flag_control_manual_enabled)
led_color_4 = LED_AMBER;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat�s */
/* handling used satus */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
@@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) {
case LED_RED: // red
set_rgb(255,0,0);
break;
case LED_ORANGE: // orange
set_rgb(255,150,0);
break;
case LED_YELLOW: // yellow
set_rgb(255,70,0);
set_rgb(200,200,0);
break;
case LED_PURPLE: // purple
set_rgb(255,0,255);
@@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) {
case LED_BLUE: // blue
set_rgb(0,0,255);
break;
case LED_CYAN: // cyan
set_rgb(0,128,128);
break;
case LED_WHITE: // white
set_rgb(255,255,255);
break;
case LED_AMBER: // amber
set_rgb(255,20,0);
set_rgb(255,65,0);
break;
}
}
src/drivers/drv_range_finder.h
+9 -2
View File
@@ -46,6 +46,10 @@
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
enum RANGE_FINDER_TYPE {
RANGE_FINDER_TYPE_LASER = 0,
};
/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
@@ -53,8 +57,11 @@
struct range_finder_report {
uint64_t timestamp;
uint64_t error_count;
float distance; /** in meters */
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
unsigned type; /**< type, following RANGE_FINDER_TYPE enum */
float distance; /**< in meters */
float minimum_distance; /**< minimum distance the sensor can measure */
float maximum_distance; /**< maximum distance the sensor can measure */
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
};
/*
src/drivers/gps/gps.cpp
+2 -2
View File
@@ -282,8 +282,8 @@ GPS::task_main()
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 10.0f;
_report.epv_m = 10.0f;
_report.eph_m = 3.0f;
_report.epv_m = 7.0f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
src/drivers/meas_airspeed/meas_airspeed.cpp
+115 -13
View File
@@ -87,6 +87,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <drivers/airspeed/airspeed.h>
@@ -121,6 +122,14 @@ protected:
virtual int collect();
math::LowPassFilter2p _filter;
/**
* Correct for 5V rail voltage variations
*/
void voltage_correction(float &diff_pres_pa, float &temperature);
int _t_system_power;
struct system_power_s system_power;
};
/*
@@ -129,10 +138,11 @@ protected:
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1)
{
memset(&system_power, 0, sizeof(system_power));
}
int
@@ -194,19 +204,48 @@ MEASAirspeed::collect()
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
/* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value
*/
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
if (diff_press_pa < 0.0f) {
diff_press_pa = 0.0f;
}
We negate the result so that positive differential pressures
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
@@ -219,6 +258,13 @@ MEASAirspeed::collect()
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -287,6 +333,62 @@ MEASAirspeed::cycle()
USEC2TICK(CONVERSION_INTERVAL));
}
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
void
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
}
if (_t_system_power == -1) {
// not available
return;
}
bool updated = false;
orb_check(_t_system_power, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
}
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
// not valid, skip correction
return;
}
const float slope = 65.0f;
/*
apply a piecewise linear correction, flattening at 0.5V from 5V
*/
float voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -0.5f) {
voltage_diff = -0.5f;
}
diff_press_pa -= voltage_diff * slope;
/*
the temperature masurement varies as well
*/
const float temp_slope = 0.887f;
voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
/**
* Local functions in support of the shell command.
*/
@@ -409,7 +511,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -437,7 +539,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
src/drivers/px4io/px4io.cpp
+20 -4
View File
@@ -944,8 +944,23 @@ PX4IO::task_main()
int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
if (pret != OK) {
log("voltage scaling upload failed");
log("vscale upload failed");
}
/* send RC throttle failsafe value to IO */
int32_t failsafe_param_val;
param_t failsafe_param = param_find("RC_FAILS_THR");
if (failsafe_param > 0) {
param_get(failsafe_param, &failsafe_param_val);
uint16_t failsafe_thr = failsafe_param_val;
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
if (pret != OK) {
log("failsafe upload failed");
}
}
}
}
@@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
/* we do not know the RC input, only publish if RC OK flag is set */
/* if no raw RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
/* only keep publishing RC input if we ever got a valid input */
if (_rc_last_valid == 0) {
/* we have never seen valid RC signals, abort */
return OK;
}
}
/* lazily advertise on first publication */
src/drivers/px4io/px4io_uploader.cpp
+7 -2
View File
@@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[])
continue;
}
if (bl_rev <= 2)
if (bl_rev <= 2) {
ret = verify_rev2(fw_size);
else if(bl_rev == 3) {
} else if(bl_rev == 3) {
ret = verify_rev3(fw_size);
} else {
/* verify rev 4 and higher still uses the same approach and
* every version *needs* to be verified.
*/
ret = verify_rev3(fw_size);
}
src/drivers/sf0x/sf0x.cpp
+10 -2
View File
@@ -124,6 +124,8 @@ private:
orb_advert_t _range_finder_topic;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_last_read(0),
_range_finder_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -720,12 +723,17 @@ SF0X::cycle()
if (OK != collect_ret) {
/* we know the sensor needs about four seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL) {
log("collection error");
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
log("collection error #%u", _consecutive_fail_count);
}
_consecutive_fail_count++;
/* restart the measurement state machine */
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
src/drivers/stm32/adc/adc.cpp
+46 -1
View File
@@ -41,6 +41,7 @@
*/
#include <nuttx/config.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <sys/types.h>
@@ -64,6 +65,8 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/system_power.h>
/*
* Register accessors.
* For now, no reason not to just use ADC1.
@@ -119,6 +122,8 @@ private:
unsigned _channel_count;
adc_msg_s *_samples; /**< sample buffer */
orb_advert_t _to_system_power;
/** work trampoline */
static void _tick_trampoline(void *arg);
@@ -134,13 +139,16 @@ private:
*/
uint16_t _sample(unsigned channel);
// update system_power ORB topic, only on FMUv2
void update_system_power(void);
};
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_channel_count(0),
_samples(nullptr)
_samples(nullptr),
_to_system_power(0)
{
_debug_enabled = true;
@@ -290,6 +298,43 @@ ADC::_tick()
/* scan the channel set and sample each */
for (unsigned i = 0; i < _channel_count; i++)
_samples[i].am_data = _sample(_samples[i].am_channel);
update_system_power();
}
void
ADC::update_system_power(void)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
system_power_s system_power;
system_power.timestamp = hrt_absolute_time();
system_power.voltage5V_v = 0;
for (unsigned i = 0; i < _channel_count; i++) {
if (_samples[i].am_channel == 4) {
// it is 2:1 scaled
system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
}
}
// these are not ADC related, but it is convenient to
// publish these to the same topic
system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
// note that the valid pins are active low
system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
// OC pins are active low
system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
/* lazily publish */
if (_to_system_power > 0) {
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
} else {
_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
}
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
uint16_t
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+6 -1
View File
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
// don't allow bad values to propogate via the filter
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
return output;
}
float LowPassFilter2p::reset(float sample) {
_delay_element_1 = _delay_element_2 = sample;
return apply(sample);
}
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+16 -4
View File
@@ -52,18 +52,30 @@ public:
_delay_element_1 = _delay_element_2 = 0;
}
// change parameters
/**
* Change filter parameters
*/
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// apply - Add a new raw value to the filter
// and retrieve the filtered result
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
float apply(float sample);
// return the cutoff frequency
/**
* Return the cutoff frequency
*/
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
/**
* Reset the filter state to this value
*/
float reset(float sample);
private:
float _cutoff_freq;
float _a1;
src/modules/commander/airspeed_calibration.cpp
+8 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. 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
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
src/modules/commander/commander.cpp
+8 -3
View File
@@ -396,6 +396,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
bool ret = false;
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
return false;
}
/* only handle high-priority commands here */
/* request to set different system mode */
@@ -633,11 +638,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
default:
/* warn about unsupported commands */
/* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
@@ -1047,7 +1052,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
src/modules/dataman/dataman.c
+17 -2
View File
@@ -44,7 +44,9 @@
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <queue.h>
#include <string.h>
#include "dataman.h"
@@ -594,6 +596,20 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
else
close(g_task_fd);
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
@@ -603,7 +619,7 @@ task_main(int argc, char *argv[])
return -1;
}
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[])
exit(1);
}
src/modules/dataman/dataman.h
+1 -1
View File
@@ -79,7 +79,7 @@ extern "C" {
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 126
#define DM_MAX_DATA_SIZE 124
/* Retrieve from the data manager store */
__EXPORT ssize_t
src/modules/fw_att_control/fw_att_control_main.cpp
+12 -6
View File
@@ -660,18 +660,24 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
/* if airspeed is not updating, we assume the normal average speed */
if (!isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
airspeed = _airspeed.true_airspeed_m_s;
}
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+1 -1
View File
@@ -961,7 +961,7 @@ FixedwingEstimator::task_main()
}
// Publish results
if (_initialized) {
if (_initialized && (check == OK)) {
src/modules/mavlink/mavlink_commands.cpp
+73
View File
@@ -0,0 +1,73 @@
/****************************************************************************
*
* Copyright (c) 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
* 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 mavlink_commands.cpp
* Mavlink commands stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "mavlink_commands.h"
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
}
MavlinkCommandsStream::~MavlinkCommandsStream()
{
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
}
}
}
src/modules/mavlink/mavlink_commands.h
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (c) 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
* 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 mavlink_commands.h
* Mavlink commands stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_COMMANDS_H_
#define MAVLINK_COMMANDS_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
class Mavlink;
class MavlinkCommansStream;
#include "mavlink_main.h"
class MavlinkCommandsStream
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
~MavlinkCommandsStream();
void update(const hrt_abstime t);
};
#endif /* MAVLINK_COMMANDS_H_ */
src/modules/mavlink/mavlink_main.cpp
+220 -11
View File
@@ -81,6 +81,7 @@
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -166,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
@@ -185,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
}
}
}
static void usage(void);
@@ -203,14 +209,21 @@ Mavlink::Mavlink() :
_task_running(false),
_hil_enabled(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_pub(-1),
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -261,7 +274,6 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
}
Mavlink::~Mavlink()
@@ -394,6 +406,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
return false;
}
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
inst->pass_message(msg);
}
}
}
int
Mavlink::get_uart_fd(unsigned index)
{
@@ -808,10 +832,10 @@ void Mavlink::publish_mission()
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
_mission_pub = orb_advertise(ORB_ID(mission), &mission);
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
orb_publish(ORB_ID(mission), _mission_pub, &mission);
orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
}
}
@@ -1616,6 +1640,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
}
}
int
Mavlink::message_buffer_init(int size)
{
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
return (_message_buffer.data == 0) ? ERROR : OK;
}
void
Mavlink::message_buffer_destroy()
{
_message_buffer.size = 0;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
free(_message_buffer.data);
}
int
Mavlink::message_buffer_count()
{
int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
if (n < 0) {
n += _message_buffer.size;
}
return n;
}
int
Mavlink::message_buffer_is_empty()
{
return _message_buffer.read_ptr == _message_buffer.write_ptr;
}
bool
Mavlink::message_buffer_write(void *ptr, int size)
{
// bytes available to write
int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
if (available < 0) {
available += _message_buffer.size;
}
if (size > available) {
// buffer overflow
return false;
}
char *c = (char *) ptr;
int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer
if (n < size) {
// message goes over end of the buffer
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
_message_buffer.write_ptr = 0;
} else {
n = 0;
}
// now: n = bytes already written
int p = size - n; // number of bytes to write
memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
return true;
}
int
Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
{
// bytes available to read
int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
if (available == 0) {
return 0; // buffer is empty
}
int n = 0;
if (available > 0) {
// read pointer is before write pointer, all available bytes can be read
n = available;
*is_part = false;
} else {
// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
n = _message_buffer.size - _message_buffer.read_ptr;
*is_part = _message_buffer.write_ptr > 0;
}
*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
return n;
}
void
Mavlink::message_buffer_mark_read(int n)
{
_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
}
void
Mavlink::pass_message(mavlink_message_t *msg)
{
if (_passing_on) {
/* size is 8 bytes plus variable payload */
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_write(msg, size);
pthread_mutex_unlock(&_message_buffer_mutex);
}
}
int
Mavlink::task_main(int argc, char *argv[])
{
@@ -1632,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,10 +1815,22 @@ Mavlink::task_main(int argc, char *argv[])
break;
case 'f':
_forwarding_on = true;
break;
case 'p':
_passing_on = true;
break;
case 'v':
_verbose = true;
break;
case 'w':
_wait_to_transmit = true;
break;
default:
err_flag = true;
break;
@@ -1740,6 +1895,17 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
if (OK != message_buffer_init(500)) {
errx(1, "can't allocate message buffer, exiting");
}
/* initialize message buffer mutex */
pthread_mutex_init(&_message_buffer_mutex, NULL);
}
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
@@ -1766,6 +1932,8 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
@@ -1783,6 +1951,9 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
@@ -1826,6 +1997,9 @@ Mavlink::task_main(int argc, char *argv[])
set_hil_enabled(status->hil_state == HIL_STATE_ON);
}
/* update commands stream */
commands_stream.update(t);
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1884,6 +2058,37 @@ Mavlink::task_main(int argc, char *argv[])
}
}
/* pass messages from other UARTs */
if (_passing_on) {
bool is_part;
void *read_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
/* write first part of buffer */
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
/* write second part of buffer if there is some */
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
}
}
}
perf_end(_loop_perf);
}
@@ -1928,6 +2133,10 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
if (_passing_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
/* destroy log buffer */
mavlink_logbuffer_destroy(&_logbuffer);
@@ -2067,7 +2276,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
}
int mavlink_main(int argc, char *argv[])
src/modules/mavlink/mavlink_main.h
+47 -1
View File
@@ -138,6 +138,8 @@ public:
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
int get_uart_fd();
@@ -153,10 +155,12 @@ public:
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; };
bool get_hil_enabled() { return _hil_enabled; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
@@ -196,6 +200,16 @@ public:
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
protected:
Mavlink *next;
@@ -210,6 +224,8 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
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. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
@@ -234,6 +250,8 @@ private:
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
@@ -252,6 +270,18 @@ private:
bool _flow_control_enabled;
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
/**
* Send one parameter.
*
@@ -315,6 +345,22 @@ private:
int configure_stream(const char *stream_name, const float rate);
void configure_stream_threadsafe(const char *stream_name, const float rate);
int message_buffer_init(int size);
void message_buffer_destroy();
int message_buffer_count();
int message_buffer_is_empty();
bool message_buffer_write(void *ptr, int size);
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
void pass_message(mavlink_message_t *msg);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
src/modules/mavlink/mavlink_messages.cpp
+106 -20
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include "mavlink_messages.h"
@@ -262,22 +263,21 @@ protected:
void send(const hrt_abstime t)
{
if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
};
@@ -641,6 +641,47 @@ protected:
};
class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
const char *get_name()
{
return "VICON_POSITION_ESTIMATE";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamViconPositionEstimate();
}
private:
MavlinkOrbSubscription *pos_sub;
struct vehicle_vicon_position_s *pos;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_vicon_position_estimate_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->roll,
pos->pitch,
pos->yaw);
}
}
};
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
@@ -1253,8 +1294,6 @@ protected:
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
@@ -1265,15 +1304,60 @@ protected:
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
const char *get_name()
{
return "DISTANCE_SENSOR";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
}
private:
MavlinkOrbSubscription *range_sub;
struct range_finder_report *range;
protected:
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
range = (struct range_finder_report *)range_sub->get_data();
}
void send(const hrt_abstime t)
{
(void)range_sub->update(t);
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -1300,5 +1384,7 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
new MavlinkStreamDistanceSensor(),
new MavlinkStreamViconPositionEstimate(),
nullptr
};
src/modules/mavlink/mavlink_receiver.cpp
+38 -31
View File
@@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
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);
}
void
@@ -222,12 +226,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
@@ -254,7 +256,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
if (_flow_pub <= 0) {
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
@@ -288,7 +290,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
@@ -313,7 +315,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (_vicon_position_pub <= 0) {
if (_vicon_position_pub < 0) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
@@ -374,7 +376,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub <= 0) {
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
@@ -402,7 +404,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub <= 0) {
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@@ -429,7 +431,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_rc_pub == 0) {
if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
@@ -451,7 +453,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub == 0) {
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
@@ -620,11 +622,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */
if (_sensors_pub > 0) {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
if (_sensors_pub < 0) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
}
}
@@ -639,11 +641,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
@@ -695,11 +697,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
if (_gps_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
} else {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
}
}
@@ -753,11 +755,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
if (_attitude_pub < 0) {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
}
}
@@ -777,11 +779,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
}
}
@@ -821,11 +823,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
}
}
@@ -862,11 +864,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
}
@@ -918,6 +920,11 @@ MavlinkReceiver::receive_thread(void *arg)
/* handle packet with parameter component */
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
if (_mavlink->get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(&msg, _mavlink);
}
}
}
}
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -120,7 +120,6 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
int _manual_sub;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub;
@@ -139,6 +138,7 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
src/modules/mavlink/mavlink_stream.h
+1 -1
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mavlink_stream.cpp
* @file mavlink_stream.h
* Mavlink messages stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
src/modules/mavlink/module.mk
+4 -1
View File
@@ -42,6 +42,9 @@ SRCS += mavlink_main.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp
mavlink_rate_limiter.cpp \
mavlink_commands.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
src/modules/navigator/navigator_main.cpp
+4 -4
View File
@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +611,7 @@ Navigator::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
src/modules/position_estimator_inav/position_estimator_inav_main.c
+1 -1
View File
@@ -864,7 +864,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
src/modules/position_estimator_inav/position_estimator_inav_params.c
+1 -1
View File
@@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
src/modules/px4iofirmware/controls.c
+94 -84
View File
@@ -134,8 +134,6 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
@@ -201,94 +199,104 @@ controls_tick() {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
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];
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
scaled = -scaled;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
if (mapped == 3 && r_setup_rc_thr_failsafe) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i))) {
r_rc_values[i] = 0;
}
}
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/*
@@ -316,8 +324,13 @@ controls_tick() {
* Handle losing RC input
*/
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set raw channel count to zero */
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* must have R/C input (NO FAILSAFE!).
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;
@@ -369,10 +379,10 @@ controls_tick() {
mixer_tick();
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
}
src/modules/px4iofirmware/protocol.h
+15 -13
View File
@@ -164,10 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum { /* DSM bind states */
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* 12 occupied by CRC */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum { /* DSM bind states */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52
src/modules/px4iofirmware/px4io.h
+1
View File
@@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#endif
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_control_values (&r_page_controls[0])
src/modules/px4iofirmware/registers.c
+1
View File
@@ -169,6 +169,7 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
src/modules/sdlog2/sdlog2.c
+50 -26
View File
@@ -84,6 +84,8 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -224,11 +226,11 @@ sdlog2_usage(const char *reason)
}
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
}
/**
@@ -255,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
main_thread_should_exit = false;
deamon_task = task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
}
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +865,8 @@ int sdlog2_thread_main(int argc, char *argv[])
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int system_power_sub;
int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +892,8 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
@@ -898,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* track changes in distance status */
bool dist_bottom_present = false;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
@@ -1089,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
@@ -1098,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(LPOS);
if (buf.local_pos.dist_bottom_valid) {
dist_bottom_present = true;
}
if (dist_bottom_present) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
}
/* --- LOCAL POSITION SETPOINT --- */
@@ -1154,7 +1152,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
// TODO not implemented yet
log_msg.msg_type = LOG_VICN_MSG;
log_msg.body.log_VICN.x = buf.vicon_pos.x;
log_msg.body.log_VICN.y = buf.vicon_pos.y;
log_msg.body.log_VICN.z = buf.vicon_pos.z;
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- FLOW --- */
@@ -1184,6 +1189,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
@@ -1226,6 +1232,24 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
/* copy servo rail status topic here too */
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
LOGBUFFER_WRITE_AND_COUNT(PWR);
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
src/modules/sdlog2/sdlog2_messages.h
+65 -43
View File
@@ -101,6 +101,8 @@ struct log_LPOS_s {
float x;
float y;
float z;
float ground_dist;
float ground_dist_rate;
float vx;
float vy;
float vz;
@@ -110,6 +112,7 @@ struct log_LPOS_s {
uint8_t xy_flags;
uint8_t z_flags;
uint8_t landed;
uint8_t ground_dist_flags;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -174,6 +177,7 @@ struct log_OUT0_s {
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
float air_temperature_celsius;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -277,6 +281,40 @@ struct log_TELE_s {
uint8_t txbuf;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 23
struct log_ESTM_s {
float s[10];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
struct log_PWR_s {
float peripherals_5v;
float servo_rail_5v;
float servo_rssi;
uint8_t usb_ok;
uint8_t brick_ok;
uint8_t servo_ok;
uint8_t low_power_rail_overcurrent;
uint8_t high_power_rail_overcurrent;
};
/* --- VICN - VICON POSITION --- */
#define LOG_VICN_MSG 25
struct log_VICN_s {
float x;
float y;
float z;
float roll;
float pitch;
float yaw;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -299,59 +337,43 @@ struct log_PARM_s {
float value;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 132
struct log_ESTM_s {
float s[10];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
// struct log_ESTM_s {
// float s[32];
// uint8_t n_states;
// uint8_t states_nan;
// uint8_t covariance_nan;
// uint8_t kalman_gain_nan;
// };
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Nf", "Name,Value"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
LOG_FORMAT(PARM, "Nf", "Name,Value")
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
#endif /* SDLOG2_MESSAGES_H_ */
src/modules/sensors/sensor_params.c
+3 -25
View File
@@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
/**
* Failsafe channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_CH, 0);
/**
* Failsafe channel mode.
*
* 0 = too low means signal loss,
* 1 = too high means signal loss
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FS_MODE, 0);
/**
* Failsafe channel PWM threshold.
*
* @min 0
* @max 1
* @min 800
* @max 2200
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
src/modules/sensors/sensors.cpp
+12 -23
View File
@@ -263,9 +263,7 @@ private:
float rc_scale_yaw;
float rc_scale_flaps;
int rc_fs_ch;
int rc_fs_mode;
float rc_fs_thr;
int32_t rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
@@ -313,8 +311,6 @@ private:
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_ch;
param_t rc_fs_mode;
param_t rc_fs_thr;
param_t battery_voltage_scaling;
@@ -531,9 +527,7 @@ Sensors::Sensors() :
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -689,8 +683,6 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
/* update RC function mappings */
@@ -1033,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
@@ -1312,19 +1305,15 @@ Sensors::rc_poll()
manual_control.aux5 = NAN;
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4)
if (rc_input.channel_count < 4) {
return;
}
/* failsafe check */
if (_parameters.rc_fs_ch != 0) {
if (_parameters.rc_fs_mode == 0) {
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
return;
} else if (_parameters.rc_fs_mode == 1) {
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
return;
}
/* check for failsafe */
if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
/* do not publish manual control setpoints when there are none */
return;
}
unsigned channel_limit = rc_input.channel_count;
src/modules/uORB/objects_common.cpp
+4 -1
View File
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/servorail_status.h"
ORB_DEFINE(servorail_status, struct servorail_status_s);
#include "topics/system_power.h"
ORB_DEFINE(system_power, struct system_power_s);
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
@@ -124,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
ORB_DEFINE(offboard_mission, struct mission_s);
ORB_DEFINE(onboard_mission, struct mission_s);
#include "topics/mission_result.h"
src/modules/uORB/topics/airspeed.h
+3 -2
View File
@@ -52,9 +52,10 @@
* Airspeed
*/
struct airspeed_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
};
/**
src/modules/uORB/topics/differential_pressure.h
+5 -4
View File
@@ -52,13 +52,14 @@
* Differential pressure.
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
};
src/modules/uORB/topics/mission.h
+1 -1
View File
@@ -105,7 +105,7 @@ struct mission_s
*/
/* register this as object request broker structure */
ORB_DECLARE(mission);
ORB_DECLARE(offboard_mission);
ORB_DECLARE(onboard_mission);
#endif
src/modules/uORB/topics/system_power.h
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file system_power.h
*
* Definition of the system_power voltage and power status uORB topic.
*/
#ifndef SYSTEM_POWER_H_
#define SYSTEM_POWER_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* voltage and power supply status
*/
struct system_power_s {
uint64_t timestamp; /**< microseconds since system boot */
float voltage5V_v; /**< peripheral 5V rail voltage */
uint8_t usb_connected:1; /**< USB is connected when 1 */
uint8_t brick_valid:1; /**< brick power is good when 1 */
uint8_t servo_valid:1; /**< servo power is good when 1 */
uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */
uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(system_power);
#endif