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

WIP

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 14:38:43 -04:00
parent 9d1aeb6aa7
commit 23011ac404
41 changed files with 145 additions and 1463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/holybro/pix32v5/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/mro/x21-777/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/mro/x21/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v2/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v3/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v3/src/board_config.h
-6
View File
@@ -159,12 +159,6 @@
#define BOARD_HAS_ON_RESET 1
/* Internal IMU Heater
*
* Connected to the IO MCU; tell compiler to enable support
*/
#define PX4IO_HEATER_ENABLED
__BEGIN_DECLS
/****************************************************************************************************
boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v5/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v6c/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
BIN
View File
Binary file not shown.
msg/actuator_armed.msg
-1
View File
@@ -2,7 +2,6 @@ uint64 timestamp # time since system start (microseconds)
bool armed # Set to true if system is armed
bool prearmed # Set to true if the actuator safety is disabled but motors are not armed
bool ready_to_arm # Set to true if system is ready to be armed
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
bool manual_lockdown # Set to true if manual throttle kill switch is engaged
bool force_failsafe # Set to true if the actuators are forced to the failsafe position
msg/heater_status.msg
-4
View File
@@ -14,7 +14,3 @@ uint32 controller_time_on_usec
float32 proportional_value
float32 integrator_value
float32 feed_forward_value
uint8 MODE_GPIO = 1
uint8 MODE_PX4IO = 2
uint8 mode
msg/px4io_status.msg
+3 -13
View File
@@ -6,19 +6,16 @@ float32 voltage_v # Servo rail voltage in volts
float32 rssi_v # RSSI pin voltage in volts
# PX4IO status flags (PX4IO_P_STATUS_FLAGS)
bool status_arm_sync
bool status_failsafe
bool status_fmu_initialized
bool status_fmu_ok
bool status_init_ok
bool status_outputs_armed
bool status_raw_pwm
bool status_rc_ok
bool status_rc_dsm
bool status_rc_ppm
bool status_rc_sbus
bool status_rc_st24
bool status_rc_sumd
bool status_safety_button_event # px4io safety button was pressed for longer than 1 second
# PX4IO alarms (PX4IO_P_STATUS_ALARMS)
@@ -26,18 +23,11 @@ bool alarm_pwm_error
bool alarm_rc_lost
# PX4IO arming (PX4IO_P_SETUP_ARMING)
bool arming_failsafe_custom
bool arming_fmu_armed
bool arming_fmu_prearmed
bool arming_force_failsafe
bool arming_io_arm_ok
bool arming_lockdown
bool arming_termination_failsafe
uint16[8] pwm
uint16[8] pwm_disarmed
uint16[8] pwm_failsafe
uint16[8] pwm_rate_hz
uint16[4] pwm_group_rate_hz
uint16[18] raw_inputs
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h
-1
View File
@@ -117,7 +117,6 @@ public:
virtual ~ArchPX4IOSerial();
virtual int init();
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
/**
platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp
-64
View File
@@ -158,70 +158,6 @@ ArchPX4IOSerial::init()
return 0;
}
int
ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case 1: /* XXX magic number - test operation */
switch (arg) {
case 0:
syslog(LOG_INFO, "test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
for (;;) {
while (!(rSR & USART_SR_TXE))
;
rDR = 0x55;
}
return 0;
case 1: {
unsigned fails = 0;
for (unsigned count = 0;; count++) {
uint16_t value = count & 0xffff;
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
fails++;
}
if (count >= 5000) {
syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
perf_print_counter(_pc_txns);
perf_print_counter(_pc_retries);
perf_print_counter(_pc_timeouts);
perf_print_counter(_pc_crcerrs);
perf_print_counter(_pc_dmaerrs);
perf_print_counter(_pc_protoerrs);
perf_print_counter(_pc_uerrs);
perf_print_counter(_pc_idle);
perf_print_counter(_pc_badidle);
count = 0;
}
}
return 0;
}
case 2:
syslog(LOG_INFO, "test 2\n");
return 0;
}
default:
break;
}
return -1;
}
int
ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
{
platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp
-64
View File
@@ -170,70 +170,6 @@ ArchPX4IOSerial::init()
return 0;
}
int
ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case 1: /* XXX magic number - test operation */
switch (arg) {
case 0:
syslog(LOG_INFO, "test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
for (;;) {
while (!(rISR & USART_ISR_TXE))
;
rTDR = 0x55;
}
return 0;
case 1: {
unsigned fails = 0;
for (unsigned count = 0;; count++) {
uint16_t value = count & 0xffff;
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
fails++;
}
if (count >= 5000) {
syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
perf_print_counter(_pc_txns);
perf_print_counter(_pc_retries);
perf_print_counter(_pc_timeouts);
perf_print_counter(_pc_crcerrs);
perf_print_counter(_pc_dmaerrs);
perf_print_counter(_pc_protoerrs);
perf_print_counter(_pc_uerrs);
perf_print_counter(_pc_idle);
perf_print_counter(_pc_badidle);
count = 0;
}
}
return 0;
}
case 2:
syslog(LOG_INFO, "test 2\n");
return 0;
}
default:
break;
}
return -1;
}
int
ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
{
platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp
-64
View File
@@ -208,70 +208,6 @@ ArchPX4IOSerial::init()
return 0;
}
int
ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case 1: /* XXX magic number - test operation */
switch (arg) {
case 0:
syslog(LOG_INFO, "test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
for (;;) {
while (!(rISR & USART_ISR_TXE))
;
rTDR = 0x55;
}
return 0;
case 1: {
unsigned fails = 0;
for (unsigned count = 0;; count++) {
uint16_t value = count & 0xffff;
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
fails++;
}
if (count >= 5000) {
syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
perf_print_counter(_pc_txns);
perf_print_counter(_pc_retries);
perf_print_counter(_pc_timeouts);
perf_print_counter(_pc_crcerrs);
perf_print_counter(_pc_dmaerrs);
perf_print_counter(_pc_protoerrs);
perf_print_counter(_pc_uerrs);
perf_print_counter(_pc_idle);
perf_print_counter(_pc_badidle);
count = 0;
}
}
return 0;
}
case 2:
syslog(LOG_INFO, "test 2\n");
return 0;
}
default:
break;
}
return -1;
}
int
ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
{
src/drivers/drv_io_heater.h
-65
View File
@@ -1,65 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 drv_io_heater.h
*
* IO IMU heater control interface (PX4IO)
*/
#ifndef _DRV_IO_HEATER_H
#define _DRV_IO_HEATER_H
#include <stdint.h>
#include <sys/ioctl.h>
/*
* ioctl() definitions
*/
#define IO_HEATER_DEVICE_PATH "/dev/px4io"
#define _IO_HEATER_BASE (0x2e00)
#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0)
/* ... to IOX_SET_VALUE + 8 */
/* enum passed to IO_HEATER_CONTROL ioctl()*/
enum IO_HEATER_MODE {
HEATER_MODE_OFF = 0,
HEATER_MODE_ON = 1,
HEATER_MODE_DISABLED = -1
};
#endif
src/drivers/drv_pwm_output.h
-3
View File
@@ -111,9 +111,6 @@ __BEGIN_DECLS
*/
#define _PWM_SERVO_BASE 0x2a00
/** start DSM bind */
#define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10)
/** specific rates for configuring the timer for OneShot or PWM */
#define PWM_RATE_ONESHOT 0u
#define PWM_RATE_LOWER_LIMIT 1u
src/drivers/drv_sbus.h
-58
View File
@@ -1,58 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_sbus.h
*
* Futaba S.BUS / S.BUS 2 compatible interface.
*/
#ifndef _DRV_SBUS_H
#define _DRV_SBUS_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_orb_dev.h"
/**
* Path for the default S.BUS device
*/
#define SBUS0_DEVICE_PATH "/dev/sbus0"
#define _SBUS_BASE 0x2c00
/** Enable S.BUS version 1 / 2 output (0 to disable) */
#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0)
#endif /* _DRV_SBUS_H */
src/drivers/heater/heater.cpp
-81
View File
@@ -46,34 +46,11 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_io_heater.h>
#if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED)
// Heater on some boards is on IO MCU
// Use ioctl calls to IO driver to turn heater on/off
# define HEATER_PX4IO
#else
// Use direct calls to turn GPIO pin on/off
# ifndef GPIO_HEATER_OUTPUT
# error "To use the heater driver, the board_config.h must define and initialize GPIO_HEATER_OUTPUT"
# endif
# define HEATER_GPIO
#endif
Heater::Heater() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
#ifdef HEATER_PX4IO
_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
if (_io_fd < 0) {
PX4_ERR("Unable to open heater device path");
return;
}
#endif
_heater_status_pub.advertise();
}
@@ -96,65 +73,23 @@ int Heater::custom_command(int argc, char *argv[])
void Heater::disable_heater()
{
// Reset heater to off state.
#ifdef HEATER_PX4IO
if (_io_fd >= 0) {
px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
}
#endif
#ifdef HEATER_GPIO
px4_arch_configgpio(GPIO_HEATER_OUTPUT);
#endif
}
void Heater::initialize_heater_io()
{
// Initialize heater to off state.
#ifdef HEATER_PX4IO
if (_io_fd < 0) {
_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
}
if (_io_fd >= 0) {
px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
}
#endif
#ifdef HEATER_GPIO
px4_arch_configgpio(GPIO_HEATER_OUTPUT);
#endif
}
void Heater::heater_off()
{
#ifdef HEATER_PX4IO
if (_io_fd >= 0) {
px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
}
#endif
#ifdef HEATER_GPIO
HEATER_OUTPUT_EN(false);
#endif
}
void Heater::heater_on()
{
#ifdef HEATER_PX4IO
if (_io_fd >= 0) {
px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
}
#endif
#ifdef HEATER_GPIO
HEATER_OUTPUT_EN(true);
#endif
}
bool Heater::initialize_topics()
@@ -182,14 +117,6 @@ bool Heater::initialize_topics()
void Heater::Run()
{
if (should_exit()) {
#if defined(HEATER_PX4IO)
// must be closed from wq thread
if (_io_fd >= 0) {
px4_close(_io_fd);
}
#endif
exit_and_cleanup();
return;
}
@@ -260,14 +187,6 @@ void Heater::publish_status()
status.proportional_value = _proportional_value;
status.integrator_value = _integrator_value;
status.feed_forward_value = _param_sens_imu_temp_ff.get();
#ifdef HEATER_PX4IO
status.mode = heater_status_s::MODE_PX4IO;
#endif
#ifdef HEATER_GPIO
status.mode = heater_status_s::MODE_GPIO;
#endif
status.timestamp = hrt_absolute_time();
_heater_status_pub.publish(status);
}
src/drivers/heater/heater.h
-8
View File
@@ -130,14 +130,6 @@ private:
*/
void update_params(const bool force = false);
/** Work queue struct for the scheduler. */
static struct work_s _work;
/** File descriptor for PX4IO for heater ioctl's */
#if defined(PX4IO_HEATER_ENABLED)
int _io_fd {-1};
#endif
bool _heater_initialized = false;
bool _heater_on = false;
bool _temperature_target_met = false;
src/drivers/px4io/px4io.cpp
+58 -258
View File
@@ -49,11 +49,8 @@
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_io_heater.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <lib/circuit_breaker/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
@@ -68,8 +65,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/heater_status.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
@@ -83,8 +79,6 @@
#include "uploader.h"
#include "modules/dataman/dataman.h"
#include "px4io_driver.h"
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
@@ -152,16 +146,12 @@ public:
*/
void test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; };
uint16_t system_status() const { return _status; }
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated) override;
private:
void Run() override;
void updateDisarmed();
void updateFailsafe();
void updateTimerRateGroups();
static int checkcrc(int argc, char *argv[]);
@@ -173,7 +163,6 @@ private:
unsigned _hardware{0}; ///< Hardware revision
unsigned _max_actuators{0}; ///< Maximum # of actuators supported by PX4IO
unsigned _max_controls{0}; ///< Maximum # of controls supported by PX4IO
unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO
unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO
@@ -199,6 +188,7 @@ private:
uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic
uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic
uORB::Subscription _t_vehicle_status{ORB_ID(vehicle_status)}; ///< vehicle status topic
uORB::Subscription _heater_status_sub{ORB_ID(heater_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -216,23 +206,14 @@ private:
ButtonPublisher _button_publisher;
bool _previous_safety_off{false};
bool _lockdown_override{false}; ///< override the safety lockdown
int32_t _thermal_control{-1}; ///< thermal control state
bool _analog_rc_rssi_stable{false}; ///< true when analog RSSI input is stable
float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage
bool _test_fmu_fail{false}; ///< To test what happens if IO loses FMU
bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active
MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
bool _pwm_min_configured{false};
bool _pwm_max_configured{false};
bool _pwm_fail_configured{false};
bool _pwm_dis_configured{false};
bool _pwm_rev_configured{false};
/**
* Update IO's arming-related state
*/
@@ -305,15 +286,6 @@ private:
*/
int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
/**
* Handle a status update from IO.
*
* Publish IO status information if necessary.
*
* @param status The status register
*/
int io_handle_status(uint16_t status);
/**
* Handle issuing dsm bind ioctl to px4io.
*
@@ -332,7 +304,6 @@ private:
void update_params();
DEFINE_PARAMETERS(
(ParamInt<px4::params::PWM_SBUS_MODE>) _param_pwm_sbus_mode,
(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
@@ -412,7 +383,6 @@ int PX4IO::init()
_hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
@@ -452,8 +422,7 @@ int PX4IO::init()
}
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN,
0);
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
if (ret != OK) {
mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t");
@@ -482,28 +451,6 @@ int PX4IO::init()
return OK;
}
void PX4IO::updateDisarmed()
{
uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) {
values[i] = _mixing_output.disarmedValue(i);
}
io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators);
}
void PX4IO::updateFailsafe()
{
uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) {
values[i] = _mixing_output.actualFailsafeValue(i);
}
io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators);
}
void PX4IO::Run()
{
if (should_exit()) {
@@ -519,6 +466,23 @@ void PX4IO::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (_thermal_control) {
if (_heater_status_sub.updated()) {
heater_status_s heater_status;
if (_heater_status_sub.copy(&heater_status)) {
if (heater_status.heater_on) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL);
} else {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF);
}
}
}
}
/* if we have new control data from the ORB, handle it */
if (_param_sys_hitl.get() <= 0) {
_mixing_output.update();
@@ -595,11 +559,6 @@ void PX4IO::Run()
update_params();
/* Check if the flight termination circuit breaker has been updated */
bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !disable_flighttermination);
if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) {
_thermal_control = _param_sens_en_themal.get();
@@ -615,21 +574,6 @@ void PX4IO::Run()
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl);
}
/* S.BUS output */
if (_param_pwm_sbus_mode.get() == 1) {
/* enable S.BUS 1 */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
} else if (_param_pwm_sbus_mode.get() == 2) {
/* enable S.BUS 2 */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
} else {
/* disable S.BUS */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES,
(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
}
}
}
@@ -738,8 +682,6 @@ void PX4IO::update_params()
// sync params to IO
updateTimerRateGroups();
updateFailsafe();
updateDisarmed();
_first_param_update = false;
return;
}
@@ -776,38 +718,7 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
if (armed.prearmed) {
set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
}
if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = true;
} else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = false;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
if (true) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
@@ -824,60 +735,6 @@ PX4IO::io_set_arming_state()
return 0;
}
int PX4IO::io_handle_status(uint16_t status)
{
int ret = 1;
/**
* WARNING: This section handles in-air resets.
*/
/* check for IO reset - force it back to armed if necessary */
if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the sync flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
} else {
ret = 0;
/* set new status */
_status = status;
}
/**
* Get and handle the safety button status
*/
const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
if (safety_button_pressed) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
_button_publisher.safetyButtonTriggerEvent();
}
/**
* Inform PX4IO board about safety_off state for LED control
*/
vehicle_status_s vehicle_status;
if (_t_vehicle_status.update(&vehicle_status)) {
if (_previous_safety_off != vehicle_status.safety_off) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
_previous_safety_off = vehicle_status.safety_off;
}
}
return ret;
}
int PX4IO::dsm_bind_ioctl(int dsmMode)
{
// Do not bind if invalid pulses are provided
@@ -933,7 +790,30 @@ int PX4IO::io_get_status()
const uint16_t STATUS_VSERVO = regs[4];
const uint16_t STATUS_VRSSI = regs[5];
io_handle_status(STATUS_FLAGS);
/* set new status */
_status = STATUS_FLAGS;
/**
* Get and handle the safety button status
*/
const bool safety_button_pressed = _status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
if (safety_button_pressed) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
_button_publisher.safetyButtonTriggerEvent();
}
/**
* Inform PX4IO board about safety_off state for LED control
*/
vehicle_status_s vehicle_status;
if (_t_vehicle_status.update(&vehicle_status)) {
if (_previous_safety_off != vehicle_status.safety_off) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
_previous_safety_off = vehicle_status.safety_off;
}
}
const float rssi_v = STATUS_VRSSI * 0.001f; // voltage is scaled to mV
@@ -963,48 +843,32 @@ int PX4IO::io_get_status()
status.free_memory_bytes = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM);
// PX4IO_P_STATUS_FLAGS
status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
status.status_rc_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_OK;
status.status_rc_ppm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_PPM;
status.status_rc_dsm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_DSM;
status.status_rc_sbus = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SBUS;
status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24;
status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD;
status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
status.status_fmu_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_OK;
status.status_raw_pwm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RAW_PWM;
status.status_arm_sync = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
status.status_init_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
status.status_failsafe = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
// PX4IO_P_STATUS_ALARMS
status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR;
status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR;
// PX4IO_P_SETUP_ARMING
status.arming_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK;
status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED;
status.arming_fmu_prearmed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_PREARMED;
status.arming_failsafe_custom = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
status.arming_lockdown = SETUP_ARMING & PX4IO_P_SETUP_ARMING_LOCKDOWN;
status.arming_force_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
status.arming_termination_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
status.arming_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK;
status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED;
for (unsigned i = 0; i < _max_actuators; i++) {
status.pwm[i] = io_reg_get(PX4IO_PAGE_SERVOS, i);
status.pwm_disarmed[i] = io_reg_get(PX4IO_PAGE_DISARMED_PWM, i);
status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
status.pwm[i] = io_reg_get(PX4IO_PAGE_DIRECT_PWM, i);
}
{
int i = 0;
for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
// This is a bit different than below, setting the groups, not the channels
status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
}
}
status.pwm_group_rate_hz[0] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0);
status.pwm_group_rate_hz[1] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP1);
status.pwm_group_rate_hz[2] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP2);
status.pwm_group_rate_hz[3] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP3);
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
@@ -1233,8 +1097,7 @@ int PX4IO::print_status()
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1));
printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
printf("%" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT));
@@ -1269,14 +1132,10 @@ int PX4IO::print_status()
/* setup and state */
uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
printf("features 0x%04" PRIx16 "%s%s%s\n", features,
((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
printf("features 0x%04" PRIx16 "%s\n", features,
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE));
printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
/* IMU heater (Pixhawk 2.1) */
@@ -1304,11 +1163,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
/* regular ioctl? */
switch (cmd) {
case DSM_BIND_START:
/* bind a DSM receiver */
ret = dsm_bind_ioctl(arg);
break;
case PX4IO_SET_DEBUG:
PX4_DEBUG("PX4IO_SET_DEBUG");
@@ -1317,7 +1171,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
case PX4IO_REBOOT_BOOTLOADER:
if (system_status() & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
if (_status & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
PX4_ERR("not upgrading IO firmware, system is armed");
return -EINVAL;
@@ -1357,36 +1211,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
case SBUS_SET_PROTO_VERSION:
PX4_DEBUG("SBUS_SET_PROTO_VERSION");
if (arg == 1) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
} else if (arg == 2) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES,
(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
}
break;
case PX4IO_HEATER_CONTROL:
if (arg == (unsigned long)HEATER_MODE_DISABLED) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_IGNORE);
} else if (arg == 1) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL);
} else {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF);
}
break;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
@@ -1507,7 +1331,7 @@ int PX4IO::bind(int argc, char *argv[])
pulses = atoi(argv[1]);
}
get_instance()->ioctl(nullptr, DSM_BIND_START, pulses);
get_instance()->dsm_bind_ioctl(pulses);
return 0;
}
@@ -1678,28 +1502,6 @@ int PX4IO::custom_command(int argc, char *argv[])
return bind(argc - 1, argv + 1);
}
if (!strcmp(verb, "sbus1_out")) {
int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
if (ret != 0) {
PX4_ERR("S.BUS v1 failed (%i)", ret);
return 1;
}
return 0;
}
if (!strcmp(verb, "sbus2_out")) {
int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
if (ret != 0) {
PX4_ERR("S.BUS v2 failed (%i)", ret);
return 1;
}
return 0;
}
if (!strcmp(verb, "test_fmu_fail")) {
get_instance()->test_fmu_fail(true);
return 0;
@@ -1736,8 +1538,6 @@ Output driver communicating with the IO co-processor.
PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");
PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out");
PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out");
PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates");
PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_ok", "re-enable IO updates");
src/drivers/px4io/px4io_params.c
-10
View File
@@ -53,13 +53,3 @@
* @group System
*/
PARAM_DEFINE_INT32(SYS_USE_IO, 0);
/**
* S.BUS out
*
* Set to 1 to enable S.BUS version 1 output instead of RSSI.
*
* @boolean
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0);
src/examples/hwtest/hwtest.c
-8
View File
@@ -66,7 +66,6 @@ int ex_hwtest_main(int argc, char *argv[])
memset(&arm, 0, sizeof(arm));
arm.timestamp = hrt_absolute_time();
arm.ready_to_arm = true;
arm.armed = true;
orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm);
orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm);
@@ -75,13 +74,6 @@ int ex_hwtest_main(int argc, char *argv[])
int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);
if (arm.ready_to_arm && arm.armed) {
warnx("Actuator armed");
} else {
errx(1, "Arming actuators failed");
}
hrt_abstime stime;
int count = 0;
src/lib/mixer_module/mixer_module.cpp
-1
View File
@@ -79,7 +79,6 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou
/* Safely initialize armed flags */
_armed.armed = false;
_armed.prearmed = false;
_armed.ready_to_arm = false;
_armed.lockdown = false;
_armed.force_failsafe = false;
_armed.in_esc_calibration_mode = false;
src/modules/commander/Commander.cpp
-2
View File
@@ -93,7 +93,6 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
{
return (a.armed == b.armed &&
a.prearmed == b.prearmed &&
a.ready_to_arm == b.ready_to_arm &&
a.lockdown == b.lockdown &&
a.manual_lockdown == b.manual_lockdown &&
a.force_failsafe == b.force_failsafe &&
@@ -2232,7 +2231,6 @@ Commander::run()
// publish actuator_armed first (used by output modules)
_actuator_armed.armed = _arm_state_machine.isArmed();
_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
_actuator_armed.timestamp = hrt_absolute_time();
_actuator_armed_pub.publish(_actuator_armed);
src/modules/px4iofirmware/CMakeLists.txt
-8
View File
@@ -31,12 +31,9 @@
#
############################################################################
option(PX4IO_PERF "Enable px4io perf counters" OFF)
add_library(px4iofirmware
adc.cpp
controls.cpp
mixer.cpp
px4io.cpp
registers.c
safety_button.cpp
@@ -54,8 +51,3 @@ target_link_libraries(px4iofirmware
nuttx_mm
rc
)
if(PX4IO_PERF)
target_compile_definitions(px4iofirmware PRIVATE PX4IO_PERF)
target_link_libraries(px4iofirmware PRIVATE perf)
endif()
src/modules/px4iofirmware/adc.cpp
-22
View File
@@ -45,10 +45,6 @@
#include <drivers/drv_hrt.h>
#if defined(PX4IO_PERF)
# include <perf/perf_counter.h>
#endif
#define DEBUG
#include "px4io.h"
@@ -79,17 +75,9 @@
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
#define rDR REG(STM32_ADC_DR_OFFSET)
#if defined(PX4IO_PERF)
perf_counter_t adc_perf;
#endif
int
adc_init(void)
{
#if defined(PX4IO_PERF)
adc_perf = perf_alloc(PC_ELAPSED, "adc");
#endif
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
@@ -143,10 +131,6 @@ adc_init(void)
uint16_t
adc_measure(unsigned channel)
{
#if defined(PX4IO_PERF)
perf_begin(adc_perf);
#endif
/* clear any previous EOC */
rSR = 0;
(void)rDR;
@@ -162,9 +146,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) {
#if defined(PX4IO_PERF)
perf_end(adc_perf);
#endif
return 0xffff;
}
}
@@ -173,8 +154,5 @@ adc_measure(unsigned channel)
uint16_t result = rDR;
rSR = 0;
#if defined(PX4IO_PERF)
perf_end(adc_perf);
#endif
return result;
}
src/modules/px4iofirmware/controls.cpp
-45
View File
@@ -50,21 +50,11 @@
#include <rc/dsm.h>
#include <uORB/topics/input_rc.h>
#if defined(PX4IO_PERF)
# include <perf/perf_counter.h>
#endif
#include "px4io.h"
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);
#if defined(PX4IO_PERF)
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
#endif
static int _dsm_fd = -1;
int _sbus_fd = -1;
@@ -79,9 +69,6 @@ static unsigned _frame_drops = 0;
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{
#if defined(PX4IO_PERF)
perf_begin(c_gather_dsm);
#endif
uint8_t n_bytes = 0;
uint8_t *bytes;
bool dsm_11_bit;
@@ -118,10 +105,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
}
}
#if defined(PX4IO_PERF)
perf_end(c_gather_dsm);
#endif
/* get data from FD and attempt to parse with DSM and ST24 libs */
uint8_t st24_rssi, lost_count;
uint16_t st24_channel_count = 0;
@@ -198,12 +181,6 @@ controls_init(void)
/* S.bus input (USART3) */
_sbus_fd = sbus_init("/dev/ttyS2", false);
#if defined(PX4IO_PERF)
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
#endif
}
void
@@ -244,10 +221,6 @@ controls_tick()
_rssi = 0;
}
#if defined(PX4IO_PERF)
perf_begin(c_gather_sbus);
#endif
bool sbus_updated = false;
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
@@ -282,18 +255,11 @@ controls_tick()
}
}
#if defined(PX4IO_PERF)
perf_end(c_gather_sbus);
#endif
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
#if defined(PX4IO_PERF)
perf_begin(c_gather_ppm);
#endif
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
@@ -303,16 +269,9 @@ controls_tick()
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
#if defined(PX4IO_PERF)
perf_end(c_gather_ppm);
#endif
bool dsm_updated = false, st24_updated = false, sumd_updated = false;
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) {
#if defined(PX4IO_PERF)
perf_begin(c_gather_dsm);
#endif
(void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated);
@@ -327,10 +286,6 @@ controls_tick()
if (sumd_updated) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD);
}
#if defined(PX4IO_PERF)
perf_end(c_gather_dsm);
#endif
}
/* limit number of channels to allowable data size */
src/modules/px4iofirmware/mixer.cpp
-251
View File
@@ -1,251 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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 mixer.cpp
*
* Control channel input/output mixer and failsafe.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_platform_common/px4_config.h>
#include <syslog.h>
#include <sys/types.h>
#include <stdbool.h>
#include <float.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <rc/sbus.h>
extern "C" {
/* #define DEBUG */
#include "px4io.h"
}
/*
* Maximum interval in us before FMU signal is considered lost
*/
#define FMU_INPUT_DROP_LIMIT_US 500000
/* current servo arm/disarm state */
static volatile bool mixer_servos_armed = false;
static volatile bool should_arm = false;
static volatile bool should_arm_nothrottle = false;
static volatile bool should_always_enable_pwm = false;
static bool new_fmu_data = false;
static uint64_t last_fmu_update = 0;
extern int _sbus_fd;
/* selected control values and count for mixing */
enum mixer_source {
MIX_NONE,
MIX_DISARMED,
MIX_FAILSAFE,
};
void
mixer_tick()
{
/* check that we are receiving fresh data from the FMU */
irqstate_t irq_flags = enter_critical_section();
const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
leave_critical_section(irq_flags);
if ((fmu_data_received_time == 0) ||
hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
} else {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
/* this flag is never cleared once OK */
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED);
if (fmu_data_received_time > last_fmu_update) {
new_fmu_data = true;
last_fmu_update = fmu_data_received_time;
}
}
/* default to disarmed mixing */
mixer_source source = MIX_DISARMED;
/*
* Decide which set of controls we're using.
*/
/* Do not mix if we have raw PWM and FMU is ok. */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - copy values from r_page_direct_pwm */
source = MIX_NONE;
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
}
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
*/
should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
&& (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
;
should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
&& ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
));
/* we enable PWM output always on the IO side if FMU is up and running
* as zero-outputs can be controlled by FMU by sending a 0 PWM command
*/
should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK));
/*
* Check if FMU is still alive, if not, terminate the flight
*/
if (REG_TO_BOOL(r_setup_flighttermination) && /* Flight termination is allowed */
(source == MIX_DISARMED) && /* and if we ended up not changing the default mixer */
should_arm && /* and we should be armed, so we intended to provide outputs */
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { /* and FMU is initialized */
atomic_modify_or(&r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */
}
/*
* Check if we should force failsafe - and do it if we have to
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
source = MIX_FAILSAFE;
}
/*
* Set failsafe status flag depending on mixing source
*/
if (source == MIX_FAILSAFE) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE);
} else {
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE));
}
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
}
} else if (source == MIX_DISARMED) {
/* copy disarmed values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_disarmed[i];
}
}
/* set arming */
bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
/* lockdown means to send a valid pulse which disables the outputs */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = true;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true, 0);
mixer_servos_armed = true;
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM enabled");
} else if (!needs_to_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false, 0);
mixer_servos_armed = false;
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED));
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed
&& (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE))
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servos[i]);
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
}
} else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
/* copy values into reporting register */
r_page_servos[i] = r_page_servo_disarmed[i];
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
}
}
src/modules/px4iofirmware/protocol.h
+10 -35
View File
@@ -79,16 +79,13 @@
#define PX4IO_PROTOCOL_VERSION 5
/* maximum allowable sizes on this protocol version */
#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
@@ -97,21 +94,20 @@
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
#define PX4IO_P_STATUS_FREEMEM 0
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
/* (1 << 0) */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
/* UNUSED (1 << 7) */
/* UNUSED (1 << 8) */
/* UNUSED (1 << 9) */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
/* UNUSED (1 << 11) */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
#define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT (1 << 14) /* px4io safety button was pressed for longer than 1 second */
@@ -123,9 +119,6 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
/* array of PWM servo output values, microseconds */
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
@@ -152,22 +145,14 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
/* UNUSED (1 << 0) */
/* UNUSED (1 << 1) */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 2) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 6 /* DSM bind state */
enum { /* DSM bind states */
@@ -187,9 +172,9 @@ enum { /* DSM bind states */
/* storage space of 12 occupied by CRC */
#define PX4IO_P_SETUP_SAFETY_BUTTON_ACK 14 /**< ACK from FMU when it gets safety button pressed status */
#define PX4IO_P_SETUP_SAFETY_OFF 15 /**< FMU inform PX4IO about safety_off for LED indication*/
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
/* UNUSED 16 */
#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
/* UNUSED 18 */
#define PX4IO_P_SETUP_PWM_RATE_GROUP0 19 /* Configure timer group 0 update rate in Hz */
#define PX4IO_P_SETUP_PWM_RATE_GROUP1 20 /* Configure timer group 1 update rate in Hz */
#define PX4IO_P_SETUP_PWM_RATE_GROUP2 21 /* Configure timer group 2 update rate in Hz */
@@ -202,16 +187,6 @@ enum { /* DSM bind states */
/* PWM output */
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
/* PWM disarmed values that are active */
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* Serial protocol encapsulation.
*/
src/modules/px4iofirmware/px4io.cpp
+72 -41
View File
@@ -55,10 +55,6 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_watchdog.h>
#if defined(PX4IO_PERF)
# include <lib/perf/perf_counter.h>
#endif
#include <stm32_uart.h>
#define DEBUG
@@ -77,6 +73,20 @@ static volatile uint32_t last_msg_counter;
static volatile uint8_t msg_next_out;
static volatile uint8_t msg_next_in;
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
/*
* Maximum interval in us before FMU signal is considered lost
*/
#define FMU_INPUT_DROP_LIMIT_US 500000
/* current servo arm/disarm state */
static volatile bool mixer_servos_armed = false;
static bool new_fmu_data = false;
static uint64_t last_fmu_update = 0;
/*
* WARNING: too large buffers here consume the memory required
* for mixer handling. Do not allocate more than 80 bytes for
@@ -272,6 +282,64 @@ calculate_fw_crc(void)
r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16;
}
static void mixer_tick()
{
/* check that we are receiving fresh data from the FMU */
irqstate_t irq_flags = enter_critical_section();
const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
leave_critical_section(irq_flags);
if ((fmu_data_received_time == 0) ||
hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
} else {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
if (fmu_data_received_time > last_fmu_update) {
new_fmu_data = true;
last_fmu_update = fmu_data_received_time;
}
}
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
*/
const bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
&& (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true, 0);
mixer_servos_armed = true;
isr_debug(5, "> PWM enabled");
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false, 0);
mixer_servos_armed = false;
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_direct_pwm[i]);
}
}
}
extern "C" __EXPORT int user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
@@ -307,11 +375,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
LED_GREEN(false);
#endif /* LED_GREEN */
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety button handler */
safety_button_init();
@@ -324,17 +387,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
/* start the FMU interface */
interface_init();
#if defined(PX4IO_PERF)
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
#endif
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
@@ -354,30 +406,9 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
for (;;) {
watchdog_pet();
#if defined(PX4IO_PERF)
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
#endif
mixer_tick();
#if defined(PX4IO_PERF)
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
#endif
controls_tick();
#if defined(PX4IO_PERF)
perf_end(controls_perf);
#endif
/*
blink blue LED at 4Hz in normal operation. When in
override blink 4x faster so the user can clearly see
src/modules/px4iofirmware/px4io.h
-17
View File
@@ -77,13 +77,10 @@ __BEGIN_DECLS
* Registers.
*/
extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
* Register aliases.
@@ -99,13 +96,6 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
#define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]
/*
* System state structure.
@@ -123,8 +113,6 @@ struct sys_state_s {
extern struct sys_state_s system_state;
# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))
# define PX4IO_ADC_CHANNEL_COUNT 2
@@ -137,11 +125,6 @@ void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
void atomic_modify_clear(volatile uint16_t *target, uint16_t modification);
void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
/*
* Mixer
*/
extern void mixer_tick(void);
/**
* Safety button/LED.
*/
src/modules/px4iofirmware/registers.c
+2 -259
View File
@@ -55,7 +55,6 @@
#include "protocol.h"
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
/**
* PAGE 0
@@ -67,7 +66,6 @@ static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = PX4IO_BL_VERSION,
[PX4IO_P_CONFIG_MAX_TRANSFER] = PX4IO_MAX_TRANSFER_LEN,
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
@@ -80,20 +78,12 @@ static const uint16_t r_page_config[] = {
*/
volatile uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FREEMEM] = 0,
[PX4IO_P_STATUS_CPULOAD] = 0,
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
};
/**
* PAGE 3
*
* Servo PWM values
*/
uint16_t r_page_servos[PX4IO_SERVO_COUNT];
/**
* PAGE 4
*
@@ -133,51 +123,19 @@ volatile uint16_t r_page_setup[] = {
/* default to RSSI ADC functionality */
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_SBUS_RATE] = 72,
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0,
[PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 0
};
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_ADC_RSSI)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_FMU_PREARMED | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_IO_ARM_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
/*
* PAGE 104 uses r_page_servos.
*/
/**
* PAGE 105
*
* Failsafe servo PWM values
*
* Disable pulses as default.
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
/**
* PAGE 109
*
* disarmed PWM values for difficult ESCs
*
*/
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -208,60 +166,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
break;
/* handle setup for servo failsafe values */
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* ignore 0 */
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
offset++;
num_values--;
values++;
}
break;
case PX4IO_PAGE_DISARMED_PWM: {
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) {
/* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_disarmed[offset] = *values;
}
offset++;
num_values--;
values++;
}
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
break;
default:
/* avoid offset wrap */
@@ -297,19 +201,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_status_alarms &= ~value;
break;
case PX4IO_P_STATUS_FLAGS:
/*
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
break;
default:
/* just ignore writes to other registers in this page */
break;
@@ -323,29 +214,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_SETUP_FEATURES_VALID;
/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
/* switch S.Bus output pin as needed */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
/* disable the conflicting options with SBUS 1 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* disable the conflicting options with SBUS 2 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
}
#endif
/* disable the conflicting options with ADC RSSI */
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* apply changes */
r_setup_features = value;
@@ -355,58 +223,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_SETUP_ARMING_VALID;
/*
* If the failsafe termination flag is set, do not allow the autopilot to unset it
*/
value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
/*
* If failsafe termination is enabled and force failsafe bit is set, do not allow
* the autopilot to clear it.
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
r_setup_arming = value;
break;
case PX4IO_P_SETUP_PWM_RATES:
value &= PX4IO_P_SETUP_RATES_VALID;
pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
break;
case PX4IO_P_SETUP_PWM_DEFAULTRATE:
if (value < 25) {
value = 25;
}
if (value > 400) {
value = 400;
}
pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
break;
case PX4IO_P_SETUP_PWM_ALTRATE:
/* For PWM constrain to [25,400]Hz
* For Oneshot there is no rate, 0 is therefore used to select Oneshot mode
*/
if (value != 0) {
if (value < 25) {
value = 25;
}
if (value > 400) {
value = 400;
}
}
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
case PX4IO_P_SETUP_SET_DEBUG:
r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
@@ -446,13 +266,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_SBUS_RATE:
r_page_setup[offset] = value;
sbus1_set_output_rate_hz(value);
break;
case PX4IO_P_SETUP_THERMAL:
case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION:
r_page_setup[offset] = value;
break;
@@ -489,15 +303,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_PAGE_TEST:
switch (offset) {
case PX4IO_P_TEST_LED:
LED_AMBER(value & 1);
break;
}
break;
default:
return -1;
}
@@ -525,8 +330,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_STATUS:
/* PX4IO_P_STATUS_FREEMEM */
/* XXX PX4IO_P_STATUS_CPULOAD */
/* PX4IO_P_STATUS_FLAGS maintained externally */
/* PX4IO_P_STATUS_ALARMS maintained externally */
@@ -592,10 +395,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
@@ -609,14 +408,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_direct_pwm);
break;
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
default:
return -1;
}
@@ -638,51 +429,3 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
return 0;
}
/*
* Helper function to handle changes to the PWM rate control registers.
*/
static void
pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
{
for (unsigned pass = 0; pass < 2; pass++) {
for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
/* get the channel mask for this rate group */
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0) {
continue;
}
/* all channels in the group must be either default or alt-rate */
uint32_t alt = map & mask;
if (pass == 0) {
/* preflight */
if ((alt != 0) && (alt != mask)) {
/* not a legal map, bail with an alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
return;
}
} else {
/* set it - errors here are unexpected */
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) {
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) {
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
}
}
}
}
r_setup_pwm_rates = map;
r_setup_pwm_defaultrate = defaultrate;
r_setup_pwm_altrate = altrate;
}
src/modules/px4iofirmware/safety_button.cpp
-6
View File
@@ -136,12 +136,6 @@ safety_button_check(void *arg)
static void
failsafe_blink(void *arg)
{
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
src/modules/px4iofirmware/serial.cpp
-68
View File
@@ -55,20 +55,6 @@
//#define DEBUG
#include "px4io.h"
#if defined(PX4IO_PERF)
# include <perf/perf_counter.h>
static perf_counter_t pc_txns;
static perf_counter_t pc_errors;
static perf_counter_t pc_ore;
static perf_counter_t pc_fe;
static perf_counter_t pc_ne;
static perf_counter_t pc_idle;
static perf_counter_t pc_badidle;
static perf_counter_t pc_regerr;
static perf_counter_t pc_crcerr;
#endif
static void rx_handle_packet(void);
static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
static DMA_HANDLE tx_dma;
@@ -92,18 +78,6 @@ static struct IOPacket dma_packet;
void
interface_init(void)
{
#if defined(PX4IO_PERF)
pc_txns = perf_alloc(PC_ELAPSED, "txns");
pc_errors = perf_alloc(PC_COUNT, "errors");
pc_ore = perf_alloc(PC_COUNT, "overrun");
pc_fe = perf_alloc(PC_COUNT, "framing");
pc_ne = perf_alloc(PC_COUNT, "noise");
pc_idle = perf_alloc(PC_COUNT, "idle");
pc_badidle = perf_alloc(PC_COUNT, "badidle");
pc_regerr = perf_alloc(PC_COUNT, "regerr");
pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
#endif
/* allocate DMA */
tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
@@ -165,10 +139,6 @@ rx_handle_packet(void)
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
#if defined(PX4IO_PERF)
perf_count(pc_crcerr);
#endif
/* send a CRC error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
@@ -181,10 +151,6 @@ rx_handle_packet(void)
/* it's a blind write - pass it on */
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
#if defined(PX4IO_PERF)
perf_count(pc_regerr);
#endif
dma_packet.count_code = PKT_CODE_ERROR;
} else {
@@ -201,10 +167,6 @@ rx_handle_packet(void)
uint16_t *registers;
if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
#if defined(PX4IO_PERF)
perf_count(pc_regerr);
#endif
dma_packet.count_code = PKT_CODE_ERROR;
} else {
@@ -238,9 +200,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
* We are here because DMA completed, or UART reception stopped and
* we think we have a packet in the buffer.
*/
#if defined(PX4IO_PERF)
perf_begin(pc_txns);
#endif
/* disable UART DMA */
rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
@@ -265,10 +224,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
DMA_CCR_MSIZE_8BITS);
stm32_dmastart(tx_dma, NULL, NULL, false);
rCR3 |= USART_CR3_DMAT;
#if defined(PX4IO_PERF)
perf_end(pc_txns);
#endif
}
static int
@@ -283,23 +238,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
#if defined(PX4IO_PERF)
perf_count(pc_errors);
if (sr & USART_SR_ORE) {
perf_count(pc_ore);
}
if (sr & USART_SR_NE) {
perf_count(pc_ne);
}
if (sr & USART_SR_FE) {
perf_count(pc_fe);
}
#endif
/* send a line break - this will abort transmission/reception on the other end */
rCR1 |= USART_CR1_SBK;
@@ -331,9 +269,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
/* it was too short - possibly truncated */
#if defined(PX4IO_PERF)
perf_count(pc_badidle);
#endif
dma_reset();
return 0;
}
@@ -342,9 +277,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
* Looks like we received a packet. Stop the DMA and go process the
* packet.
*/
#if defined(PX4IO_PERF)
perf_count(pc_idle);
#endif
stm32_dmastop(rx_dma);
rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
}