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

Merge remote-tracking branch 'px4/master' into pwm_ioctls 

Conflicts:
	src/drivers/px4io/px4io.cpp
This commit is contained in:
Julian Oes 2013-10-11 14:05:11 +02:00
parent 326f241185 1306c9de7b
commit 2d23d5fd4e
51 changed files with 1097 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
View File

@ -32,6 +32,7 @@ then
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set SYS_AUTOCONFIG 0
param save

22
ROMFS/px4fmu_common/init.d/100_mpx_easystar
View File

@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on EasyStar"
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set SYS_AUTOCONFIG 0
param save

22
ROMFS/px4fmu_common/init.d/101_hk_bixler
View File

@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler"
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set SYS_AUTOCONFIG 0
param save

22
ROMFS/px4fmu_common/init.d/31_io_phantom
View File

@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 17
param set SYS_AUTOCONFIG 0
param save

9
ROMFS/px4fmu_common/init.d/cmp_test Normal file
View File

@ -0,0 +1,9 @@
#!nsh
cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
then
echo "CMP returned true"
else
echo "CMP returned false"
fi

120
ROMFS/px4fmu_common/init.d/rc.custom_io_esc Normal file
View File

@ -0,0 +1,120 @@
#!nsh
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.002
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.09
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 6.8
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
echo "RC script for PX4FMU + PX4IO + PPM-ESCs running"
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
usleep 10000
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start -d /dev/ttyS1 -b 115200
usleep 5000
sh /etc/init.d/rc.io
if [ $ESC_MAKER = afro ]
then
# Set PWM values for Afro ESCs
px4io idle 1050 1050 1050 1050
px4io min 1080 1080 1080 1080
px4io max 1860 1860 1860 1860
else
# Set PWM values for typical ESCs
px4io idle 900 900 900 900
px4io min 1110 1100 1100 1100
px4io max 1800 1800 1800 1800
fi
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS1 -b 115200
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer
#
if [ $FRAME_GEOMETRY == x ]
then
echo "Frame geometry X"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
if [ $FRAME_GEOMETRY == w ]
then
echo "Frame geometry W"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
else
echo "Frame geometry +"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
fi
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi
echo "Script end"

17
ROMFS/px4fmu_common/init.d/rcS
View File

@ -225,6 +225,23 @@ then
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 21
then
set FRAME_GEOMETRY x
set ESC_MAKER afro
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
if param compare SYS_AUTOSTART 30
then

3
makefiles/config_px4fmu-v2_default.mk
View File

@ -128,6 +128,9 @@ MODULES += lib/geo
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Hardware test
MODULES += examples/hwtest
#
# Transitional support - add commands from the NuttX export archive.
#

1
src/drivers/bma180/bma180.cpp
View File

@ -690,6 +690,7 @@ BMA180::measure()
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.

1
src/drivers/drv_accel.h
View File

@ -52,6 +52,7 @@
*/
struct accel_report {
uint64_t timestamp;
uint64_t error_count;
float x; /**< acceleration in the NED X board axis in m/s^2 */
float y; /**< acceleration in the NED Y board axis in m/s^2 */
float z; /**< acceleration in the NED Z board axis in m/s^2 */

1
src/drivers/drv_baro.h
View File

@ -55,6 +55,7 @@ struct baro_report {
float altitude;
float temperature;
uint64_t timestamp;
uint64_t error_count;
};
/*

1
src/drivers/drv_gyro.h
View File

@ -52,6 +52,7 @@
*/
struct gyro_report {
uint64_t timestamp;
uint64_t error_count;
float x; /**< angular velocity in the NED X board axis in rad/s */
float y; /**< angular velocity in the NED Y board axis in rad/s */
float z; /**< angular velocity in the NED Z board axis in rad/s */

1
src/drivers/drv_mag.h
View File

@ -54,6 +54,7 @@
*/
struct mag_report {
uint64_t timestamp;
uint64_t error_count;
float x;
float y;
float z;

1
src/drivers/drv_range_finder.h
View File

@ -52,6 +52,7 @@
*/
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 */
};

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

@ -153,35 +153,36 @@ ETSAirspeed::collect()
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
log("zero value from sensor");
return -1;
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
perf_count(_comms_errors);
log("zero value from sensor");
return -1;
}
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
}
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
_max_differential_pressure_pa = diff_pres_pa;
}
// XXX we may want to smooth out the readings to remove noise.
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@ -209,7 +210,7 @@ ETSAirspeed::cycle()
/* perform collection */
if (OK != collect()) {
log("collection error");
perf_count(_comms_errors);
/* restart the measurement state machine */
start();
return;

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

@ -791,6 +791,7 @@ HMC5883::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that

1
src/drivers/l3gd20/l3gd20.cpp
View File

@ -771,6 +771,7 @@ L3GD20::measure()
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0; // not recorded
switch (_orientation) {

1
src/drivers/lsm303d/lsm303d.cpp
View File

@ -1248,6 +1248,7 @@ LSM303D::measure()
accel_report.timestamp = hrt_absolute_time();
accel_report.error_count = 0; // not reported
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;

1
src/drivers/mb12xx/mb12xx.cpp
View File

@ -490,6 +490,7 @@ MB12XX::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;

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

@ -138,7 +138,6 @@ MEASAirspeed::measure()
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
@ -161,7 +160,6 @@ MEASAirspeed::collect()
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@ -207,6 +205,7 @@ MEASAirspeed::collect()
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
@ -235,7 +234,6 @@ MEASAirspeed::cycle()
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;

2
src/drivers/mpu6000/mpu6000.cpp
View File

@ -1186,7 +1186,7 @@ MPU6000::measure()
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
grb.error_count = arb.error_count = 0; // not reported
/*
* 1) Scale raw value to SI units using scaling from datasheet.

1
src/drivers/ms5611/ms5611.cpp
View File

@ -573,6 +573,7 @@ MS5611::collect()
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->read(0, (void *)&raw, 0);

224
src/drivers/px4io/px4io.cpp
View File

@ -80,6 +80,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/parameter_update.h>
#include <debug.h>
@ -94,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define UPDATE_INTERVAL_MIN 2
/**
* The PX4IO class.
*
@ -177,6 +180,11 @@ public:
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Disable RC input handling
*/
int disable_rc_handling();
/**
* Print IO status.
*
@ -185,9 +193,9 @@ public:
void print_status();
/**
* Disable RC input handling
* Fetch and print debug console output.
*/
int disable_rc_handling();
int print_debug();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/**
@ -251,6 +259,7 @@ private:
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
@ -424,8 +433,27 @@ private:
*/
void dsm_bind_ioctl(int dsmMode);
};
/**
* Handle a battery update from IO.
*
* Publish IO battery information if necessary.
*
* @param vbatt vbatt register
* @param ibatt ibatt register
*/
void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
/**
* Handle a servorail update from IO.
*
* Publish servo rail information if necessary.
*
* @param vservo vservo register
* @param vrssi vrssi register
*/
void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
};
namespace
{
@ -461,6 +489,7 @@ PX4IO::PX4IO(device::Device *interface) :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
_to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
@ -790,8 +819,8 @@ PX4IO::task_main()
/* adjust update interval */
if (_update_interval != 0) {
if (_update_interval < 5)
_update_interval = 5;
if (_update_interval < UPDATE_INTERVAL_MIN)
_update_interval = UPDATE_INTERVAL_MIN;
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
@ -1203,10 +1232,67 @@ PX4IO::io_handle_alarms(uint16_t alarms)
return 0;
}
void
PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
{
/* only publish if battery has a valid minimum voltage */
if (vbatt <= 3300) {
return;
}
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
battery_status.voltage_v = vbatt / 1000.0f;
/*
ibatt contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/*
integrate battery over time to get total mAh used
*/
if (_battery_last_timestamp != 0) {
_battery_mamphour_total += battery_status.current_a *
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
}
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
void
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
{
servorail_status_s servorail_status;
servorail_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
servorail_status.voltage_v = vservo * 0.001f;
servorail_status.rssi_v = vrssi * 0.001f;
/* lazily publish the servorail voltages */
if (_to_servorail > 0) {
orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
} else {
_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
}
}
int
PX4IO::io_get_status()
{
uint16_t regs[4];
uint16_t regs[6];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@ -1216,40 +1302,14 @@ PX4IO::io_get_status()
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
/* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) {
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
io_handle_battery(regs[2], regs[3]);
#endif
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
/*
regs[3] contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/*
integrate battery over time to get total mAh used
*/
if (_battery_last_timestamp != 0) {
_battery_mamphour_total += battery_status.current_a *
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
}
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
io_handle_vservo(regs[4], regs[5]);
#endif
return ret;
}
@ -1476,9 +1536,53 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
return io_reg_set(page, offset, value);
}
int
PX4IO::print_debug()
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
int io_fd = -1;
if (io_fd < 0) {
io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
}
/* read IO's output */
if (io_fd > 0) {
pollfd fds[1];
fds[0].fd = io_fd;
fds[0].events = POLLIN;
usleep(500);
int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
if (pret > 0) {
int count;
char buf[65];
do {
count = ::read(io_fd, buf, sizeof(buf) - 1);
if (count > 0) {
/* enforce null termination */
buf[count] = '\0';
warnx("IO CONSOLE: %s", buf);
}
} while (count > 0);
}
::close(io_fd);
return 0;
}
#endif
return 1;
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
{
/* get debug level */
int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
uint8_t frame[_max_transfer];
@ -1500,6 +1604,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
} else {
continue;
}
/*
@ -1517,6 +1623,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
/* print mixer chunk */
if (debuglevel > 5 || ret) {
warnx("fmu sent: \"%s\"", msg->text);
/* read IO's output */
print_debug();
}
if (ret) {
log("mixer send error %d", ret);
return ret;
@ -1526,6 +1641,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (buflen > 0);
/* ensure a closing newline */
msg->text[0] = '\n';
msg->text[1] = '\0';
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
retries--;
log("mixer sent");
@ -1997,8 +2118,8 @@ int
PX4IO::set_update_rate(int rate)
{
int interval_ms = 1000 / rate;
if (interval_ms < 3) {
interval_ms = 3;
if (interval_ms < UPDATE_INTERVAL_MIN) {
interval_ms = UPDATE_INTERVAL_MIN;
warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
}
@ -2244,28 +2365,37 @@ test(void)
void
monitor(void)
{
/* clear screen */
printf("\033[2J");
unsigned cancels = 3;
printf("Hit <enter> three times to exit monitor mode\n");
for (;;) {
pollfd fds[1];
fds[0].fd = 0;
fds[0].events = POLLIN;
poll(fds, 1, 500);
poll(fds, 1, 2000);
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
if (cancels-- == 0) {
printf("\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
#warning implement this
if (g_dev != nullptr) {
// if (g_dev != nullptr)
// g_dev->dump_one = true;
printf("\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
}
}
}
@ -2372,7 +2502,7 @@ px4io_main(int argc, char *argv[])
if ((argc > 2)) {
g_dev->set_update_rate(atoi(argv[2]));
} else {
errx(1, "missing argument (50 - 400 Hz)");
errx(1, "missing argument (50 - 500 Hz)");
return 1;
}
exit(0);

36
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -67,6 +67,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_position_control_params.h"
@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow position control] starting\n");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[fpc] started");
uint32_t counter = 0;
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow position control] parameters updated.\n");
mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
}
/* only run controller if position/speed changed */
@ -270,6 +280,8 @@ flow_position_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[])
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
status_changed = true;
/* calc dt */
if(last_time == 0)
{
@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[])
{
flow_sp_sumy = filtered_flow.sumy;
update_flow_sp_sumy = false;
}
}
/* calc new bodyframe speed setpoints */
float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[])
else
{
/* in manual or stabilized state just reset speed and flow sum setpoint */
//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
status_changed = false;
speed_sp.vx = 0.0f;
speed_sp.vy = 0.0f;
flow_sp_sumx = filtered_flow.sumx;
@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[])
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow position control] no attitude received.\n");
mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow position control] initialized.\n");
mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
}
}
}
}
printf("[flow position control] ending now...\n");
mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
thread_running = false;

7
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
local_pos.y = local_pos.y + global_speed[1] * dt;
local_pos.vx = global_speed[0];
local_pos.vy = global_speed[1];
local_pos.xy_valid = true;
local_pos.v_xy_valid = true;
}
else
{
@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
filtered_flow.vy = 0;
local_pos.vx = 0;
local_pos.vy = 0;
local_pos.xy_valid = false;
local_pos.v_xy_valid = false;
}
/* filtering ground distance */
@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* not possible to fly */
sonar_valid = false;
local_pos.z = 0.0f;
local_pos.z_valid = false;
}
else
{
@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
{
local_pos.z = -sonar_new;
}
local_pos.z_valid = true;
}
filtered_flow.timestamp = hrt_absolute_time();

32
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -65,6 +65,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_speed_control_params.h"
@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow speed control] starting\n");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd,"[fsc] started");
uint32_t counter = 0;
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow speed control] parameters updated.\n");
mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
}
/* only run controller if position/speed changed */
@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p;
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
status_changed = true;
/* limit roll and pitch corrections */
if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
{
@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
}
else
{
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
status_changed = false;
/* reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[])
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow speed control] no attitude received.\n");
mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow speed control] initialized.\n");
mavlink_log_info(mavlink_fd,"[fsp] initialized.");
}
}
}
}
printf("[flow speed control] ending now...\n");
mavlink_log_info(mavlink_fd,"[fsc] ending now...");
thread_running = false;

74
src/examples/hwtest/hwtest.c Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 hwtest.c
*
* Simple functional hardware test.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int ex_hwtest_main(int argc, char *argv[]);
int ex_hwtest_main(int argc, char *argv[])
{
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
int i;
float rcvalue = -1.0f;
hrt_abstime stime;
while (true) {
stime = hrt_absolute_time();
while (hrt_absolute_time() - stime < 1000000) {
for (i=0; i<8; i++)
actuators.control[i] = rcvalue;
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
}
warnx("servos set to %.1f", rcvalue);
rcvalue *= -1.0f;
}
return OK;
}

40
src/examples/hwtest/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# 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.
#
############################################################################
#
# Hardware test example application
#
MODULE_COMMAND = ex_hwtest
SRCS = hwtest.c

25
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{
/* following [2], switching on L1 distance */
return math::min(wp_radius, _L1_distance);
return math::max(wp_radius, _L1_distance);
}
bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed_vector)
{
/* this follows the logic presented in [1] */
float eta;
@ -128,7 +129,11 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float distance_A_to_airplane = vector_A_to_airplane.length();
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* extension from [2] */
/* estimate airplane position WRT to B */
math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
/* extension from [2], fly directly to A */
if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
/* calculate eta to fly to waypoint A */
@ -143,6 +148,21 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
// XXX this can be useful as last-resort guard, but is currently not needed
#if 0
} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
/* extension, fly back to waypoint */
/* calculate eta to fly to waypoint B */
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = bearing_wp_b;
#endif
} else {
/* calculate eta to fly along the line between A and B */
@ -162,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
eta = eta1 + eta2;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
}
/* limit angle to +-90 degrees */

8
src/lib/ecl/l1/ecl_l1_pos_controller.h
View File

@ -130,6 +130,14 @@ public:
bool reached_loiter_target();
/**
* Returns true if following a circle (loiter)
*/
bool circle_mode() {
return _circle_mode;
}
/**
* Get the switch distance
*

50
src/modules/commander/commander.cpp
View File

@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
} else {
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (safety->safety_switch_available && !safety->safety_off) {
if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(status, safety, new_arming_state, armed);
arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@ -532,6 +544,9 @@ static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
/* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode));
@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[])
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
}
@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
} else {
// XXX: Add emergency stuff if sensors are lost
@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg)
else
tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
break;
}

29
src/modules/commander/state_machine_helper.cpp
View File

@ -67,7 +67,9 @@ static bool main_state_changed = true;
static bool navigation_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
const struct vehicle_control_mode_s *control_mode,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
* Perform an atomic state update
@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
} else {
/* enforce lockdown in HIL */
if (control_mode->flag_system_hil_enabled) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
switch (new_arming_state) {
case ARMING_STATE_INIT:
@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED) {
|| status->arming_state == ARMING_STATE_ARMED
|| control_mode->flag_system_hil_enabled) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
&& (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
valid_transition = false;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");

2
src/modules/commander/state_machine_helper.h
View File

@ -58,7 +58,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);

4
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main()
pitch_sp = _att_sp.pitch_body;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral)
_roll_ctrl.reset_integrator();
} else {
/*
* Scale down roll and pitch as the setpoints are radians

31
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -129,9 +130,11 @@ private:
int _accel_sub; /**< body frame accelerations */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* publications */
_attitude_sp_pub(-1),
_nav_capabilities_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_groundspeed_undershoot(0.0f),
_global_pos_valid(false)
{
_nav_capabilities.turn_distance = 0.0f;
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// else integrators should be constantly reset.
if (_control_mode.flag_control_position_enabled) {
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* execute navigation once we have a setpoint */
if (_setpoint_valid) {
@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else {
/*
* No valid next waypoint, go for heading hold.
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp.setX(global_triplet.current.lat / 1e7f);
@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
@ -968,6 +982,21 @@ FixedwingPositionControl::task_main()
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
/* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
if (_nav_capabilities_pub > 0) {
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
} else {
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
}
}
}
}

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

@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[])
lowspeed_counter++;
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
/* sleep quarter the time */
usleep(25000);
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
/* sleep quarter the time */
usleep(25000);
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
/* sleep quarter the time */
usleep(25000);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
if (baudrate > 57600) {
mavlink_pm_queued_send();

33
src/modules/mavlink/missionlib.c
View File

@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
sp->param2 = param2;
sp->param3 = param3;
sp->param4 = param4;
/* define the turn distance */
float orbit = 15.0f;
if (command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = param2;
} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
command == (int)MAV_CMD_NAV_LOITER_TIME ||
command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = param3;
} else {
// XXX set default orbit via param
// 15 initialized above
}
sp->turn_distance_xy = orbit;
sp->turn_distance_z = orbit;
}
/**
@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
int last_setpoint_index = -1;
bool last_setpoint_valid = false;
/* at first waypoint, but cycled once through mission */
if (index == 0 && last_waypoint_index > 0) {
last_setpoint_index = last_waypoint_index;
} else {
if (index > 0) {
last_setpoint_index = index - 1;
}
@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
int next_setpoint_index = -1;
bool next_setpoint_valid = false;
/* at last waypoint, try to re-loop through mission as default */
if (index == (wpm->size - 1) && wpm->size > 1) {
next_setpoint_index = 0;
} else if (wpm->size > 1) {
/* next waypoint */
if (wpm->size > 1) {
next_setpoint_index = index + 1;
}

21
src/modules/mavlink/orb_listener.c
View File

@ -67,6 +67,7 @@ extern bool gcs_link;
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
@ -122,6 +123,7 @@ static void l_optical_flow(const struct listener *l);
static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static void l_nav_cap(const struct listener *l);
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@ -148,6 +150,7 @@ static const struct listener listeners[] = {
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
{l_home, &mavlink_subs.home_sub, 0},
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
{l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@ -691,6 +694,19 @@ l_airspeed(const struct listener *l)
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
void
l_nav_cap(const struct listener *l)
{
orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
hrt_absolute_time() / 1000,
"turn dist",
nav_cap.turn_distance);
}
static void *
uorb_receive_thread(void *arg)
{
@ -837,6 +853,11 @@ uorb_receive_start(void)
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
/* --- NAVIGATION CAPABILITIES --- */
mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */
nav_cap.turn_distance = 0.0f;
/* start the listener loop */
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);

5
src/modules/mavlink/orb_topics.h
View File

@ -67,6 +67,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/navigation_capabilities.h>
struct mavlink_subscriptions {
int sensor_sub;
@ -92,6 +93,7 @@ struct mavlink_subscriptions {
int rates_setpoint_sub;
int home_sub;
int airspeed_sub;
int navigation_capabilities_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos;
/** Local position */
extern struct vehicle_local_position_s local_pos;
/** navigation capabilities */
extern struct navigation_capabilities_s nav_cap;
/** Vehicle status */
extern struct vehicle_status_s v_status;

44
src/modules/mavlink/waypoints.c
View File

@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
*
* The distance calculation is based on the WGS84 geoid (GPS)
*/
float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
{
static uint16_t counter;
if (seq < wpm->size) {
mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
float dxy = radius_earth * c;
float dz = alt - wp->z;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
} else {
return -1.0f;
}
counter++;
}
/*
* Calculate distance in local frame (NED)
*/
float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
{
if (seq < wpm->size) {
mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
float dy = (cur->y - y);
float dz = (cur->z - z);
*dist_xy = sqrtf(dx * dx + dy * dy);
*dist_z = fabsf(dz);
return sqrtf(dx * dx + dy * dy + dz * dz);
} else {
@ -303,11 +306,13 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
}
}
void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
{
static uint16_t counter;
if (!global_pos->valid && !local_pos->xy_valid) {
if ((!global_pos->valid && !local_pos->xy_valid) ||
/* no waypoint */
wpm->size == 0) {
/* nothing to check here, return */
return;
}
@ -330,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
orbit = 15.0f;
}
/* keep vertical orbit */
float vertical_switch_distance = orbit;
/* Take the larger turn distance - orbit or turn_distance */
if (orbit < turn_distance)
orbit = turn_distance;
int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
/* Check if conditions of mission item are satisfied */
// XXX TODO
}
if (dist >= 0.f && dist <= orbit) {
if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
wpm->pos_reached = true;
}
// check if required yaw reached
float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
@ -463,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
{
/* check for timed-out operations */
if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@ -486,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
}
}
check_waypoints_reached(now, global_position, local_position);
check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
return OK;
}
@ -1009,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
}
}
check_waypoints_reached(now, global_pos, local_pos);
// check_waypoints_reached(now, global_pos, local_pos);
}

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

@ -55,6 +55,7 @@
#include <stdbool.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/navigation_capabilities.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage;
void mavlink_wpm_init(mavlink_wpm_storage *state);
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
struct vehicle_local_position_s *local_pos);
struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
struct vehicle_local_position_s *local_pos);

30
src/modules/px4iofirmware/registers.c
View File

@ -671,27 +671,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
#ifdef ADC_VSERVO
/* PX4IO_P_STATUS_VSERVO */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* XXX pending measurements
*
* slope = xxx
* intercept = xxx
*
* Intercept corrected for best results @ 5.0V.
*/
unsigned counts = adc_measure(ADC_VSERVO);
if (counts != 0xffff) {
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
// use 3:1 scaling on 3.3V ADC input
unsigned mV = counts * 9900 / 4096;
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
}
}
#endif
#ifdef ADC_RSSI
/* PX4IO_P_STATUS_VRSSI */
{
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
// use 1:1 scaling on 3.3V ADC input
unsigned mV = counts * 3300 / 4096;
r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
}
}
#endif
/* XXX PX4IO_P_STATUS_VRSSI */
/* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);

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

@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced)
void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
/* only read if publishing */
if (!_publishing)
return;
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {

26
src/modules/systemlib/perf_counter.c
View File

@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle)
}
}
uint64_t
perf_event_count(perf_counter_t handle)
{
if (handle == NULL)
return 0;
switch (handle->type) {
case PC_COUNT:
return ((struct perf_ctr_count *)handle)->event_count;
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
return pce->event_count;
}
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
return pci->event_count;
}
default:
break;
}
return 0;
}
void
perf_print_all(void)
{

8
src/modules/systemlib/perf_counter.h
View File

@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void);
*/
__EXPORT extern void perf_reset_all(void);
/**
* Return current event_count
*
* @param handle The counter returned from perf_alloc.
* @return event_count
*/
__EXPORT extern uint64_t perf_event_count(perf_counter_t handle);
__END_DECLS
#endif

3
src/modules/uORB/objects_common.cpp
View File

@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s);
#include "topics/battery_status.h"
ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/servorail_status.h"
ORB_DEFINE(servorail_status, struct servorail_status_s);
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);

1
src/modules/uORB/topics/differential_pressure.h
View File

@ -53,6 +53,7 @@
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint16_t differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */

67
src/modules/uORB/topics/servorail_status.h Normal file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* 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 servorail_status.h
*
* Definition of the servorail status uORB topic.
*/
#ifndef SERVORAIL_STATUS_H_
#define SERVORAIL_STATUS_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* Servorail voltages and status
*/
struct servorail_status_s {
uint64_t timestamp; /**< microseconds since system boot */
float voltage_v; /**< Servo rail voltage in volts */
float rssi_v; /**< RSSI pin voltage in volts */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(servorail_status);
#endif

2
src/modules/uORB/topics/vehicle_attitude_setpoint.h
View File

@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s
float thrust; /**< Thrust in Newton the power system should generate */
bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */
};
/**

2
src/modules/uORB/topics/vehicle_global_position_setpoint.h
View File

@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s
float param2;
float param3;
float param4;
float turn_distance_xy; /**< The distance on the plane which will mark this as reached */
float turn_distance_z; /**< The distance in Z direction which will mark this as reached */
};
/**

235
src/systemcmds/tests/tests_file.c
View File

@ -54,7 +54,8 @@
int
test_file(int argc, char *argv[])
{
const iterations = 200;
const unsigned iterations = 100;
const unsigned alignments = 65;
/* check if microSD card is mounted */
struct stat buffer;
@ -63,68 +64,190 @@ test_file(int argc, char *argv[])
return 1;
}
uint8_t buf[512];
hrt_abstime start, end;
perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
memset(buf, 0, sizeof(buf));
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
warnx("aligned write - please wait..");
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
if ((0x3 & (uintptr_t)buf))
warnx("memory is unaligned!");
for (unsigned a = 0; a < alignments; a++) {
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
perf_begin(wperf);
write(fd, buf, sizeof(buf));
fsync(fd);
perf_end(wperf);
printf("\n");
warnx("----- alignment test: %u bytes -----", a);
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
hrt_abstime start, end;
//perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
warnx("testing unaligned writes - please wait..");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
//perf_begin(wperf);
int wret = write(fd, write_buf + a, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
warn("WRITE ERROR!");
if ((0x3 & (uintptr_t)(write_buf + a)))
errx(1, "memory is unaligned, align shift: %d", a);
}
fsync(fd);
//perf_end(wperf);
}
end = hrt_absolute_time();
//warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
//perf_print_counter(wperf);
//perf_free(wperf);
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
errx(1, "READ ERROR!");
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j + a]) {
warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
compare_ok = false;
break;
}
}
if (!compare_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
}
}
/*
* ALIGNED WRITES AND UNALIGNED READS
*/
close(fd);
int ret = unlink("/fs/microsd/testfile");
fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
warnx("testing aligned writes - please wait..");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
err(1, "WRITE ERROR!");
}
}
fsync(fd);
warnx("reading data aligned..");
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
bool align_read_ok = true;
/* read back data unaligned */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
err(1, "READ ERROR!");
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
align_read_ok = false;
break;
}
}
if (!align_read_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
}
}
warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR");
warnx("reading data unaligned..");
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
bool unalign_read_ok = true;
int unalign_read_err_count = 0;
memset(read_buf, 0, sizeof(read_buf));
/* read back data unaligned */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf + a, chunk_sizes[c]);
if (rret != chunk_sizes[c]) {
err(1, "READ ERROR!");
}
for (int j = 0; j < chunk_sizes[c]; j++) {
if ((read_buf + a)[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]);
unalign_read_ok = false;
unalign_read_err_count++;
if (unalign_read_err_count > 10)
break;
}
}
if (!unalign_read_ok) {
errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
}
}
ret = unlink("/fs/microsd/testfile");
close(fd);
if (ret)
err(1, "UNLINKING FILE FAILED");
}
}
end = hrt_absolute_time();
warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
perf_print_counter(wperf);
perf_free(wperf);
int ret = unlink("/fs/microsd/testfile");
if (ret)
err(1, "UNLINKING FILE FAILED");
warnx("unaligned write - please wait..");
struct {
uint8_t byte;
uint8_t unaligned[512];
} unaligned_buf;
if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0)
warnx("creating unaligned memory failed.");
wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
perf_begin(wperf);
write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned));
fsync(fd);
perf_end(wperf);
}
end = hrt_absolute_time();
close(fd);
warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
perf_print_counter(wperf);
perf_free(wperf);
/* list directory */
DIR *d;
struct dirent *dir;
DIR *d;
struct dirent *dir;
d = opendir("/fs/microsd");
if (d) {