Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-05 17:56:10 +01:00
parent 904efa8fa8 e7f2c053c2
commit 572084f357
59 changed files with 1269 additions and 2926 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+75 -14
View File
@@ -143,7 +143,7 @@ int commander_thread_main(int argc, char *argv[]);
static int buzzer_init(void);
static void buzzer_deinit(void);
static void tune_confirm();
static void tune_confirm(void);
static int led_init(void);
static void led_deinit(void);
static int led_toggle(int led);
@@ -264,7 +264,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u
return 0;
}
void tune_confirm() {
void tune_confirm(void) {
ioctl(buzzer, TONE_SET_ALARM, 3);
}
@@ -914,17 +914,69 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
/*
* do not report an error for commands that are
* handled directly by MAVLink.
*/
case MAV_CMD_PREFLIGHT_STORAGE:
case MAV_CMD_PREFLIGHT_STORAGE: {
if (current_status.flag_system_armed) {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed");
} else {
// XXX move this to LOW PRIO THREAD of commander app
/* Read all parameters from EEPROM to RAM */
if (((int)(cmd->param1)) == 0) {
/* read all parameters from EEPROM to RAM */
int read_ret = param_load_default();
if (read_ret == OK) {
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
mavlink_log_info(mavlink_fd, "[cmd] OK loading params from");
mavlink_log_info(mavlink_fd, param_get_default_file());
result = MAV_RESULT_ACCEPTED;
} else if (read_ret == 1) {
mavlink_log_info(mavlink_fd, "[cmd] OK no changes in");
mavlink_log_info(mavlink_fd, param_get_default_file());
result = MAV_RESULT_ACCEPTED;
} else {
if (read_ret < -1) {
mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from");
mavlink_log_info(mavlink_fd, param_get_default_file());
} else {
mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named");
mavlink_log_info(mavlink_fd, param_get_default_file());
}
result = MAV_RESULT_FAILED;
}
} else if (((int)(cmd->param1)) == 1) {
/* write all parameters from RAM to EEPROM */
int write_ret = param_save_default();
if (write_ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] OK saved param file");
mavlink_log_info(mavlink_fd, param_get_default_file());
result = MAV_RESULT_ACCEPTED;
} else {
if (write_ret < -1) {
mavlink_log_info(mavlink_fd, "[cmd] ERR params file does not exit:");
mavlink_log_info(mavlink_fd, param_get_default_file());
} else {
mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to");
mavlink_log_info(mavlink_fd, param_get_default_file());
}
result = MAV_RESULT_FAILED;
}
} else {
mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
result = MAV_RESULT_UNSUPPORTED;
}
}
}
break;
default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;
usleep(200000);
/* announce command rejection */
ioctl(buzzer, TONE_SET_ALARM, 4);
}
@@ -932,7 +984,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
/* supported command handling stop */
if (result == MAV_RESULT_FAILED ||
result == MAV_RESULT_DENIED ||
result == MAV_RESULT_UNSUPPORTED) {
ioctl(buzzer, TONE_SET_ALARM, 5);
} else if (result == MAV_RESULT_ACCEPTED) {
tune_confirm();
}
/* send any requested ACKs */
if (cmd->confirmation > 0) {
@@ -1523,12 +1581,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.flag_control_manual_enabled = false;
current_status.flag_control_offboard_enabled = true;
state_changed = true;
tune_confirm();
mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME.");
mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST");
} else {
if (current_status.offboard_control_signal_lost) {
mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!");
mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL");
state_changed = true;
tune_confirm();
}
}
@@ -1550,7 +1610,7 @@ int commander_thread_main(int argc, char *argv[])
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
current_status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!");
mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
@@ -1560,10 +1620,11 @@ int commander_thread_main(int argc, char *argv[])
if (current_status.offboard_control_signal_lost_interval > 100000) {
current_status.offboard_control_signal_lost = true;
current_status.failsave_lowlevel_start_time = hrt_absolute_time();
current_status.failsave_lowlevel = true;
tune_confirm();
/* kill motors after timeout */
if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) {
current_status.failsave_lowlevel = true;
state_changed = true;
}
}
apps/drivers/boards/px4fmu/Makefile
+1
View File
@@ -36,5 +36,6 @@
#
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
LIBNAME = brd_px4fmu
include $(APPDIR)/mk/app.mk
apps/drivers/boards/px4fmu/px4fmu_init.c
-1
View File
@@ -57,7 +57,6 @@
#include <nuttx/i2c.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
#include <nuttx/arch.h>
#include "stm32_internal.h"
#include "px4fmu_internal.h"
apps/drivers/boards/px4io/Makefile
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Board-specific startup code for the PX4IO
#
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
LIBNAME = brd_px4io
include $(APPDIR)/mk/app.mk
nuttx/configs/px4io/src/up_boot.c → apps/drivers/boards/px4io/px4io_init.c
+95 -82
View File
@@ -1,82 +1,95 @@
/************************************************************************************
* configs/stm3210e-eval/src/up_boot.c
* arch/arm/src/board/up_boot.c
*
* Copyright (C) 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 NuttX 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.
*
************************************************************************************/
/************************************************************************************
* Included Files
************************************************************************************/
#include <nuttx/config.h>
#include <debug.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "px4io_internal.h"
#include <arch/board/up_hrt.h>
#include <arch/board/drv_pwm_servo.h>
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
void stm32_boardinitialize(void)
{
/* Configure on-board LEDs if LED support has been selected. */
#ifdef CONFIG_ARCH_LEDS
up_ledinit();
#endif
}
/****************************************************************************
*
* Copyright (C) 2012 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 px4io_init.c
*
* PX4IO-specific early startup code. This file implements the
* nsh_archinitialize() function that is called early by nsh during startup.
*
* Code here is run before the rcS script is invoked; it should start required
* subsystems and perform board-specific initialisation.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdbool.h>
#include <stdio.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include "stm32_internal.h"
#include "px4io_internal.h"
#include "stm32_uart.h"
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
#include <drivers/drv_pwm_output.h>
/****************************************************************************
* Public Functions
****************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void stm32_boardinitialize(void)
{
/* configure GPIOs */
stm32_configgpio(GPIO_ACC1_PWR_EN);
stm32_configgpio(GPIO_ACC2_PWR_EN);
stm32_configgpio(GPIO_SERVO_PWR_EN);
stm32_configgpio(GPIO_RELAY1_EN);
stm32_configgpio(GPIO_RELAY2_EN);
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
}
nuttx/configs/px4io/src/px4io_internal.h → apps/drivers/boards/px4io/px4io_internal.h
+3 -20
View File
@@ -35,8 +35,7 @@
* @file PX4IO hardware definitions.
*/
#ifndef __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H
#define __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H
#pragma once
/************************************************************************************
* Included Files
@@ -46,6 +45,8 @@
#include <nuttx/compiler.h>
#include <stdint.h>
#include <stm32_internal.h>
/************************************************************************************
* Definitions
************************************************************************************/
@@ -97,21 +98,3 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
/************************************************************************************
* Public Types
************************************************************************************/
/************************************************************************************
* Public data
************************************************************************************/
#ifndef __ASSEMBLY__
/************************************************************************************
* Public Functions
************************************************************************************/
#endif /* __ASSEMBLY__ */
#endif /* __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H */
apps/drivers/boards/px4io/px4io_pwm_servo.c
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (C) 2012 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 px4fmu_pwm_servo.c
*
* Configuration data for the stm32 pwm_servo driver.
*
* Note that these arrays must always be fully-sized.
*/
#include <stdint.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
{
.base = STM32_TIM2_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM2EN,
.clock_freq = STM32_APB1_TIM2_CLKIN
},
{
.base = STM32_TIM3_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM3EN,
.clock_freq = STM32_APB1_TIM3_CLKIN
},
{
.base = STM32_TIM4_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM4EN,
.clock_freq = STM32_APB1_TIM4_CLKIN
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
{
.gpio = GPIO_TIM2_CH1OUT,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
},
{
.gpio = GPIO_TIM2_CH2OUT,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
},
{
.gpio = GPIO_TIM4_CH3OUT,
.timer_index = 2,
.timer_channel = 3,
.default_value = 1000,
},
{
.gpio = GPIO_TIM4_CH4OUT,
.timer_index = 2,
.timer_channel = 4,
.default_value = 1000,
},
{
.gpio = GPIO_TIM3_CH1OUT,
.timer_index = 1,
.timer_channel = 1,
.default_value = 1000,
},
{
.gpio = GPIO_TIM3_CH2OUT,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1000,
},
{
.gpio = GPIO_TIM3_CH3OUT,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1000,
},
{
.gpio = GPIO_TIM3_CH4OUT,
.timer_index = 1,
.timer_channel = 4,
.default_value = 1000,
}
};
apps/drivers/drv_gpio.h
+32
View File
@@ -67,6 +67,38 @@
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO
/*
* PX4IO GPIO numbers.
*
* XXX note that these are here for reference/future use; currently
* there is no good way to wire these up without a common STM32 GPIO
* driver, which isn't implemented yet.
*/
/* outputs */
# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */
# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */
# define GPIO_SERVO_POWER (1<<2) /**< servo power */
# define GPIO_RELAY1 (1<<3) /**< relay 1 */
# define GPIO_RELAY2 (1<<4) /**< relay 2 */
# define GPIO_LED_BLUE (1<<5) /**< blue LED */
# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */
# define GPIO_LED_SAFETY (1<<7) /**< safety LED */
/* inputs */
# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */
# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */
# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */
/**
* Default GPIO device - other devices may also support this protocol if
* they also export GPIO-like things. This is always the GPIOs on the
* main board.
*/
# define GPIO_DEVICE_PATH "/dev/px4io"
#endif
#ifndef GPIO_DEVICE_PATH
# error No GPIO support for this board.
#endif
apps/drivers/drv_led.h
+2 -1
View File
@@ -50,6 +50,7 @@
#define LED_AMBER 0
#define LED_RED 0 /* some boards have red rather than amber */
#define LED_BLUE 1
#define LED_SAFETY 2
#define LED_ON _IOC(_LED_BASE, 0)
#define LED_OFF _IOC(_LED_BASE, 1)
@@ -59,6 +60,6 @@ __BEGIN_DECLS
/*
* Initialise the LED driver.
*/
__EXPORT extern void drv_led_start();
__EXPORT extern void drv_led_start(void);
__END_DECLS
apps/drivers/drv_rc_input.h
+18 -4
View File
@@ -57,15 +57,23 @@
#define RC_INPUT_DEVICE_PATH "/dev/input_rc"
/**
* Maximum number of R/C input channels in the system.
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
*/
#define RC_INPUT_MAX_CHANNELS 16
#define RC_INPUT_MAX_CHANNELS 18
/**
* Input signal type, value is a control position from zero to 100
* percent.
*/
typedef uint8_t rc_input_t;
typedef uint16_t rc_input_t;
enum RC_INPUT_SOURCE {
RC_INPUT_SOURCE_UNKNOWN = 0,
RC_INPUT_SOURCE_PX4FMU_PPM,
RC_INPUT_SOURCE_PX4IO_PPM,
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
RC_INPUT_SOURCE_PX4IO_SBUS
};
/**
* R/C input status structure.
@@ -74,10 +82,16 @@ typedef uint8_t rc_input_t;
* on the board involved.
*/
struct rc_input_values {
/** decoding time */
uint64_t timestamp;
/** number of channels actually being seen */
uint32_t channel_count;
/** desired pulse widths for each of the supported channels */
/** Input source */
enum RC_INPUT_SOURCE input_source;
/** measured pulse widths for each of the supported channels */
rc_input_t values[RC_INPUT_MAX_CHANNELS];
};
apps/drivers/hmc5883/hmc5883.cpp
+14 -4
View File
@@ -66,6 +66,9 @@
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <float.h>
/*
@@ -631,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
@@ -1039,11 +1043,17 @@ int HMC5883::check_calibration()
offset_valid = false;
}
if (_calibrated && !(offset_valid && scale_valid)) {
warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ",
if (_calibrated != (offset_valid && scale_valid)) {
warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
(offset_valid) ? "" : "offset invalid.");
_calibrated = false;
// XXX Notify system via uORB
_calibrated = (offset_valid && scale_valid);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
return 0;
}
apps/drivers/px4io/px4io.cpp
+163 -23
View File
@@ -61,9 +61,10 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
@@ -73,7 +74,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
#include "px4io/protocol.h"
#include <px4io/protocol.h>
#include "uploader.h"
@@ -87,6 +88,8 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
void set_rx_mode(unsigned mode);
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
@@ -95,6 +98,7 @@ private:
int _task; ///< worker task
volatile bool _task_should_exit;
volatile bool _connected; ///< true once we have received a valid frame
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
@@ -102,6 +106,9 @@ private:
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
@@ -113,6 +120,9 @@ private:
// XXX how should this work?
bool _send_needed; ///< If true, we need to send a packet to IO
bool _config_needed; ///< if true, we need to set a config update to IO
uint8_t _rx_mode; ///< the current RX mode on IO
/**
* Trampoline to the worker task
@@ -144,6 +154,11 @@ private:
*/
void io_send();
/**
* Send a config packet to PX4IO
*/
void config_send();
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
@@ -168,13 +183,16 @@ PX4IO::PX4IO() :
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false)
_send_needed(false),
_config_needed(false),
_rx_mode(RX_MODE_PPM_ONLY)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -238,12 +256,24 @@ PX4IO::init()
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
}
/* wait a second for it to detect IO */
for (unsigned i = 0; i < 10; i++) {
if (_connected) {
debug("PX4IO connected");
break;
}
usleep(100000);
}
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
return OK;
}
@@ -262,22 +292,20 @@ PX4IO::task_main()
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
if (_serial_fd < 0) {
debug("failed to open serial port for IO: %d", errno);
_task = -1;
_exit(errno);
log("failed to open serial port: %d", errno);
goto out;
}
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors);
/* XXX send a who-are-you request */
/* XXX verify firmware/protocol version */
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
perf_alloc(PC_COUNT, "PX4IO receive errors"));
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
@@ -293,9 +321,14 @@ PX4IO::task_main()
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
/* advertise the rc inputs */
memset(&_input_rc, 0, sizeof(_input_rc));
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
@@ -311,7 +344,7 @@ PX4IO::task_main()
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000);
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
/* this would be bad... */
if (ret < 0) {
@@ -361,8 +394,18 @@ PX4IO::task_main()
_send_needed = false;
io_send();
}
/* send a config packet to IO if required */
if (_config_needed) {
_config_needed = false;
config_send();
}
}
out:
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
@@ -409,15 +452,34 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report))
return;
lock();
/* XXX handle R/C inputs here ... needs code sharing/library */
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO */
_input_rc.timestamp = hrt_absolute_time();
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
/* remember the latched arming switch state */
_switch_armed = rep->armed;
_send_needed = true;
out:
unlock();
}
@@ -425,6 +487,7 @@ void
PX4IO::io_send()
{
px4io_command cmd;
int ret;
cmd.f2i_magic = F2I_MAGIC;
@@ -439,7 +502,23 @@ PX4IO::io_send()
cmd.arm_ok = _armed.armed;
hx_stream_send(_io_stream, &cmd, sizeof(cmd));
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
void
PX4IO::config_send()
{
px4io_config cfg;
int ret;
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
cfg.serial_rx_mode = _rx_mode;
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)
debug("config error %d", ret);
}
int
@@ -555,8 +634,49 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
return ret;
}
void
PX4IO::set_rx_mode(unsigned mode)
{
if (mode != _rx_mode) {
_rx_mode = mode;
_config_needed = true;
}
}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
{
void
test(void)
{
int fd;
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
if (fd < 0) {
puts("open fail");
exit(1);
}
ioctl(fd, PWM_SERVO_ARM, 0);
ioctl(fd, PWM_SERVO_SET(0), 1000);
ioctl(fd, PWM_SERVO_SET(1), 1100);
ioctl(fd, PWM_SERVO_SET(2), 1200);
ioctl(fd, PWM_SERVO_SET(3), 1300);
ioctl(fd, PWM_SERVO_SET(4), 1400);
ioctl(fd, PWM_SERVO_SET(5), 1500);
ioctl(fd, PWM_SERVO_SET(6), 1600);
ioctl(fd, PWM_SERVO_SET(7), 1700);
close(fd);
exit(0);
}
}
int
px4io_main(int argc, char *argv[])
{
@@ -624,5 +744,25 @@ px4io_main(int argc, char *argv[])
return ret;
}
errx(1, "need a verb, only support 'start' and 'update'");
if (!strcmp(argv[1], "rx_spektrum6")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6);
}
if (!strcmp(argv[1], "rx_spektrum7")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7);
}
if (!strcmp(argv[1], "rx_sbus")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
}
if (!strcmp(argv[1], "test"))
test();
errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'");
}
apps/drivers/stm32/drv_hrt.c
+27 -7
View File
@@ -276,6 +276,17 @@ static void hrt_call_invoke(void);
* Specific registers and bits used by PPM sub-functions
*/
#ifdef CONFIG_HRT_PPM
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*/
# ifndef GTIM_CCER_CC1NP
# define GTIM_CCER_CC1NP 0
# define GTIM_CCER_CC2NP 0
# define GTIM_CCER_CC3NP 0
# define GTIM_CCER_CC4NP 0
# define PPM_EDGE_FLIP
# endif
# if HRT_PPM_CHANNEL == 1
# define rCCR_PPM rCCR1 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */
@@ -284,6 +295,7 @@ static void hrt_call_invoke(void);
# define CCMR1_PPM 1 /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC1P
# elif HRT_PPM_CHANNEL == 2
# define rCCR_PPM rCCR2 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */
@@ -292,6 +304,7 @@ static void hrt_call_invoke(void);
# define CCMR1_PPM 2 /* not on TI1/TI2 */
# define CCMR2_PPM 0 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC2P
# elif HRT_PPM_CHANNEL == 3
# define rCCR_PPM rCCR3 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */
@@ -300,6 +313,7 @@ static void hrt_call_invoke(void);
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 1 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC3P
# elif HRT_PPM_CHANNEL == 4
# define rCCR_PPM rCCR4 /* capture register for PPM */
# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */
@@ -308,6 +322,7 @@ static void hrt_call_invoke(void);
# define CCMR1_PPM 0 /* not on TI1/TI2 */
# define CCMR2_PPM 2 /* on TI3, not on TI4 */
# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC4P
# else
# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
# endif
@@ -323,7 +338,7 @@ static void hrt_call_invoke(void);
/* decoded PPM buffer */
#define PPM_MAX_CHANNELS 12
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
__EXPORT unsigned ppm_decoded_channels;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
/* PPM edge history */
@@ -371,12 +386,12 @@ static void hrt_ppm_decode(uint32_t status);
static void
hrt_tim_init(void)
{
/* clock/power on our timer */
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
/* claim our interrupt vector */
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
/* clock/power on our timer */
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
/* disable and configure the timer */
rCR1 = 0;
rCR2 = 0;
@@ -532,9 +547,14 @@ hrt_tim_isr(int irq, void *context)
#ifdef CONFIG_HRT_PPM
/* was this a PPM edge? */
if (status & (SR_INT_PPM | SR_OVF_PPM))
hrt_ppm_decode(status);
if (status & (SR_INT_PPM | SR_OVF_PPM)) {
/* if required, flip edge sensitivity */
# ifdef PPM_EDGE_FLIP
rCCER ^= CCER_PPM_FLIP;
# endif
hrt_ppm_decode(status);
}
#endif
/* was this a timer tick? */
@@ -562,7 +582,7 @@ hrt_absolute_time(void)
{
hrt_abstime abstime;
uint32_t count;
uint32_t flags;
irqstate_t flags;
/*
* Counter state. Marked volatile as they may change
apps/drivers/stm32/drv_pwm_servo.c
+5 -9
View File
@@ -171,10 +171,8 @@ pwm_channel_init(unsigned channel)
int
up_pwm_servo_set(unsigned channel, servo_position_t value)
{
if (channel >= PWM_SERVO_MAX_CHANNELS) {
lldbg("pwm_channel_set: bogus channel %u\n", channel);
if (channel >= PWM_SERVO_MAX_CHANNELS)
return -1;
}
unsigned timer = pwm_channels[channel].timer_index;
@@ -214,17 +212,15 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
servo_position_t
up_pwm_servo_get(unsigned channel)
{
if (channel >= PWM_SERVO_MAX_CHANNELS) {
lldbg("pwm_channel_get: bogus channel %u\n", channel);
if (channel >= PWM_SERVO_MAX_CHANNELS)
return 0;
}
unsigned timer = pwm_channels[channel].timer_index;
servo_position_t value = 0;
/* test timer for validity */
if ((pwm_timers[timer].base == 0) ||
(pwm_channels[channel].gpio == 0))
(pwm_channels[channel].timer_channel == 0))
return 0;
/* configure the channel */
@@ -246,7 +242,7 @@ up_pwm_servo_get(unsigned channel)
break;
}
return value;
return value + 1;
}
int
@@ -261,7 +257,7 @@ up_pwm_servo_init(uint32_t channel_mask)
/* now init channels */
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
/* don't do init for disabled channels; this leaves the pin configs alone */
if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0))
if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0))
pwm_channel_init(i);
}
apps/drivers/stm32/drv_pwm_servo.h
+3 -4
View File
@@ -42,7 +42,7 @@
#include <drivers/drv_pwm_output.h>
/* configuration limits */
#define PWM_SERVO_MAX_TIMERS 2
#define PWM_SERVO_MAX_TIMERS 4
#define PWM_SERVO_MAX_CHANNELS 8
/* array of timers dedicated to PWM servo use */
@@ -53,9 +53,6 @@ struct pwm_servo_timer {
uint32_t clock_freq;
};
/* supplied by board-specific code */
__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
/* array of channels in logical order */
struct pwm_servo_channel {
uint32_t gpio;
@@ -64,4 +61,6 @@ struct pwm_servo_channel {
servo_position_t default_value;
};
/* supplied by board-specific code */
__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
apps/mavlink/mavlink.c
+7 -8
View File
@@ -589,16 +589,15 @@ int mavlink_thread_main(int argc, char *argv[])
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 5 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
apps/mavlink/mavlink_parameters.c
+53 -51
View File
@@ -233,66 +233,68 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
} break;
case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
// case MAVLINK_MSG_ID_COMMAND_LONG: {
// mavlink_command_long_t cmd_mavlink;
// mavlink_msg_command_long_decode(msg, &cmd_mavlink);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// uint8_t result = MAV_RESULT_UNSUPPORTED;
if (cmd_mavlink.target_system == mavlink_system.sysid &&
((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
// if (cmd_mavlink.target_system == mavlink_system.sysid &&
// ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
/* preflight parameter load / store */
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
/* Read all parameters from EEPROM to RAM */
// // XXX move this to LOW PRIO THREAD of commander app
if (((int)(cmd_mavlink.param1)) == 0) {
// /* preflight parameter load / store */
// if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
// /* Read all parameters from EEPROM to RAM */
/* read all parameters from EEPROM to RAM */
int read_ret = param_load_default();
if (read_ret == OK) {
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
result = MAV_RESULT_ACCEPTED;
} else if (read_ret == 1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load");
result = MAV_RESULT_ACCEPTED;
} else {
if (read_ret < -1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM");
} else {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found");
}
result = MAV_RESULT_FAILED;
}
// if (((int)(cmd_mavlink.param1)) == 0) {
} else if (((int)(cmd_mavlink.param1)) == 1) {
// /* read all parameters from EEPROM to RAM */
// int read_ret = param_load_default();
// if (read_ret == OK) {
// //printf("[mavlink pm] Loaded EEPROM params in RAM\n");
// mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
// } else if (read_ret == 1) {
// mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
// } else {
// if (read_ret < -1) {
// mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file());
// } else {
// mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file());
// }
// result = MAV_RESULT_FAILED;
// }
/* write all parameters from RAM to EEPROM */
int write_ret = param_save_default();
if (write_ret == OK) {
mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
// } else if (((int)(cmd_mavlink.param1)) == 1) {
} else {
if (write_ret < -1) {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM");
} else {
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found");
}
result = MAV_RESULT_FAILED;
}
// /* write all parameters from RAM to EEPROM */
// int write_ret = param_save_default();
// if (write_ret == OK) {
// mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file());
// result = MAV_RESULT_ACCEPTED;
} else {
//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request");
result = MAV_RESULT_UNSUPPORTED;
}
}
}
// } else {
// if (write_ret < -1) {
// mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
// } else {
// mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
// }
// result = MAV_RESULT_FAILED;
// }
/* send back command result */
//mavlink_msg_command_ack_send(chan, cmd.command, result);
} break;
// } else {
// //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
// mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request");
// result = MAV_RESULT_UNSUPPORTED;
// }
// }
// }
// /* send back command result */
// //mavlink_msg_command_ack_send(chan, cmd.command, result);
// } break;
}
}
apps/mavlink/mavlink_receiver.c
+5 -9
View File
@@ -253,12 +253,12 @@ handle_message(mavlink_message_t *msg)
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid] == 0) {
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
ml_armed = false;
}
@@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg)
rc_hil.timestamp = hrt_absolute_time();
rc_hil.chan_count = 4;
rc_hil.chan[0].raw = 1500 + man.x / 2;
rc_hil.chan[1].raw = 1500 + man.y / 2;
rc_hil.chan[2].raw = 1500 + man.r / 2;
rc_hil.chan[3].raw = 1500 + man.z / 2;
rc_hil.chan[0].scaled = man.x / 1000.0f;
rc_hil.chan[1].scaled = man.y / 1000.0f;
apps/mavlink/orb_listener.c
+25 -10
View File
@@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
struct mavlink_subscriptions mavlink_subs;
@@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l);
static void l_vehicle_gps_position(struct listener *l);
static void l_vehicle_status(struct listener *l);
static void l_rc_channels(struct listener *l);
static void l_input_rc(struct listener *l);
static void l_global_position(struct listener *l);
static void l_local_position(struct listener *l);
static void l_global_position_setpoint(struct listener *l);
@@ -116,6 +118,7 @@ struct listener listeners[] = {
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
{l_vehicle_status, &status_sub, 0},
{l_rc_channels, &rc_sub, 0},
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
{l_global_position, &mavlink_subs.global_pos_sub, 0},
{l_local_position, &mavlink_subs.local_pos_sub, 0},
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
@@ -274,21 +277,29 @@ l_rc_channels(struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
// XXX Add RC channels scaled message here
}
void
l_input_rc(struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
if (gcs_link)
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
rc.timestamp / 1000,
rc_raw.timestamp / 1000,
0,
rc.chan[0].raw,
rc.chan[1].raw,
rc.chan[2].raw,
rc.chan[3].raw,
rc.chan[4].raw,
rc.chan[5].raw,
rc.chan[6].raw,
rc.chan[7].raw,
rc.rssi);
(rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
(rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
(rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
(rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
(rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
(rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
(rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
(rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
255);
}
void
@@ -583,6 +594,10 @@ uorb_receive_start(void)
rc_sub = orb_subscribe(ORB_ID(rc_channels));
orb_set_interval(rc_sub, 100); /* 10Hz updates */
/* --- RC RAW VALUE --- */
mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
orb_set_interval(mavlink_subs.input_rc_sub, 100);
/* --- GLOBAL POS VALUE --- */
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
apps/mavlink/orb_topics.h
+2
View File
@@ -57,6 +57,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
struct mavlink_subscriptions {
int sensor_sub;
@@ -75,6 +76,7 @@ struct mavlink_subscriptions {
int spl_sub;
int spg_sub;
int debug_key_value;
int input_rc_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
apps/multirotor_att_control/multirotor_att_control_main.c
+2 -2
View File
@@ -220,9 +220,9 @@ mc_thread_main(int argc, char *argv[])
/* only move setpoint if manual input is != 0 */
// XXX turn into param
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.25f) {
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
} else if (manual.throttle <= 0.25f) {
} else if (manual.throttle <= 0.3f) {
att_sp.yaw_body = att.yaw;
}
att_sp.thrust = manual.throttle;
apps/multirotor_att_control/multirotor_attitude_control.c
+38 -38
View File
@@ -57,50 +57,50 @@
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params {
float yaw_p;
float yaw_i;
float yaw_d;
float yaw_awu;
float yaw_lim;
//float yaw_awu;
//float yaw_lim;
float att_p;
float att_i;
float att_d;
float att_awu;
float att_lim;
//float att_awu;
//float att_lim;
float att_xoff;
float att_yoff;
//float att_xoff;
//float att_yoff;
};
struct mc_att_control_param_handles {
param_t yaw_p;
param_t yaw_i;
param_t yaw_d;
param_t yaw_awu;
param_t yaw_lim;
//param_t yaw_awu;
//param_t yaw_lim;
param_t att_p;
param_t att_i;
param_t att_d;
param_t att_awu;
param_t att_lim;
//param_t att_awu;
//param_t att_lim;
param_t att_xoff;
param_t att_yoff;
//param_t att_xoff;
//param_t att_yoff;
};
/**
@@ -122,17 +122,17 @@ static int parameters_init(struct mc_att_control_param_handles *h)
h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D");
h->yaw_awu = param_find("MC_YAWPOS_AWU");
h->yaw_lim = param_find("MC_YAWPOS_LIM");
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->att_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I");
h->att_d = param_find("MC_ATT_D");
h->att_awu = param_find("MC_ATT_AWU");
h->att_lim = param_find("MC_ATT_LIM");
//h->att_awu = param_find("MC_ATT_AWU");
//h->att_lim = param_find("MC_ATT_LIM");
h->att_xoff = param_find("MC_ATT_XOFF");
h->att_yoff = param_find("MC_ATT_YOFF");
//h->att_xoff = param_find("MC_ATT_XOFF");
//h->att_yoff = param_find("MC_ATT_YOFF");
return OK;
}
@@ -142,17 +142,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d));
param_get(h->yaw_awu, &(p->yaw_awu));
param_get(h->yaw_lim, &(p->yaw_lim));
//param_get(h->yaw_awu, &(p->yaw_awu));
//param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->att_p, &(p->att_p));
param_get(h->att_i, &(p->att_i));
param_get(h->att_d, &(p->att_d));
param_get(h->att_awu, &(p->att_awu));
param_get(h->att_lim, &(p->att_lim));
//param_get(h->att_awu, &(p->att_awu));
//param_get(h->att_lim, &(p->att_lim));
param_get(h->att_xoff, &(p->att_xoff));
param_get(h->att_yoff, &(p->att_yoff));
//param_get(h->att_xoff, &(p->att_xoff));
//param_get(h->att_yoff, &(p->att_yoff));
return OK;
}
@@ -186,10 +186,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
p.att_lim, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
p.att_lim, PID_MODE_DERIVATIV_SET);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
initialized = true;
}
@@ -202,18 +202,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* apply parameters */
printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim);
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
apps/multirotor_att_control/multirotor_rate_control.c
+20 -20
View File
@@ -58,28 +58,28 @@
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
float yawrate_awu;
float yawrate_lim;
//float yawrate_awu;
//float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
float attrate_awu;
float attrate_lim;
//float attrate_awu;
//float attrate_lim;
float rate_lim;
};
@@ -89,14 +89,14 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t yawrate_awu;
param_t yawrate_lim;
//param_t yawrate_awu;
//param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
param_t attrate_awu;
param_t attrate_lim;
//param_t attrate_awu;
//param_t attrate_lim;
};
/**
@@ -118,14 +118,14 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->yawrate_awu = param_find("MC_YAWRATE_AWU");
h->yawrate_lim = param_find("MC_YAWRATE_LIM");
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
h->attrate_awu = param_find("MC_ATTRATE_AWU");
h->attrate_lim = param_find("MC_ATTRATE_LIM");
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
return OK;
}
@@ -135,14 +135,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
//param_get(h->yawrate_awu, &(p->yawrate_awu));
//param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
param_get(h->attrate_awu, &(p->attrate_awu));
param_get(h->attrate_lim, &(p->attrate_lim));
//param_get(h->attrate_awu, &(p->attrate_awu));
//param_get(h->attrate_lim, &(p->attrate_lim));
return OK;
}
apps/px4/tests/test_hrt.c
-2
View File
@@ -121,7 +121,6 @@ int test_ppm(int argc, char *argv[])
int test_tone(int argc, char *argv[])
{
#ifdef CONFIG_TONE_ALARM
int fd, result;
unsigned long tone;
@@ -171,7 +170,6 @@ out:
if (fd >= 0)
close(fd);
#endif
return 0;
}
apps/px4io/Makefile
+5 -6
View File
@@ -39,11 +39,10 @@
# Note that we pull a couple of specific files from the systemlib, since
# we can't support it all.
#
CSRCS = comms.c \
mixer.c \
px4io.c \
safety.c \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c
CSRCS = $(wildcard *.c) \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk
apps/px4io/comms.c
+62 -10
View File
@@ -45,6 +45,7 @@
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <poll.h>
#include <nuttx/clock.h>
@@ -81,7 +82,6 @@ comms_check(void)
{
static hrt_abstime last_report_time;
hrt_abstime now, delta;
uint8_t c;
/* should we send a report to the FMU? */
now = hrt_absolute_time();
@@ -93,7 +93,7 @@ comms_check(void)
last_report_time = now;
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
for (int i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
@@ -102,21 +102,47 @@ comms_check(void)
hx_stream_send(stream, &report, sizeof(report));
}
/* feed any received bytes to the HDLC receive engine */
while (read(fmu_fd, &c, 1) == 1)
hx_stream_rx(stream, c);
/*
* Check for bytes and feed them to the RX engine.
* Limit the number of bytes we actually process on any one iteration.
*/
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
int frame_rx;
int frame_bad;
static void
comms_handle_config(const void *buffer, size_t length)
{
const struct px4io_config *cfg = (struct px4io_config *)buffer;
if (length != sizeof(*cfg)) {
frame_bad++;
return;
}
frame_rx++;
mixer_set_serial_mode(cfg->serial_rx_mode);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
comms_handle_command(const void *buffer, size_t length)
{
struct px4io_command *cmd;
const struct px4io_command *cmd = (struct px4io_command *)buffer;
/* make sure it's what we are expecting */
if (length != sizeof(struct px4io_command))
if (length != sizeof(*cmd)) {
frame_bad++;
return;
}
cmd = (struct px4io_command *)buffer;
frame_rx++;
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
@@ -133,4 +159,30 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
system_state.relays[i] = cmd->relay_state[i];
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
const uint16_t *type = (const uint16_t *)buffer;
/* make sure it's what we are expecting */
if (length > 2) {
switch (*type) {
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
default:
break;
}
}
frame_bad++;
}
apps/px4io/mixer.c
+185 -49
View File
@@ -40,23 +40,21 @@
#include <sys/types.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <arch/board/drv_ppm_input.h>
#include <arch/board/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/ppm_decode.h>
#include "px4io.h"
#ifdef CONFIG_DISABLE_MQUEUE
# error Mixer requires message queues - set CONFIG_DISABLE_MQUEUE=n and try again
#endif
static mqd_t input_queue;
/*
* Count of periodic calls in which we have no data.
*/
@@ -67,7 +65,12 @@ static unsigned mixer_input_drops;
* Count of periodic calls in which we have no FMU input.
*/
static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 10
#define FMU_INPUT_DROP_LIMIT 20
/*
* Serial port fd for serial RX protocols
*/
static int rx_port = -1;
/*
* HRT periodic call used to check for control input data.
@@ -89,9 +92,6 @@ static void mixer_get_rc_input(void);
*/
static void mixer_update(int mixer, uint16_t *inputs, int input_count);
/* servo driver handle */
int mixer_servo_fd;
/* current servo arm/disarm state */
bool mixer_servos_armed;
@@ -104,15 +104,10 @@ struct mixer {
} mixers[IO_SERVO_COUNT];
int
mixer_init(const char *mq_name)
mixer_init(void)
{
/* open the control input queue; this should always exist */
input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK);
ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL);
/* open the servo driver */
mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY);
ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL);
/* open the serial port */
rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
/* look for control data at 50Hz */
hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
@@ -120,6 +115,38 @@ mixer_init(const char *mq_name)
return 0;
}
void
mixer_set_serial_mode(uint8_t serial_mode)
{
if (serial_mode == system_state.serial_rx_mode)
return;
struct termios t;
tcgetattr(rx_port, &t);
switch (serial_mode) {
case RX_MODE_PPM_ONLY:
break;
case RX_MODE_SPEKTRUM_6:
case RX_MODE_SPEKTRUM_7:
/* 115200, no parity, one stop bit */
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
break;
case RX_MODE_FUTABA_SBUS:
/* 100000, even parity, two stop bits */
cfsetspeed(&t, 100000);
t.c_cflag |= (CSTOPB | PARENB);
break;
default:
return;
}
tcsetattr(rx_port, TCSANOW, &t);
system_state.serial_rx_mode = serial_mode;
}
static void
mixer_tick(void *arg)
{
@@ -176,9 +203,8 @@ mixer_tick(void *arg)
* If we are armed, update the servo output.
*/
if (system_state.armed)
ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value);
up_pwm_servo_set(i, mixers[i].current_value);
}
}
/*
@@ -187,34 +213,155 @@ mixer_tick(void *arg)
should_arm = system_state.armed && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0);
up_pwm_servo_arm(true);
mixer_servos_armed = true;
} else if (!should_arm && mixer_servos_armed) {
/* armed but need to disarm*/
ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0);
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
}
static void
mixer_update(int mixer, uint16_t *inputs, int input_count)
{
/* simple passthrough for now */
if (mixer < input_count) {
mixers[mixer].current_value = inputs[mixer];
} else {
mixers[mixer].current_value = 0;
}
}
static bool
mixer_get_spektrum_input(void)
{
static uint8_t buf[16];
static unsigned count;
/* always read as much data as we can into the buffer */
if (count >= sizeof(buf))
count = 0;
ssize_t result = read(rx_port, buf, sizeof(buf) - count);
/* no data or an error */
if (result <= 0)
return false;
count += result;
/* if there are more than two bytes in the buffer, check for sync */
if (count >= 2) {
if ((buf[0] != 0x3) || (buf[1] != 0x1)) {
/* not in sync; look for a possible sync marker */
for (unsigned i = 1; i < count; i++) {
if (buf[i] == 0x3) {
/* could be a frame marker; move buffer bytes */
count -= i;
memmove(buf, buf + i, count);
break;
}
}
}
}
if (count < sizeof(buf))
return false;
/* we got a frame; decode it */
const uint16_t *channels = (const uint16_t *)&buf[2];
/*
* Channel assignment for DX6i vs. DX7 is different.
*
* DX7 etc. is:
*
* 0: Aileron
* 1: Flaps
* 2: Gear
* 3: Elevator
* 4: Aux2
* 5: Throttle
* 6: Rudder
*
* DX6i is:
*
* 0: Aileron
* 1: Flaps
* 2: Elevator
* 3: Rudder
* 4: Throttle
* 5: Gear
* 6: <notused>
*
* We convert these to our standard Futaba-style assignment:
*
* 0: Throttle (Throttle)
* 1: Roll (Aileron)
* 2: Pitch (Elevator)
* 3: Yaw (Rudder)
* 4: Override (Flaps)
* 5: FUNC_0 (Gear)
* 6: FUNC_1 (Aux2)
*/
if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) {
system_state.rc_channel_data[0] = channels[5]; /* Throttle */
system_state.rc_channel_data[1] = channels[0]; /* Roll */
system_state.rc_channel_data[2] = channels[3]; /* Pitch */
system_state.rc_channel_data[3] = channels[6]; /* Yaw */
system_state.rc_channel_data[4] = channels[1]; /* Override */
system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */
system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */
system_state.rc_channels = 7;
} else {
system_state.rc_channel_data[0] = channels[4]; /* Throttle */
system_state.rc_channel_data[1] = channels[0]; /* Roll */
system_state.rc_channel_data[2] = channels[2]; /* Pitch */
system_state.rc_channel_data[3] = channels[3]; /* Yaw */
system_state.rc_channel_data[4] = channels[1]; /* Override */
system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */
system_state.rc_channels = 6;
}
count = 0;
return true;
}
static bool
mixer_get_sbus_input(void)
{
/* XXX not implemented yet */
return false;
}
static void
mixer_get_rc_input(void)
{
ssize_t len;
bool got_input = false;
/*
* Pull channel data from the message queue into the system state structure.
*
*/
len = mq_receive(input_queue, &system_state.rc_channel_data, sizeof(system_state.rc_channel_data), NULL);
switch (system_state.serial_rx_mode) {
case RX_MODE_PPM_ONLY:
if (ppm_decoded_channels > 0) {
/* copy channel data */
system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[i];
got_input = true;
}
break;
/*
* If we have data, update the count and status.
*/
if (len > 0) {
system_state.rc_channels = len / sizeof(system_state.rc_channel_data[0]);
case RX_MODE_SPEKTRUM_6:
case RX_MODE_SPEKTRUM_7:
got_input = mixer_get_spektrum_input();
break;
case RX_MODE_FUTABA_SBUS:
got_input = mixer_get_sbus_input();
break;
default:
break;
}
if (got_input) {
mixer_input_drops = 0;
system_state.fmu_report_due = true;
} else {
/*
@@ -232,14 +379,3 @@ mixer_get_rc_input(void)
}
}
}
static void
mixer_update(int mixer, uint16_t *inputs, int input_count)
{
/* simple passthrough for now */
if (mixer < input_count) {
mixers[mixer].current_value = inputs[mixer];
} else {
mixers[mixer].current_value = 0;
}
}
apps/px4io/protocol.h
+13 -1
View File
@@ -48,7 +48,7 @@
#define PX4IO_OUTPUT_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 2
#define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1)
@@ -62,6 +62,18 @@ struct px4io_command {
bool arm_ok;
};
/* config message from FMU to IO */
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
uint8_t serial_rx_mode;
#define RX_MODE_PPM_ONLY 0
#define RX_MODE_SPEKTRUM_6 1
#define RX_MODE_SPEKTRUM_7 2
#define RX_MODE_FUTABA_SBUS 3
};
/* report from IO to FMU */
struct px4io_report {
uint16_t i2f_magic;
apps/px4io/px4io.c
+25 -31
View File
@@ -47,9 +47,7 @@
#include <nuttx/clock.h>
#include <arch/board/up_boardinitialize.h>
#include <arch/board/drv_gpio.h>
#include <arch/board/drv_ppm_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
@@ -61,8 +59,6 @@ int gpio_fd;
static const char cursor[] = {'|', '/', '-', '\\'};
static const char *rc_input_mq_name = "rc_input";
static struct hrt_call timer_tick_call;
volatile int timers[TIMER_NUM_TIMERS];
static void timer_tick(void *arg);
@@ -73,8 +69,15 @@ int user_start(int argc, char *argv[])
bool heartbeat = false;
bool failsafe = false;
/* Do board init */
(void)up_boardinitialize();
/* configure the high-resolution time/callout interface */
hrt_init();
/* init the FMU link */
comms_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
up_pwm_servo_init(0xff);
/* print some startup info */
lib_lowprintf("\nPX4IO: starting\n");
@@ -84,32 +87,22 @@ int user_start(int argc, char *argv[])
/* start the software timer service */
hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL);
/* Open the GPIO driver so we can do LEDs and the like. */
gpio_fd = open("/dev/gpio", 0);
ASSERTCODE((gpio_fd >= 0), A_GPIO_OPEN_FAIL);
/* default all the LEDs to off while we start */
LED_AMBER(heartbeat);
LED_BLUE(failsafe);
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
/* turn on servo power */
POWER_SERVO(true);
/* configure the PPM input driver */
ppm_input_init(rc_input_mq_name);
/* start the mixer */
mixer_init(rc_input_mq_name);
mixer_init();
/* start the safety switch handler */
safety_init();
/* init the FMU link */
comms_init();
/* set up some timers for the main loop */
timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */
timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */
timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */
/*
@@ -121,33 +114,34 @@ int user_start(int argc, char *argv[])
comms_check();
/* blink the heartbeat LED */
if (timers[TIMER_BLINK_AMBER] == 0) {
timers[TIMER_BLINK_AMBER] = 250;
LED_AMBER((heartbeat = !heartbeat));
if (timers[TIMER_BLINK_BLUE] == 0) {
timers[TIMER_BLINK_BLUE] = 250;
LED_BLUE(heartbeat = !heartbeat);
}
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
if (timers[TIMER_BLINK_BLUE] == 0) {
timers[TIMER_BLINK_BLUE] = 125;
LED_BLUE((failsafe = !failsafe));
if (timers[TIMER_BLINK_AMBER] == 0) {
timers[TIMER_BLINK_AMBER] = 125;
failsafe = !failsafe;
}
} else {
LED_BLUE((failsafe = false));
failsafe = false;
}
LED_AMBER(failsafe);
/* print some simple status */
if (timers[TIMER_STATUS_PRINT] == 0) {
timers[TIMER_STATUS_PRINT] = 1000;
lib_lowprintf("%c %s | %s | %s | C=%d \r",
lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r",
cursor[cycle++ & 3],
(system_state.armed ? "ARMED" : "SAFE"),
(system_state.rc_channels ? "RC OK" : "NO RC"),
(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
system_state.rc_channels
system_state.rc_channels,
frame_rx, frame_bad
);
}
}
/* Should never reach here */
apps/px4io/px4io.h
+42 -16
View File
@@ -32,10 +32,18 @@
****************************************************************************/
/**
* @file General defines and structures for the PX4IO module firmware.
* @file px4io.h
*
* General defines and structures for the PX4IO module firmware.
*/
#include <arch/board/drv_gpio.h>
#include <nuttx/config.h>
#include <stdbool.h>
#include <stdint.h>
#include <drivers/boards/px4io/px4io_internal.h>
#include "protocol.h"
/*
@@ -44,6 +52,16 @@
#define MAX_CONTROL_CHANNELS 12
#define IO_SERVO_COUNT 8
/*
* Debug logging
*/
#if 1
# define debug(fmt, ...) lib_lowprintf(fmt "\n", ##args)
#else
# define debug(fmt, ...) do {} while(0)
#endif
/*
* System state structure.
*/
@@ -83,10 +101,19 @@ struct sys_state_s
* If true, new control data from the FMU has been received.
*/
bool fmu_data_received;
/*
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
*/
uint8_t serial_rx_mode;
};
extern struct sys_state_s system_state;
extern int frame_rx;
extern int frame_bad;
/*
* Software countdown timers.
*
@@ -102,26 +129,25 @@ extern volatile int timers[TIMER_NUM_TIMERS];
/*
* GPIO handling.
*/
extern int gpio_fd;
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define POWER_SERVO(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_POWER), (_s))
#define POWER_ACC1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC1), (_s))
#define POWER_ACC2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC2), (_s))
#define POWER_RELAY1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY1, (_s))
#define POWER_RELAY2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY2, (_s))
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s))
#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
#define LED_AMBER(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_AMBER), !(_s))
#define LED_BLUE(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_BLUE), !(_s))
#define LED_SAFETY(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_SAFETY), !(_s))
#define OVERCURRENT_ACC ioctl(gpio_fd, GPIO_GET(GPIO_ACC_OVERCURRENT), 0)
#define OVERCURRENT_SERVO ioctl(gpio_fd, GPIO_GET(GPIO_SERVO_OVERCURRENT), 0)
#define BUTTON_SAFETY ioctl(gpio_fd, GPIO_GET(GPIO_SAFETY_BUTTON), 0)
#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT)
#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
/*
* Mixer
*/
extern int mixer_init(const char *mq_name);
extern int mixer_init(void);
extern void mixer_set_serial_mode(uint8_t newmode);
/*
* Safety switch/LED.
apps/px4io/safety.c
-3
View File
@@ -46,9 +46,6 @@
#include <nuttx/clock.h>
#include <arch/board/up_boardinitialize.h>
#include <arch/board/drv_gpio.h>
#include <arch/board/drv_ppm_input.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
apps/sensors/sensor_params.c
+7 -7
View File
@@ -68,28 +68,28 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 30.0f);
PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 30.0f);
PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
@@ -103,21 +103,21 @@ PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.1f);
PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
apps/sensors/sensors.cpp
+119 -99
View File
@@ -58,12 +58,15 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_rc_input.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
@@ -90,19 +93,6 @@
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#ifdef CONFIG_HRT_PPM
extern "C" {
extern uint16_t ppm_buffer[];
extern unsigned ppm_decoded_channels;
extern uint64_t ppm_last_valid_decode;
}
/* PPM Settings */
# define PPM_MIN 1000
# define PPM_MAX 2000
# define PPM_MID (PPM_MIN+PPM_MAX)/2
#endif
/**
* Sensor app start / stop handling function
*
@@ -155,6 +145,7 @@ private:
int _gyro_sub; /**< raw gyro data subscription */
int _accel_sub; /**< raw accel data subscription */
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -174,6 +165,7 @@ private:
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];
float mag_offset[3];
@@ -343,6 +335,7 @@ Sensors::Sensors() :
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
_vstatus_sub(-1),
_params_sub(-1),
@@ -476,14 +469,13 @@ Sensors::parameters_update()
warnx("Failed getting exponential gain for chan %d", i);
}
_rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
/* handle blowup in the scaling factor calculation */
if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) {
_rc.chan[i].scaling_factor = 0;
if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
_parameters.scaling_factor[i] = 0;
}
_rc.chan[i].mid = _parameters.trim[i];
}
/* update RC function mappings */
@@ -868,99 +860,126 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
void
Sensors::ppm_poll()
{
struct manual_control_setpoint_s manual_control;
/* fake low-level driver, directly pulling from driver variables */
static orb_advert_t rc_input_pub = -1;
struct rc_input_values raw;
/* check to see whether a new frame has been decoded */
if (_ppm_last_valid == ppm_last_valid_decode)
return;
/* require at least two chanels to consider the signal valid */
if (ppm_decoded_channels < 4)
return;
raw.timestamp = ppm_last_valid_decode;
unsigned channel_limit = ppm_decoded_channels;
if (channel_limit > _rc_max_chan_count)
channel_limit = _rc_max_chan_count;
if (ppm_decoded_channels > 1) {
/* we are accepting this decode */
_ppm_last_valid = ppm_last_valid_decode;
/* Read out values from HRT */
for (unsigned int i = 0; i < channel_limit; i++) {
_rc.chan[i].raw = ppm_buffer[i];
/* scale around the mid point differently for lower and upper range */
if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
for (int i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
}
/* reverse channel if required */
if (i == _rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
raw.channel_count = ppm_decoded_channels;
/* publish to object request broker */
if (rc_input_pub <= 0) {
rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
} else {
orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
}
}
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
bool rc_updated;
orb_check(_rc_sub, &rc_updated);
if (rc_updated) {
struct rc_input_values rc_input;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
struct manual_control_setpoint_s manual_control;
/* require at least two chanels to consider the signal valid */
if (rc_input.channel_count < 2)
return;
unsigned channel_limit = rc_input.channel_count;
if (channel_limit > _rc_max_chan_count)
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
_ppm_last_valid = rc_input.timestamp;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
/* scale around the mid point differently for lower and upper range */
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
/* reverse channel if required */
if (i == _rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
_rc.chan[i].scaled = 0.0f;
}
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
_rc.chan[i].scaled = 0.0f;
_rc.chan_count = rc_input.channel_count;
_rc.timestamp = rc_input.timestamp;
//_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor;
manual_control.timestamp = rc_input.timestamp;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
manual_control.roll *= _parameters.rc_scale_roll;
}
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
manual_control.pitch *= _parameters.rc_scale_pitch;
}
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* mode switch input */
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
}
_rc.chan_count = ppm_decoded_channels;
_rc.timestamp = ppm_last_valid_decode;
manual_control.timestamp = ppm_last_valid_decode;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
manual_control.roll *= _parameters.rc_scale_roll;
}
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
manual_control.pitch *= _parameters.rc_scale_pitch;
}
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* mode switch input */
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
}
#endif
@@ -991,6 +1010,7 @@ Sensors::task_main()
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
apps/systemcmds/param/param.c
+16 -11
View File
@@ -59,7 +59,7 @@ __EXPORT int param_main(int argc, char *argv[]);
static void do_save(const char* param_file_name);
static void do_load(const char* param_file_name);
static void do_import(const char* param_file_name);
static void do_show(void);
static void do_show(const char* search_string);
static void do_show_print(void *arg, param_t param);
int
@@ -96,13 +96,16 @@ param_main(int argc, char *argv[])
} else {
param_set_default_file(NULL);
}
warnx("selected parameter file %s", param_get_default_file());
warnx("selected parameter default file %s", param_get_default_file());
exit(0);
}
if (!strcmp(argv[1], "show")) {
do_show();
}
if (!strcmp(argv[1], "show"))
if (argc >= 3) {
do_show(argv[2]);
} else {
do_show(NULL);
}
}
errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'");
@@ -144,9 +147,6 @@ do_load(const char* param_file_name)
if (result < 0) {
errx(1, "error importing from '%s'", param_file_name);
} else {
/* set default file name for next storage operation */
param_set_default_file(param_file_name);
}
exit(0);
@@ -170,10 +170,10 @@ do_import(const char* param_file_name)
}
static void
do_show(void)
do_show(const char* search_string)
{
printf(" + = saved, * = unsaved\n");
param_foreach(do_show_print, NULL, false);
param_foreach(do_show_print, search_string, false);
exit(0);
}
@@ -183,6 +183,11 @@ do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char*)arg;
/* print nothing if search string valid and not matching */
if (arg != NULL && (strcmp(search_string, param_name(param) != 0)))
return;
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
apps/systemlib/hx_stream.c
+10 -6
View File
@@ -160,6 +160,11 @@ hx_stream_init(int fd,
void
hx_stream_free(hx_stream_t stream)
{
/* free perf counters (OK if they are NULL) */
perf_free(stream->pc_tx_frames);
perf_free(stream->pc_rx_frames);
perf_free(stream->pc_rx_errors);
free(stream);
}
@@ -186,10 +191,8 @@ hx_stream_send(hx_stream_t stream,
const uint8_t *p = (const uint8_t *)data;
unsigned resid = count;
if (resid > HX_STREAM_MAX_FRAME) {
errno = EINVAL;
return -1;
}
if (resid > HX_STREAM_MAX_FRAME)
return -EINVAL;
/* start the frame */
hx_tx_raw(stream, FBO);
@@ -214,10 +217,11 @@ hx_stream_send(hx_stream_t stream,
/* check for transmit error */
if (stream->txerror) {
stream->txerror = false;
return -1;
return -EIO;
}
return -1;
perf_count(stream->pc_tx_frames);
return 0;
}
void
apps/systemlib/hx_stream.h
+1 -2
View File
@@ -102,8 +102,7 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
* @param stream A handle returned from hx_stream_init.
* @param data Pointer to the data to send.
* @param count The number of bytes to send.
* @return Zero on success, nonzero with errno
* set on error.
* @return Zero on success, -errno on error.
*/
__EXPORT extern int hx_stream_send(hx_stream_t stream,
const void *data,
apps/systemlib/param/param.c
+1 -1
View File
@@ -482,7 +482,7 @@ param_reset_all(void)
}
static const char *param_default_file = "/eeprom/parameters";
static char *param_user_file;
static char *param_user_file = NULL;
int
param_set_default_file(const char* filename)
apps/systemlib/pid/pid.c
+2 -2
View File
@@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (output > pid->limit) output = pid->limit;
//if (output > pid->limit) output = pid->limit;
if (output < -pid->limit) output = -pid->limit;
//if (output < -pid->limit) output = -pid->limit;
if (isfinite(output)) {
pid->last_output = output;
apps/systemlib/ppm_decode.h
+6 -1
View File
@@ -46,12 +46,17 @@
*/
#define PPM_MAX_CHANNELS 12
/* PPM input nominal min/max values */
#define PPM_MIN 1000
#define PPM_MAX 2000
#define PPM_MID ((PPM_MIN + PPM_MAX) / 2)
__BEGIN_DECLS
/*
* PPM decoder state
*/
__EXPORT extern uint16_t ppm_buffer[]; /**< decoded PPM channel values */
__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */
__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */
apps/uORB/topics/rc_channels.h
+1 -13
View File
@@ -50,14 +50,6 @@
* @{
*/
enum RC_CHANNELS_STATUS
{
UNKNOWN = 0,
KNOWN = 1,
SIGNAL = 2,
TIMEOUT = 3
};
/**
* This defines the mapping of the RC functions.
* The value assigned to the specific function corresponds to the entry of
@@ -85,12 +77,7 @@ struct rc_channels_s {
uint64_t timestamp; /**< In microseconds since boot time. */
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
struct {
uint16_t mid; /**< midpoint (0). */
float scaling_factor; /**< scaling factor from raw counts to -1..+1 */
uint16_t raw; /**< current raw value */
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
uint16_t override;
enum RC_CHANNELS_STATUS status; /**< status of the channel */
} chan[RC_CHANNELS_FUNCTION_MAX];
uint8_t chan_count; /**< maximum number of valid channels */
@@ -98,6 +85,7 @@ struct rc_channels_s {
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
uint8_t function[RC_CHANNELS_FUNCTION_MAX];
uint8_t rssi; /**< Overall receive signal strength */
bool is_valid; /**< Inputs are valid, no timeout */
}; /**< radio control channels. */
/**
nuttx/configs/px4fmu/nsh/defconfig
-16
View File
@@ -306,25 +306,9 @@ CONFIG_USART6_RXDMA=y
# set HRT_PPM_CHANNEL to the timer capture/compare channel to be
# used, and define GPIO_PPM_IN to configure the appropriate timer
# GPIO.
# CONFIG_TONE_ALARM
# Enables the tone alarm (buzzer) driver The board definition must
# set TONE_ALARM_TIMER and TONE_ALARM_CHANNEL to the timer and
# capture/compare channels to be used.
# CONFIG_PWM_SERVO
# Enables the PWM servo driver. The driver configuration must be
# supplied by the board support at initialisation time.
# Note that USART2 must be disabled on the PX4 board for this to
# be available.
# CONFIG_MULTIPORT
# Enabled support for run-time (or EEPROM based boot-time) configuration
# of ports for different functions (e.g. USART2 or ARDrone or PWM out)
#
#
CONFIG_HRT_TIMER=y
CONFIG_HRT_PPM=y
CONFIG_TONE_ALARM=y
CONFIG_PWM_SERVO=n
CONFIG_MULTIPORT=n
#
# STM32F40xxx specific SPI device driver settings
nuttx/configs/px4io/include/board.h
+3 -21
View File
@@ -47,9 +47,9 @@
# include <stdint.h>
# include <stdbool.h>
#endif
#include "stm32_rcc.h"
#include "stm32_sdio.h"
#include "stm32_internal.h"
#include <stm32_rcc.h>
#include <stm32_sdio.h>
#include <stm32_internal.h>
/************************************************************************************
* Definitions
@@ -116,12 +116,6 @@
# define GPIO_PPM_IN GPIO_TIM1_CH1IN
#endif
/*
* PWM
*
* PWM configuration is provided via the configuration structure in up_boardinitialize.c
*/
/************************************************************************************
* Public Data
************************************************************************************/
@@ -151,17 +145,5 @@ extern "C" {
EXTERN void stm32_boardinitialize(void);
/************************************************************************************
* Power switch support.
*/
extern void up_power_init(void);
extern void up_power_set(int port, bool state);
extern bool up_power_error(int port);
#undef EXTERN
#if defined(__cplusplus)
}
#endif
#endif /* __ASSEMBLY__ */
#endif /* __ARCH_BOARD_BOARD_H */
nuttx/configs/px4io/include/drv_gpio.h
-67
View File
@@ -1,67 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 GPIO driver for PX4IO
*/
#include <sys/ioctl.h>
#define _GPIO_IOCTL_BASE 0x7700
#define GPIO_SET(_x) _IOC(_GPIO_IOCTL_BASE, _x)
#define GPIO_GET(_x) _IOC(_GPIO_IOCTL_BASE + 1, _x)
/*
* List of GPIOs; must be sorted with settable GPIOs first.
*/
#define GPIO_ACC1_POWER 0 /* settable */
#define GPIO_ACC2_POWER 1
#define GPIO_SERVO_POWER 2
#define GPIO_RELAY1 3
#define GPIO_RELAY2 4
#define GPIO_LED_BLUE 5
#define GPIO_LED_AMBER 6
#define GPIO_LED_SAFETY 7
#define GPIO_ACC_OVERCURRENT 8 /* readonly */
#define GPIO_SERVO_OVERCURRENT 9
#define GPIO_SAFETY_BUTTON 10
#define GPIO_MAX_SETTABLE 7
#define GPIO_MAX 10
/*
* GPIO driver init function.
*/
extern int gpio_drv_init(void);
nuttx/configs/px4io/include/drv_ppm_input.h
-100
View File
@@ -1,100 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 PPM input decoder.
*
* Works in conjunction with the HRT driver, exports a device node
* and a message queue (if message queues are enabled).
*
* Note that the device node supports both blocking and non-blocking
* opens, but actually never blocks. A nonblocking open will return
* EWOULDBLOCK if there has not been an update since the last read,
* while a blocking open will always return the most recent data.
*/
#include <sys/ioctl.h>
#define _PPM_INPUT_BASE 0x7600
/*
* Fetch the state of the PPM input detector.
*/
#define PPM_INPUT_STATUS _IOC(_PPM_INPUT_BASE, 0)
typedef enum {
PPM_STATUS_NO_SIGNAL = 0,
PPM_STATUS_SIGNAL_CURRENT = 1,
} ppm_input_status_t;
/*
* Fetch the number of channels decoded (only valid when PPM_STATUS_SIGNAL_CURRENT).
*/
#define PPM_INPUT_CHANNELS _IOC(_PPM_INPUT_BASE, 1)
typedef int ppm_input_channel_count_t;
/*
* Device node
*/
#define PPM_DEVICE_NODE "/dev/ppm_input"
/*
* Message queue; if message queues are supported, PPM input data is
* supplied to the queue when a frame is decoded.
*/
#ifndef CONFIG_DISABLE_MQUEUE
# define PPM_MESSAGE_QUEUE "ppm_input"
#endif
/*
* Private hook from the HRT driver to the PPM decoder.
*
* This function is called for every edge of the incoming PPM
* signal.
*
* @param reset If true, the decoder should be reset (e.g.)
* capture failure was detected.
* @param count The counter value at which the edge
* was captured.
*/
void ppm_input_decode(bool reset, uint16_t count);
/*
* PPM input initialisation function.
*
* If message queues are enabled, and mq_name is not NULL, received input
* is posted to the message queue as an array of 16-bit unsigned channel values.
*/
int ppm_input_init(const char *mq_name);
nuttx/configs/px4io/include/drv_pwm_servo.h
-94
View File
@@ -1,94 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 PWM servo driver.
*
* The pwm_servo driver supports servos connected to STM32 timer
* blocks.
*
* Servo values can be set either with the PWM_SERVO_SET ioctl, or
* by writing an array of servo_position_t values to the device.
* Writing a value of 0 to a channel suppresses any output for that
* channel.
*
* Servo values can be read back either with the PWM_SERVO_GET
* ioctl, or by reading an array of servo_position_t values
* from the device.
*
* Attempts to set a channel that is not configured are ignored,
* and unconfigured channels always read zero.
*
* The PWM_SERVO_ARM / PWM_SERVO_DISARM calls globally arm
* (enable) and disarm (disable) all servo outputs.
*/
#include <sys/ioctl.h>
#define _PWM_SERVO_BASE 0x7500
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
typedef uint16_t servo_position_t;
/* configuration limits */
#define PWM_SERVO_MAX_TIMERS 3
#define PWM_SERVO_MAX_CHANNELS 8
struct pwm_servo_config {
/* rate (in Hz) of PWM updates */
uint32_t update_rate;
/* array of timers dedicated to PWM servo use */
struct pwm_servo_timer {
uint32_t base;
uint32_t clock_register;
uint32_t clock_bit;
uint32_t clock_freq;
} timers[PWM_SERVO_MAX_TIMERS];
/* array of channels in logical order */
struct pwm_servo_channel {
uint32_t gpio;
uint8_t timer_index;
uint8_t timer_channel;
servo_position_t default_value;
} channels[PWM_SERVO_MAX_CHANNELS];
};
extern int pwm_servo_init(const struct pwm_servo_config *config);
nuttx/configs/px4io/include/up_boardinitialize.h
-43
View File
@@ -1,43 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Board initialisation prototype(s)
*/
#ifndef __UP_BOARDINITIALIZE_H
#define __UP_BOARDINITIALIZE_H
extern int up_boardinitialize(void);
#endif
nuttx/configs/px4io/include/up_hrt.h
-129
View File
@@ -1,129 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 High-resolution timer callouts and timekeeping.
*/
#ifndef UP_HRT_H_
#define UP_HRT_H_
#include <sys/types.h>
#include <stdbool.h>
#include <time.h>
#include <queue.h>
/*
* Absolute time, in microsecond units.
*
* Absolute time is measured from some arbitrary epoch shortly after
* system startup. It should never wrap or go backwards.
*/
typedef uint64_t hrt_abstime;
/*
* Callout function type.
*
* Note that callouts run in the timer interrupt context, so
* they are serialised with respect to each other, and must not
* block.
*/
typedef void (* hrt_callout)(void *arg);
/*
* Callout record.
*/
struct hrt_call {
struct sq_entry_s link;
hrt_abstime deadline;
hrt_abstime period;
hrt_callout callout;
void *arg;
};
/*
* Get absolute time.
*/
extern hrt_abstime hrt_absolute_time(void);
/*
* Convert a timespec to absolute time.
*/
extern hrt_abstime ts_to_abstime(struct timespec *ts);
/*
* Convert absolute time to a timespec.
*/
extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
* with hrt_called().
*/
extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
/*
* Call callout(arg) at absolute time calltime.
*/
extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
/*
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
* jitter but should not drift.
*/
extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
/*
* If this returns true, the entry has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
*/
extern bool hrt_called(struct hrt_call *entry);
/*
* Remove the entry from the callout list.
*/
extern void hrt_cancel(struct hrt_call *entry);
/*
* Initialise the HRT.
*/
extern void hrt_init(int timer);
#endif /* UP_HRT_H_ */
nuttx/configs/px4io/io/appconfig
+3
View File
@@ -31,4 +31,7 @@
#
############################################################################
CONFIGURED_APPS += drivers/boards/px4io
CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += px4io
nuttx/configs/px4io/io/defconfig
+8 -7
View File
@@ -84,7 +84,7 @@ CONFIG_ARCH_INTERRUPTSTACK=n
CONFIG_ARCH_STACKDUMP=y
CONFIG_ARCH_BOOTLOADER=n
CONFIG_ARCH_LEDS=n
CONFIG_ARCH_BUTTONS=y
CONFIG_ARCH_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=n
CONFIG_ARMV7M_CMNVECTOR=y
@@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n
# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
# CONFIG_USARTn_2STOP - Two stop bits
#
CONFIG_SERIAL_TERMIOS=y
CONFIG_USART1_SERIAL_CONSOLE=y
CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n
@@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32
CONFIG_USART2_TXBUFSIZE=32
CONFIG_USART3_TXBUFSIZE=32
CONFIG_USART1_RXBUFSIZE=32
CONFIG_USART2_RXBUFSIZE=32
CONFIG_USART1_RXBUFSIZE=64
CONFIG_USART2_RXBUFSIZE=128
CONFIG_USART3_RXBUFSIZE=32
CONFIG_USART1_BAUD=57600
CONFIG_USART1_BAUD=115200
CONFIG_USART2_BAUD=115200
CONFIG_USART3_BAUD=115200
@@ -204,7 +206,6 @@ CONFIG_USART3_2STOP=0
#
CONFIG_HRT_TIMER=y
CONFIG_HRT_PPM=y
CONFIG_PWM_SERVO=y
#
# General build options
@@ -389,10 +390,10 @@ CONFIG_DISABLE_CLOCK=n
CONFIG_DISABLE_POSIX_TIMERS=y
CONFIG_DISABLE_PTHREAD=y
CONFIG_DISABLE_SIGNALS=y
CONFIG_DISABLE_MQUEUE=n
CONFIG_DISABLE_MQUEUE=y
CONFIG_DISABLE_MOUNTPOINT=y
CONFIG_DISABLE_ENVIRON=y
CONFIG_DISABLE_POLL=y
CONFIG_DISABLE_POLL=n
#
# Misc libc settings
nuttx/configs/px4io/src/Makefile
+1 -11
View File
@@ -40,17 +40,7 @@ CFLAGS += -I$(TOPDIR)/sched
ASRCS =
AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_boot.c up_hrt.c\
drv_pwm_servo.c drv_ppm_input.c drv_gpio.c \
up_boardinitialize.c
ifeq ($(CONFIG_NSH_ARCHINIT),y)
CSRCS += up_nsh.c
endif
ifeq ($(CONFIG_ADC),y)
CSRCS += up_adc.c
endif
CSRCS = empty.c
COBJS = $(CSRCS:.c=$(OBJEXT))

Some files were not shown because too many files have changed in this diff Show More