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

Merge branch 'fmuv2_bringup_new_state_machine_drton' of https://github.com/cvg/Firmware_Private into fmuv2_bringup_new_state_machine_drton

Browse Source
This commit is contained in:
Simon Wilks 2013-08-18 23:12:05 +02:00
commit 44afdbcd65
35 changed files with 731 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
.gitignore vendored
View File

@ -23,10 +23,13 @@ Firmware.sublime-workspace
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/
NuttX
nuttx-configs/px4io-v2/src/.depend
nuttx-configs/px4io-v2/src/Make.dep
nuttx-configs/px4io-v2/src/libboard.a
nuttx-configs/px4io-v1/src/.depend
nuttx-configs/px4io-v1/src/Make.dep
nuttx-configs/px4io-v1/src/libboard.a
/nuttx-configs/px4io-v2/src/.depend
/nuttx-configs/px4io-v2/src/Make.dep
/nuttx-configs/px4io-v2/src/libboard.a
/nuttx-configs/px4io-v1/src/.depend
/nuttx-configs/px4io-v1/src/Make.dep
/nuttx-configs/px4io-v1/src/libboard.a
/NuttX
/Documentation/doxy.log
/Documentation/html/
/Documentation/doxygen*objdb*tmp

13
Documentation/Doxyfile
View File

@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME = NO
# disable (NO) the todo list. This list is created by putting \todo
# commands in the documentation.
GENERATE_TODOLIST = YES
GENERATE_TODOLIST = NO
# The GENERATE_TESTLIST tag can be used to enable (YES) or
# disable (NO) the test list. This list is created by putting \test
# commands in the documentation.
GENERATE_TESTLIST = YES
GENERATE_TESTLIST = NO
# The GENERATE_BUGLIST tag can be used to enable (YES) or
# disable (NO) the bug list. This list is created by putting \bug
# commands in the documentation.
GENERATE_BUGLIST = YES
GENERATE_BUGLIST = NO
# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or
# disable (NO) the deprecated list. This list is created by putting
@ -569,7 +569,7 @@ WARN_LOGFILE = doxy.log
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
INPUT = ../apps
INPUT = ../src
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@ -599,9 +599,8 @@ RECURSIVE = YES
# excluded from the INPUT source files. This way you can easily exclude a
# subdirectory from a directory tree whose root is specified with the INPUT tag.
EXCLUDE = ../dist/ \
../docs/html/ \
html
EXCLUDE = ../src/modules/mathlib/CMSIS \
../src/modules/attitude_estimator_ekf/codegen
# The EXCLUDE_SYMLINKS tag can be used select whether or not files or
# directories that are symbolic links (a Unix filesystem feature) are excluded

27
ROMFS/px4fmu_common/init.d/10_io_f330
View File

@ -44,12 +44,22 @@ param set MAV_TYPE 2
#
mavlink start
usleep 5000
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
pwm -u 400 -m 0xff
if px4io start
then
pwm -u 400 -m 0xff
else
# SOS
tone_alarm 6
fi
#
# Allow PX4IO to recover from midair restarts.
@ -77,11 +87,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start GPS interface (depends on orb)
@ -102,15 +107,7 @@ if [ $BOARD == fmuv1 ]
then
px4io limit 200
sdlog2 start -r 50 -a -b 16
if blinkm start
then
blinkm systemstate
fi
else
px4io limit 400
sdlog2 start -r 200 -a -b 16
if rgbled start
then
#rgbled systemstate
fi
fi

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

@ -81,6 +81,19 @@ else
fi
fi
#
# Start system state indicator
#
if rgbled start
then
echo "Using external RGB Led"
else
if blinkm start
then
blinkm systemstate
fi
fi
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -30,6 +30,7 @@ MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/airspeed

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RXDMA is not set
# CONFIG_UART4_RXDMA is not set
# CONFIG_UART5_RS485 is not set
CONFIG_UART5_RXDMA=y
CONFIG_UART5_RXDMA=n
# CONFIG_USART6_RS485 is not set
CONFIG_USART6_RXDMA=y
# CONFIG_USART7_RXDMA is not set

2
src/drivers/boards/px4fmu-v1/board_config.h
View File

@ -103,6 +103,7 @@ __BEGIN_DECLS
#define PX4_I2C_BUS_ESC 1
#define PX4_I2C_BUS_ONBOARD 2
#define PX4_I2C_BUS_EXPANSION 3
#define PX4_I2C_BUS_LED 3
/*
* Devices on the onboard bus.
@ -112,6 +113,7 @@ __BEGIN_DECLS
#define PX4_I2C_OBDEV_HMC5883 0x1e
#define PX4_I2C_OBDEV_MS5611 0x76
#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
#define PX4_I2C_OBDEV_LED 0x55
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
#define PX4_I2C_OBDEV_PX4IO 0x1a

9
src/drivers/drv_rgbled.h
View File

@ -98,7 +98,14 @@ typedef enum {
RGBLED_COLOR_GREEN,
RGBLED_COLOR_BLUE,
RGBLED_COLOR_WHITE,
RGBLED_COLOR_AMBER
RGBLED_COLOR_AMBER,
RGBLED_COLOR_DIM_RED,
RGBLED_COLOR_DIM_YELLOW,
RGBLED_COLOR_DIM_PURPLE,
RGBLED_COLOR_DIM_GREEN,
RGBLED_COLOR_DIM_BLUE,
RGBLED_COLOR_DIM_WHITE,
RGBLED_COLOR_DIM_AMBER
} rgbled_color_t;
/* enum passed to RGBLED_SET_MODE ioctl()*/

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

@ -94,21 +94,62 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
/**
* The PX4IO class.
*
* Encapsulates PX4FMU to PX4IO communications modeled as file operations.
*/
class PX4IO : public device::CDev
{
public:
/**
* Constructor.
*
* Initialize all class variables.
*/
PX4IO(device::Device *interface);
/**
* Destructor.
*
* Wait for worker thread to terminate.
*/
virtual ~PX4IO();
/**
* Initialize the PX4IO class.
*
* Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
*/
virtual int init();
/**
* IO Control handler.
*
* Handle all IOCTL calls to the PX4IO file descriptor.
*
* @param[in] filp file handle (not used). This function is always called directly through object reference
* @param[in] cmd the IOCTL command
* @param[in] the IOCTL command parameter (optional)
*/
virtual int ioctl(file *filp, int cmd, unsigned long arg);
/**
* write handler.
*
* Handle writes to the PX4IO file descriptor.
*
* @param[in] filp file handle (not used). This function is always called directly through object reference
* @param[in] buffer pointer to the data buffer to be written
* @param[in] len size in bytes to be written
* @return number of bytes written
*/
virtual ssize_t write(file *filp, const char *buffer, size_t len);
/**
* Set the update rate for actuator outputs from FMU to IO.
*
* @param rate The rate in Hz actuator outpus are sent to IO.
* @param[in] rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@ -116,16 +157,16 @@ public:
/**
* Set the battery current scaling and bias
*
* @param amp_per_volt
* @param amp_bias
* @param[in] amp_per_volt
* @param[in] amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
* @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param[in] len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
@ -149,11 +190,21 @@ public:
*/
void print_status();
/**
* Set the DSM VCC is controlled by relay one flag
*
* @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
*/
inline void set_dsm_vcc_ctl(bool enable)
{
_dsm_vcc_ctl = enable;
};
/**
* Get the DSM VCC is controlled by relay one flag
*
* @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
*/
inline bool get_dsm_vcc_ctl()
{
return _dsm_vcc_ctl;
@ -163,60 +214,50 @@ private:
device::Device *_interface;
// XXX
unsigned _hardware;
unsigned _max_actuators;
unsigned _max_controls;
unsigned _max_rc_input;
unsigned _max_relays;
unsigned _max_transfer;
unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///<Maximum relays supported by PX4IO
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
unsigned _update_interval; ///< subscription interval limiting send rate
unsigned _update_interval; ///<Subscription interval limiting send rate
volatile int _task; ///< worker task
volatile bool _task_should_exit;
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
int _mavlink_fd;
int _mavlink_fd; ///<mavlink file descriptor
perf_counter_t _perf_update;
perf_counter_t _perf_update; ///<local performance counter
/* cached IO state */
uint16_t _status;
uint16_t _alarms;
uint16_t _status; ///<Various IO status flags
uint16_t _alarms; ///<Various IO alarms
/* subscribed topics */
int _t_actuators; ///< actuator controls topic
int _t_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode; ///< vehicle control mode topic
int _t_param; ///< parameter update topic
int _t_param; ///<parameter update topic
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
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_safety; ///< status of safety
orb_advert_t _to_input_rc; ///<rc inputs from IO topic
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 topic
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///< mixed outputs
actuator_controls_effective_s _controls_effective; ///< effective controls
actuator_outputs_s _outputs; ///<mixed outputs
actuator_controls_effective_s _controls_effective; ///<effective controls
bool _primary_pwm_device; ///< true if we are the default PWM output
bool _primary_pwm_device; ///<true if we are the default PWM output
float _battery_amp_per_volt;
float _battery_amp_bias;
float _battery_mamphour_total;
uint64_t _battery_last_timestamp;
float _battery_amp_per_volt; ///<current sensor amps/volt
float _battery_amp_bias; ///<current sensor bias
float _battery_mamphour_total;///<amp hours consumed so far
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
/**
* Relay1 is dedicated to controlling DSM receiver power
*/
bool _dsm_vcc_ctl;
/**
* System armed
*/
bool _system_armed;
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
/**
* Trampoline to the worker task
@ -388,8 +429,7 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
_dsm_vcc_ctl(false),
_system_armed(false)
_dsm_vcc_ctl(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -508,7 +548,7 @@ PX4IO::init()
usleep(10000);
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
if ((hrt_absolute_time() - try_start_time)/1000 > 3000) {
log("failed to recover from in-air restart (1), aborting IO driver init.");
return 1;
}
@ -732,7 +772,7 @@ PX4IO::task_main()
// See if bind parameter has been set, and reset it to 0
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
if (dsm_bind_val) {
if (!_system_armed) {
if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@ -853,8 +893,6 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
_system_armed = armed.armed;
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
@ -1481,8 +1519,8 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
@ -1731,7 +1769,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
}
ssize_t
PX4IO::write(file *filp, const char *buffer, size_t len)
PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
/* Make it obvious that file * isn't used here */
{
unsigned count = len / 2;

31
src/drivers/rgbled/rgbled.cpp
View File

@ -294,6 +294,27 @@ RGBLED::set_color(rgbled_color_t color) {
case RGBLED_COLOR_AMBER: // amber
set_rgb(255,20,0);
break;
case RGBLED_COLOR_DIM_RED: // red
set_rgb(90,0,0);
break;
case RGBLED_COLOR_DIM_YELLOW: // yellow
set_rgb(80,30,0);
break;
case RGBLED_COLOR_DIM_PURPLE: // purple
set_rgb(45,0,45);
break;
case RGBLED_COLOR_DIM_GREEN: // green
set_rgb(0,90,0);
break;
case RGBLED_COLOR_DIM_BLUE: // blue
set_rgb(0,0,90);
break;
case RGBLED_COLOR_DIM_WHITE: // white
set_rgb(30,30,30);
break;
case RGBLED_COLOR_DIM_AMBER: // amber
set_rgb(80,20,0);
break;
default:
warnx("color unknown");
break;
@ -435,7 +456,8 @@ rgbled_main(int argc, char *argv[])
int rgbledadr = ADDR; /* 7bit */
int ch;
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
@ -447,9 +469,8 @@ rgbled_main(int argc, char *argv[])
rgbled_usage();
}
}
argc -= optind;
argv += optind;
const char *verb = argv[0];
const char *verb = argv[1];
int fd;
int ret;
@ -528,7 +549,7 @@ rgbled_main(int argc, char *argv[])
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
if (argc < 4) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
}
rgbled_rgbset_t v;

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

@ -52,6 +52,7 @@
#include <errno.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
#include <string.h>
#include <math.h>
#include <poll.h>
@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
/* if connected via USB */
static bool on_usb_power = false;
/* tasks waiting for low prio thread */
@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st
void print_reject_mode(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
/**
@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\tcommander is running\n");
print_status();
} else {
warnx("\tcommander not started\n");
@ -265,6 +271,47 @@ void usage(const char *reason)
exit(1);
}
void print_status() {
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
struct vehicle_status_s state;
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
const char* armed_str;
switch (state.arming_state) {
case ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
default:
armed_str = "ERR: UNKNOWN STATE";
break;
}
warnx("arming: %s", armed_str);
}
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@ -865,9 +912,14 @@ int commander_thread_main(int argc, char *argv[])
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
warnx("bat remaining: %2.2f", status.battery_remaining);
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
@ -930,9 +982,9 @@ int commander_thread_main(int argc, char *argv[])
/* store current state to reason later about a state change */
// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
bool global_pos_valid = status.condition_global_position_valid;
bool local_pos_valid = status.condition_local_position_valid;
bool airspeed_valid = status.condition_airspeed_valid;
// bool global_pos_valid = status.condition_global_position_valid;
// bool local_pos_valid = status.condition_local_position_valid;
// bool airspeed_valid = status.condition_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
@ -1259,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
}
/* publish vehicle status at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.counter++;
status.timestamp = t;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
control_mode.timestamp = t;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
armed.timestamp = t;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
/* play arming and battery warning tunes */
@ -1362,22 +1418,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (armed->armed) {
/* armed, solid */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[0] = RGBLED_COLOR_AMBER;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[0] = RGBLED_COLOR_RED;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[0] = RGBLED_COLOR_GREEN;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
}
pattern.duration[0] = 1000;
} else if (armed->ready_to_arm) {
for (i=0; i<3; i++) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[i*2] = RGBLED_COLOR_AMBER;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[i*2] = RGBLED_COLOR_RED;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i*2] = RGBLED_COLOR_GREEN;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[i*2] = 200;
@ -1385,13 +1441,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
pattern.duration[i*2+1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i*2] = RGBLED_COLOR_BLUE;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i*2] = 1000;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 800;
} else {
for (i=3; i<6; i++) {
pattern.color[i*2] = RGBLED_COLOR_BLUE;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i*2] = 100;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 100;
@ -1402,7 +1458,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
} else {
for (i=0; i<3; i++) {
pattern.color[i*2] = RGBLED_COLOR_RED;
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
pattern.duration[i*2] = 200;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 200;

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

@ -69,6 +69,11 @@ 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)
{
/*
* Perform an atomic state update
*/
irqstate_t flags = irqsave();
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
@ -114,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
{
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
armed->ready_to_arm = true;
}
break;
@ -168,11 +173,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
if (ret == TRANSITION_CHANGED) {
status->arming_state = new_arming_state;
arming_state_changed = true;
} else {
warnx("arming transition rejected");
}
}
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED)
warnx("arming transition rejected");
return ret;
}

408
src/modules/px4iofirmware/dsm.c
View File

@ -48,161 +48,44 @@
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#define DSM_FRAME_SIZE 16
#define DSM_FRAME_CHANNELS 7
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
static int dsm_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static uint8_t frame[DSM_FRAME_SIZE];
static unsigned partial_frame_count;
static unsigned channel_shift;
unsigned dsm_frame_drops;
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
static void dsm_guess_format(bool reset);
static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
int
dsm_init(const char *device)
{
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
if (dsm_fd >= 0) {
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(dsm_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(dsm_fd, TCSANOW, &t);
/* initialise the decoder */
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
/* reset the format detector */
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
return dsm_fd;
}
void
dsm_bind(uint16_t cmd, int pulses)
{
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
if (dsm_fd < 0)
return;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// XXX implement
#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
#else
switch (cmd) {
case dsm_bind_power_down:
// power down DSM satellite
POWER_RELAY1(0);
break;
case dsm_bind_power_up:
POWER_RELAY1(1);
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
stm32_configgpio(usart1RxAsOutp);
break;
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(25);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(25);
}
break;
case dsm_bind_reinit_uart:
// Restore USART rx pin
stm32_configgpio(GPIO_USART1_RX);
break;
}
#endif
}
bool
dsm_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
/*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 11ms; with 16 bytes at 115200bps
* frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) {
dsm_frame_drops++;
partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current frame.
*/
ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return false;
last_rx_time = now;
/*
* Add bytes to the current frame
*/
partial_frame_count += ret;
/*
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
return false;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
partial_frame_count = 0;
return dsm_decode(now, values, num_values);
}
static int dsm_fd = -1; /**< File handle to the DSM UART */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
/**
* Attempt to decode a single channel raw channel datum
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
*
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
*
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*
* Upon receiving a full dsm frame we attempt to decode it
*
* @param[in] raw 16 bit raw channel value from dsm frame
* @param[in] shift position of channel number in raw data
* @param[out] channel pointer to returned channel number
* @param[out] value pointer to returned channel value
* @return true=raw value successfully decoded
*/
static bool
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
@ -220,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
return true;
}
/**
* Attempt to guess if receiving 10 or 11 bit channel values
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
static void
dsm_guess_format(bool reset)
{
@ -232,14 +120,14 @@ dsm_guess_format(bool reset)
cs10 = 0;
cs11 = 0;
samples = 0;
channel_shift = 0;
dsm_channel_shift = 0;
return;
}
/* scan the channels in the current frame in both 10- and 11-bit mode */
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &frame[2 + (2 * i)];
uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
@ -250,10 +138,10 @@ dsm_guess_format(bool reset)
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel);
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
/* wait until we have seen plenty of frames - 2 should normally be enough */
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5)
return;
@ -289,13 +177,13 @@ dsm_guess_format(bool reset)
}
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
dsm_channel_shift = 11;
debug("DSM: 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
dsm_channel_shift = 10;
debug("DSM: 10-bit format");
return;
}
@ -305,27 +193,136 @@ dsm_guess_format(bool reset)
dsm_guess_format(true);
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
{
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
if (dsm_fd >= 0) {
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(dsm_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(dsm_fd, TCSANOW, &t);
/* initialise the decoder */
dsm_partial_frame_count = 0;
dsm_last_rx_time = hrt_absolute_time();
/* reset the format detector */
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
return dsm_fd;
}
/**
* Handle DSM satellite receiver bind mode handler
*
* @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
* @param[in] pulses Number of pulses for dsm_bind_send_pulses command
*/
void
dsm_bind(uint16_t cmd, int pulses)
{
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0)
return;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
// XXX implement
#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
#else
switch (cmd) {
case dsm_bind_power_down:
/*power down DSM satellite*/
POWER_RELAY1(0);
break;
case dsm_bind_power_up:
/*power up DSM satellite*/
POWER_RELAY1(1);
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
/*Set UART RX pin to active output mode*/
stm32_configgpio(usart1RxAsOutp);
break;
case dsm_bind_send_pulses:
/*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(25);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(25);
}
break;
case dsm_bind_reinit_uart:
/*Restore USART RX pin to RS232 receive mode*/
stm32_configgpio(GPIO_USART1_RX);
break;
}
#endif
}
/**
* Decode the entire dsm frame (all contained channels)
*
* @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned
* @return true=DSM frame successfully decoded, false=no update
*/
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/*
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
dsm_guess_format(true);
/* we have received something we think is a frame */
last_frame_time = frame_time;
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/* if we don't know the frame format, update the guessing state machine */
if (channel_shift == 0) {
/* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) {
dsm_guess_format(false);
return false;
}
@ -337,17 +334,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
* Each channel is a 16-bit unsigned value containing either a 10-
* or 11-bit channel value and a 4-bit channel number, shifted
* either 10 or 11 bits. The MSB may also be set to indicate the
* second frame in variants of the protocol where more than
* second dsm_frame in variants of the protocol where more than
* seven channels are being transmitted.
*/
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
uint8_t *dp = &frame[2 + (2 * i)];
uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
continue;
/* ignore channels out of range */
@ -359,7 +356,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
*num_values = channel + 1;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
if (dsm_channel_shift == 11)
value /= 2;
value += 998;
@ -390,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
values[channel] = value;
}
if (channel_shift == 11)
if (dsm_channel_shift == 11)
*num_values |= 0x8000;
/*
@ -398,3 +395,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
*/
return true;
}
/**
* Called periodically to check for input data from the DSM UART
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
* Upon receiving a full dsm frame we attempt to decode it.
*
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
/*
*/
now = hrt_absolute_time();
if ((now - dsm_last_rx_time) > 5000) {
if (dsm_partial_frame_count > 0) {
dsm_frame_drops++;
dsm_partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current dsm frame.
*/
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return false;
dsm_last_rx_time = now;
/*
* Add bytes to the current dsm frame
*/
dsm_partial_frame_count += ret;
/*
* If we don't have a full dsm frame, return
*/
if (dsm_partial_frame_count < DSM_FRAME_SIZE)
return false;
/*
* Great, it looks like we might have a dsm frame. Go ahead and
* decode it.
*/
dsm_partial_frame_count = 0;
return dsm_decode(now, values, num_values);
}

2
src/modules/px4iofirmware/protocol.h
View File

@ -74,7 +74,7 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
#define PX4IO_PROTOCOL_VERSION 2
#define PX4IO_PROTOCOL_VERSION 3
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0

2
src/modules/px4iofirmware/px4io.h
View File

@ -198,7 +198,7 @@ extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values);

3
src/modules/sensors/sensor_params.c
View File

@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);

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

@ -50,6 +50,7 @@
#include <stdio.h>
#include <errno.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <nuttx/analog/adc.h>
@ -137,6 +138,77 @@
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
/**
* Enum for board and external compass rotations.
* This enum maps from board attitude to airframe attitude.
*/
enum Rotation {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
ROTATION_YAW_135 = 3,
ROTATION_YAW_180 = 4,
ROTATION_YAW_225 = 5,
ROTATION_YAW_270 = 6,
ROTATION_YAW_315 = 7,
ROTATION_ROLL_180 = 8,
ROTATION_ROLL_180_YAW_45 = 9,
ROTATION_ROLL_180_YAW_90 = 10,
ROTATION_ROLL_180_YAW_135 = 11,
ROTATION_PITCH_180 = 12,
ROTATION_ROLL_180_YAW_225 = 13,
ROTATION_ROLL_180_YAW_270 = 14,
ROTATION_ROLL_180_YAW_315 = 15,
ROTATION_ROLL_90 = 16,
ROTATION_ROLL_90_YAW_45 = 17,
ROTATION_ROLL_90_YAW_90 = 18,
ROTATION_ROLL_90_YAW_135 = 19,
ROTATION_ROLL_270 = 20,
ROTATION_ROLL_270_YAW_45 = 21,
ROTATION_ROLL_270_YAW_90 = 22,
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_MAX
};
typedef struct
{
uint16_t roll;
uint16_t pitch;
uint16_t yaw;
} rot_lookup_t;
const rot_lookup_t rot_lookup[] =
{
{ 0, 0, 0 },
{ 0, 0, 45 },
{ 0, 0, 90 },
{ 0, 0, 135 },
{ 0, 0, 180 },
{ 0, 0, 225 },
{ 0, 0, 270 },
{ 0, 0, 315 },
{180, 0, 0 },
{180, 0, 45 },
{180, 0, 90 },
{180, 0, 135 },
{ 0, 180, 0 },
{180, 0, 225 },
{180, 0, 270 },
{180, 0, 315 },
{ 90, 0, 0 },
{ 90, 0, 45 },
{ 90, 0, 90 },
{ 90, 0, 135 },
{270, 0, 0 },
{270, 0, 45 },
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
};
/**
* Sensor app start / stop handling function
*
@ -189,8 +261,8 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@ -210,13 +282,15 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
float max[_rc_max_chan_count];
float rev[_rc_max_chan_count];
float dz[_rc_max_chan_count];
// float ex[_rc_max_chan_count];
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
@ -227,6 +301,9 @@ private:
float accel_scale[3];
float diff_pres_offset_pa;
int board_rotation;
int external_mag_rotation;
int rc_type;
int rc_map_roll;
@ -264,7 +341,6 @@ private:
param_t max[_rc_max_chan_count];
param_t rev[_rc_max_chan_count];
param_t dz[_rc_max_chan_count];
// param_t ex[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
@ -304,6 +380,9 @@ private:
param_t battery_voltage_scaling;
param_t board_rotation;
param_t external_mag_rotation;
} _parameter_handles; /**< handles for interesting parameters */
@ -312,6 +391,11 @@ private:
*/
int parameters_update();
/**
* Get the rotation matrices
*/
void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
/**
* Do accel-related initialisation.
*/
@ -448,7 +532,10 @@ Sensors::Sensors() :
_diff_pres_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3,3),
_external_mag_rotation(3,3)
{
/* basic r/c parameters */
@ -538,6 +625,10 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
/* fetch initial parameter values */
parameters_update();
}
@ -724,9 +815,33 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
return OK;
}
void
Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
{
/* first set to zero */
rot_matrix->Matrix::zero(3,3);
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
math::EulerAngles euler(roll, pitch, yaw);
math::Dcm R(euler);
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
(*rot_matrix)(i,j) = R(i, j);
}
void
Sensors::accel_init()
{
@ -750,7 +865,7 @@ Sensors::accel_init()
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
#else
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@ -758,6 +873,9 @@ Sensors::accel_init()
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#else
#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
#endif
warnx("using system accel");
@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
raw.accelerometer_m_s2[0] = accel_report.x;
raw.accelerometer_m_s2[1] = accel_report.y;
raw.accelerometer_m_s2[2] = accel_report.z;
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
vect = _board_rotation*vect;
raw.accelerometer_m_s2[0] = vect(0);
raw.accelerometer_m_s2[1] = vect(1);
raw.accelerometer_m_s2[2] = vect(2);
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
raw.gyro_rad_s[0] = gyro_report.x;
raw.gyro_rad_s[1] = gyro_report.y;
raw.gyro_rad_s[2] = gyro_report.z;
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
vect = _board_rotation*vect;
raw.gyro_rad_s[0] = vect(0);
raw.gyro_rad_s[1] = vect(1);
raw.gyro_rad_s[2] = vect(2);
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
raw.magnetometer_ga[0] = mag_report.x;
raw.magnetometer_ga[1] = mag_report.y;
raw.magnetometer_ga[2] = mag_report.z;
// XXX TODO add support for external mag orientation
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
vect = _board_rotation*vect;
raw.magnetometer_ga[0] = vect(0);
raw.magnetometer_ga[1] = vect(1);
raw.magnetometer_ga[2] = vect(2);
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;

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

@ -37,6 +37,10 @@
* Common object definitions without a better home.
*/
/**
* @defgroup topics List of all uORB topics.
*/
#include <nuttx/config.h>
#include <drivers/drv_orb_dev.h>

12
src/modules/uORB/topics/actuator_controls.h
View File

@ -55,15 +55,25 @@
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_s {
uint64_t timestamp;
float control[NUM_ACTUATOR_CONTROLS];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
#endif
#endif

9
src/modules/uORB/topics/actuator_controls_effective.h
View File

@ -53,11 +53,20 @@
#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
/**
* @addtogroup topics
* @{
*/
struct actuator_controls_effective_s {
uint64_t timestamp;
float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
};
/**
* @}
*/
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_effective_0);
ORB_DECLARE(actuator_controls_effective_1);

9
src/modules/uORB/topics/actuator_outputs.h
View File

@ -52,12 +52,21 @@
#define NUM_ACTUATOR_OUTPUTS 16
#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */
/**
* @addtogroup topics
* @{
*/
struct actuator_outputs_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
int noutputs; /**< valid outputs */
};
/**
* @}
*/
/* actuator output sets; this list can be expanded as more drivers emerge */
ORB_DECLARE(actuator_outputs_0);
ORB_DECLARE(actuator_outputs_1);

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

@ -47,6 +47,7 @@
/**
* @addtogroup topics
* @{
*/
/**

11
src/modules/uORB/topics/esc_status.h
View File

@ -51,10 +51,6 @@
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* The number of ESCs supported.
* Current (Q2/2013) we support 8 ESCs,
@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE {
};
/**
*
* @addtogroup topics
* @{
*/
/**
* Electronic speed controller status.
*/
struct esc_status_s
{

10
src/modules/uORB/topics/mission.h
View File

@ -46,11 +46,6 @@
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
enum NAV_CMD {
NAV_CMD_WAYPOINT = 0,
NAV_CMD_LOITER_TURN_COUNT,
@ -61,6 +56,11 @@ enum NAV_CMD {
NAV_CMD_TAKEOFF
};
/**
* @addtogroup topics
* @{
*/
/**
* Global position setpoint in WGS84 coordinates.
*

10
src/modules/uORB/topics/offboard_control_setpoint.h
View File

@ -43,11 +43,6 @@
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Off-board control inputs.
*
@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
};
/**
* @addtogroup topics
* @{
*/
struct offboard_control_setpoint_s {
uint64_t timestamp;

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

@ -46,6 +46,7 @@
/**
* @addtogroup topics
* @{
*/
/**

9
src/modules/uORB/topics/parameter_update.h
View File

@ -42,11 +42,20 @@
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct parameter_update_s {
/** time at which the latest parameter was updated */
uint64_t timestamp;
};
/**
* @}
*/
ORB_DECLARE(parameter_update);
#endif

10
src/modules/uORB/topics/rc_channels.h
View File

@ -45,11 +45,6 @@
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* The number of RC channel inputs supported.
* Current (Q1/2013) radios support up to 18 channels,
@ -85,6 +80,11 @@ enum RC_CHANNELS_FUNCTION
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
};
/**
* @addtogroup topics
* @{
*/
struct rc_channels_s {
uint64_t timestamp; /**< In microseconds since boot time. */

10
src/modules/uORB/topics/sensor_combined.h
View File

@ -46,17 +46,17 @@
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
enum MAGNETOMETER_MODE {
MAGNETOMETER_MODE_NORMAL = 0,
MAGNETOMETER_MODE_POSITIVE_BIAS,
MAGNETOMETER_MODE_NEGATIVE_BIAS
};
/**
* @addtogroup topics
* @{
*/
/**
* Sensor readings in raw and SI-unit form.
*

8
src/modules/uORB/topics/subsystem_info.h
View File

@ -50,10 +50,6 @@
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
*/
enum SUBSYSTEM_TYPE
{
SUBSYSTEM_TYPE_GYRO = 1,
@ -75,6 +71,10 @@ enum SUBSYSTEM_TYPE
SUBSYSTEM_TYPE_RANGEFINDER = 131072
};
/**
* @addtogroup topics
*/
/**
* State of individual sub systems
*/

9
src/modules/uORB/topics/telemetry_status.h
View File

@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
TELEMETRY_STATUS_RADIO_TYPE_WIRE
};
/**
* @addtogroup topics
* @{
*/
struct telemetry_status_s {
uint64_t timestamp;
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
@ -62,6 +67,10 @@ struct telemetry_status_s {
uint8_t txbuf; /**< how full the tx buffer is as a percentage */
};
/**
* @}
*/
ORB_DECLARE(telemetry_status);
#endif /* TOPIC_TELEMETRY_STATUS_H */

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

@ -48,6 +48,7 @@
/**
* @addtogroup topics
* @{
*/
/**

9
src/modules/uORB/topics/vehicle_command.h
View File

@ -45,11 +45,6 @@
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Commands for commander app.
*
@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT
VEHICLE_CMD_RESULT_ENUM_END=5, /* | */
};
/**
* @addtogroup topics
* @{
*/
struct vehicle_command_s
{

4
src/modules/uORB/topics/vehicle_global_position_set_triplet.h
View File

@ -60,8 +60,8 @@
*/
struct vehicle_global_position_set_triplet_s
{
bool previous_valid;
bool next_valid;
bool previous_valid; /**< flag indicating previous position is valid */
bool next_valid; /**< flag indicating next position is valid */
struct vehicle_global_position_setpoint_s previous;
struct vehicle_global_position_setpoint_s current;

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
};
/**
* @addtogroup topics
* @{
*/
/**
* state machine / state of vehicle.