Compare commits

..

9 Commits

Author SHA1 Message Date
Matthias Grob 437148f8f3 Makefile: target to test clang-format on the entire codebase 2022-06-30 16:07:37 +02:00
Matthias Grob 94f8a79c2c files_to_check_code_style: correctly omit folders 2022-06-30 16:07:02 +02:00
Matthias Grob d146405753 Add .clang-format for google style with minimal adaptations 2022-06-30 10:34:53 +02:00
Martina Rivizzigno 55563eba49 MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME 2022-06-24 19:44:43 +02:00
Matthias Grob c8fb7a6990 fw/uuv control: remove duplicated comments, restyle initializers 2022-06-24 10:05:16 -05:00
Matthias Grob 78225f7b1f examples/fixedwing_control: use initializers instead of memset 2022-06-24 10:05:16 -05:00
Matthias Grob cfd4e64b02 uuv_pos_control: remove practically unused manual control subscription 2022-06-24 10:05:16 -05:00
Matthias Grob 3a239ff649 examples: remove empty fake_gyro 2022-06-24 10:05:16 -05:00
CUAVhonglang cfa8b451c7 cuav-nora: changed brick to compatible with cuav noraplus 2022-06-24 10:03:43 -05:00
28 changed files with 108 additions and 546 deletions
+13
View File
@@ -0,0 +1,13 @@
# Google C++ style as base
Language: Cpp
BasedOnStyle: Google
# Indentation using tabs
UseTab: ForContinuationAndIndentation
TabWidth: 8
IndentWidth: 8
ContinuationIndentWidth: 8
ConstructorInitializerIndentWidth: 8
AccessModifierOffset: -8
ColumnLimit: 120 # Allow more collumns
+4
View File
@@ -375,6 +375,10 @@ format:
$(call colorecho,'Formatting with astyle')
@"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix
format_clang:
$(call colorecho,'Formatting with clang-format')
@"$(SRC_DIR)"/Tools/astyle/files_to_check_code_style.sh | xargs clang-format -i -style=file
# Testing
# --------------------------------------------------------------------
.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance
@@ -62,7 +62,7 @@ param set-default MPC_JERK_AUTO 4
param set-default MPC_LAND_SPEED 1
param set-default MPC_MAN_TILT_MAX 25
param set-default MPC_MAN_Y_MAX 40
param set-default MPC_SPOOLUP_TIME 1.5
param set-default COM_SPOOLUP_TIME 1.5
param set-default MPC_THR_HOVER 0.45
param set-default MPC_TILTMAX_AIR 25
param set-default MPC_TKO_RAMP_T 1.8
+20 -19
View File
@@ -8,23 +8,24 @@ if [ $# -gt 0 ]; then
fi
exec find boards msg src platforms test \
-path msg/templates/urtps -prune -o \
-path platforms/nuttx/NuttX -prune -o \
-path platforms/qurt/dspal -prune -o \
-path src/drivers/uavcan/libuavcan -prune -o \
-path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
-path src/drivers/cyphal/libcanard -prune -o \
-path src/lib/crypto/monocypher -prune -o \
-path src/lib/events/libevents -prune -o \
-path src/lib/parameters/uthash -prune -o \
-path src/modules/ekf2/EKF -prune -o \
-path src/modules/gyro_fft/CMSIS_5 -prune -o \
-path src/modules/mavlink/mavlink -prune -o \
-path src/modules/micrortps_bridge/micro-CDR -prune -o \
-path src/modules/micrortps_bridge/microRTPS_client -prune -o \
-path test/mavsdk_tests/catch2 -prune -o \
-path src/lib/crypto/monocypher -prune -o \
-path src/lib/crypto/libtomcrypt -prune -o \
-path src/lib/crypto/libtommath -prune -o \
-path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
-not -path "msg/templates/urtps/*" \
-not -path "platforms/nuttx/NuttX/*" \
-not -path "platforms/qurt/dspal/*" \
-not -path "src/drivers/gps/devices/*" \
-not -path "src/drivers/uavcan/libuavcan/*" \
-not -path "src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/*" \
-not -path "src/drivers/cyphal/libcanard/*" \
-not -path "src/lib/crypto/monocypher/*" \
-not -path "src/lib/events/libevents/*" \
-not -path "src/lib/parameters/uthash/*" \
-not -path "src/modules/ekf2/EKF/*" \
-not -path "src/modules/gyro_fft/CMSIS_5/*" \
-not -path "src/modules/mavlink/mavlink/*" \
-not -path "src/modules/micrortps_bridge/micro-CDR/*" \
-not -path "src/modules/micrortps_bridge/microRTPS_client/*" \
-not -path "test/mavsdk_tests/catch2/*" \
-not -path "src/lib/crypto/monocypher/*" \
-not -path "src/lib/crypto/libtomcrypt/*" \
-not -path "src/lib/crypto/libtommath/*" \
-not -path "src/modules/microdds_client/Micro-XRCE-DDS-Client/*" \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
+2 -2
View File
@@ -118,8 +118,8 @@
#define GPIO_nPOWER_IN_CAN /* PG2 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
#define GPIO_nPOWER_IN_C /* PG0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)
#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen */
#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen */
#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB is Chosen */
#define GPIO_VDD_5V_HIPOWER_EN /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
-1
View File
@@ -43,7 +43,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_DRIVERS_LIGHTS_RGBLED_LP55231=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
-1
View File
@@ -24,5 +24,4 @@ else
fi
rgbled_pwm start
rgbled_lp55231 -X start
safety_button start
-1
View File
@@ -206,7 +206,6 @@
#define DRV_TRNS_DEVTYPE_MXS 0xB0
#define DRV_HYGRO_DEVTYPE_SHT3X 0xB1
#define DRV_LED_DEVTYPE_RGBLED_LP55231 0xB2
#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2
#define DRV_FLOW_DEVTYPE_PMW3901 0xB3
-2
View File
@@ -34,6 +34,4 @@
#add_subdirectory(neopixel) # requires board support (BOARD_HAS_N_S_RGB_LED)
add_subdirectory(rgbled)
add_subdirectory(rgbled_ncp5623c)
add_subdirectory(rgbled_lp55231)
#add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__rgbled_lp55231
MAIN rgbled_lp55231
SRCS
rgbled_lp55231.cpp
DEPENDS
drivers__device
led
)
@@ -1,5 +0,0 @@
menuconfig DRIVERS_LIGHTS_RGBLED_LP55231
bool "rgbled_lp55231"
default n
---help---
Enable support for rgbled_lp55231
@@ -1,385 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
/**
*
* Driver for RGB LP55231 LED controller connected via I2C.
*/
#include <drivers/device/i2c.h>
#include <lib/led/led.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
using namespace time_literals;
static constexpr uint8_t ADDRESS = 0x32;
// register stuff
static constexpr uint8_t REG_CNTRL1 = 0x00;
static constexpr uint8_t REG_CNTRL2 = 0x01;
static constexpr uint8_t REG_RATIO_MSB = 0x02;
static constexpr uint8_t REG_RATIO_LSB = 0x03;
static constexpr uint8_t REG_OUTPUT_ONOFF_MSB = 0x04;
static constexpr uint8_t REG_OUTPUT_ONOFF_LSB = 0x05;
// Per LED control channels - fader channel assig, log dimming enable, temperature compensation
static constexpr uint8_t REG_D1_CTRL = 0x06;
static constexpr uint8_t REG_D2_CTRL = 0x07;
static constexpr uint8_t REG_D3_CTRL = 0x08;
static constexpr uint8_t REG_D4_CTRL = 0x09;
static constexpr uint8_t REG_D5_CTRL = 0x0a;
static constexpr uint8_t REG_D6_CTRL = 0x0b;
static constexpr uint8_t REG_D7_CTRL = 0x0c;
static constexpr uint8_t REG_D8_CTRL = 0x0d;
static constexpr uint8_t REG_D9_CTRL = 0x0e;
// 0x0f to 0x15 reserved
// Direct PWM control registers
static constexpr uint8_t REG_D1_PWM = 0x16;
static constexpr uint8_t REG_D2_PWM = 0x17;
static constexpr uint8_t REG_D3_PWM = 0x18;
static constexpr uint8_t REG_D4_PWM = 0x19;
static constexpr uint8_t REG_D5_PWM = 0x1a;
static constexpr uint8_t REG_D6_PWM = 0x1b;
static constexpr uint8_t REG_D7_PWM = 0x1c;
static constexpr uint8_t REG_D8_PWM = 0x1d;
static constexpr uint8_t REG_D9_PWM = 0x1e;
// 0x1f to 0x25 reserved
// Drive current registers
static constexpr uint8_t REG_D1_I_CTL = 0x26;
static constexpr uint8_t REG_D2_I_CTL = 0x27;
static constexpr uint8_t REG_D3_I_CTL = 0x28;
static constexpr uint8_t REG_D4_I_CTL = 0x29;
static constexpr uint8_t REG_D5_I_CTL = 0x2a;
static constexpr uint8_t REG_D6_I_CTL = 0x2b;
static constexpr uint8_t REG_D7_I_CTL = 0x2c;
static constexpr uint8_t REG_D8_I_CTL = 0x2d;
static constexpr uint8_t REG_D9_I_CTL = 0x2e;
// 0x2f to 0x35 reserved
static constexpr uint8_t REG_MISC = 0x36;
static constexpr uint8_t REG_PC1 = 0x37;
static constexpr uint8_t REG_PC2 = 0x38;
static constexpr uint8_t REG_PC3 = 0x39;
static constexpr uint8_t REG_STATUS_IRQ = 0x3A;
static constexpr uint8_t REG_INT_GPIO = 0x3B;
static constexpr uint8_t REG_GLOBAL_VAR = 0x3C;
static constexpr uint8_t REG_RESET = 0x3D;
static constexpr uint8_t REG_TEMP_CTL = 0x3E;
static constexpr uint8_t REG_TEMP_READ = 0x3F;
static constexpr uint8_t REG_TEMP_WRITE = 0x40;
static constexpr uint8_t REG_TEST_CTL = 0x41;
static constexpr uint8_t REG_TEST_ADC = 0x42;
// 0x43 to 0x44 reserved
static constexpr uint8_t REG_ENGINE_A_VAR = 0x45;
static constexpr uint8_t REG_ENGINE_B_VAR = 0x46;
static constexpr uint8_t REG_ENGINE_C_VAR = 0x47;
static constexpr uint8_t REG_MASTER_FADE_1 = 0x48;
static constexpr uint8_t REG_MASTER_FADE_2 = 0x49;
static constexpr uint8_t REG_MASTER_FADE_3 = 0x4A;
// 0x4b Reserved
static constexpr uint8_t REG_PROG1_START = 0x4C;
static constexpr uint8_t REG_PROG2_START = 0x4D;
static constexpr uint8_t REG_PROG3_START = 0x4E;
static constexpr uint8_t REG_PROG_PAGE_SEL = 0x4f;
// Memory is more confusing - there are 6 pages, sel by addr 4f
static constexpr uint8_t REG_PROG_MEM_BASE = 0x50;
static constexpr uint8_t REG_PROG_MEM_END = 0x6f;
static constexpr uint8_t REG_ENG1_MAP_MSB = 0x70;
static constexpr uint8_t REG_ENG1_MAP_LSB = 0x71;
static constexpr uint8_t REG_ENG2_MAP_MSB = 0x72;
static constexpr uint8_t REG_ENG2_MAP_LSB = 0x73;
static constexpr uint8_t REG_ENG3_MAP_MSB = 0x74;
static constexpr uint8_t REG_ENG3_MAP_LSB = 0x75;
static constexpr uint8_t REG_GAIN_CHANGE = 0x76;
// Colors on eval board
static constexpr uint8_t CHANNEL_L0_RED = 0;
static constexpr uint8_t CHANNEL_L0_GREEN = 3;
static constexpr uint8_t CHANNEL_L0_BLUE = 4;
static constexpr uint8_t CHANNEL_L1_RED = 1;
static constexpr uint8_t CHANNEL_L1_GREEN = 5;
static constexpr uint8_t CHANNEL_L1_BLUE = 6;
static constexpr uint8_t CHANNEL_L2_RED = 2;
static constexpr uint8_t CHANNEL_L2_GREEN = 7;
static constexpr uint8_t CHANNEL_L2_BLUE = 8;
class RGBLED_LP55231 : public device::I2C, public I2CSPIDriver<RGBLED_LP55231>
{
public:
RGBLED_LP55231(const I2CSPIDriverConfig &config);
virtual ~RGBLED_LP55231() override;
static void print_usage();
int init() override;
int probe() override;
void RunImpl();
private:
void print_status() override;
int write(uint8_t address, uint8_t value);
int read(uint8_t address, uint8_t &value);
int setChannelPWM(uint8_t channel, uint8_t value);
int setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
const LedControlDataSingle &led);
int reset();
int enable();
LedController _led_controller;
// uint8_t _channel = 0;
};
RGBLED_LP55231::RGBLED_LP55231(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config)
{
}
RGBLED_LP55231::~RGBLED_LP55231()
{
reset();
}
int RGBLED_LP55231::write(uint8_t address, uint8_t value)
{
uint8_t data[2] = {address, value};
return transfer(data, sizeof(data), nullptr, 0);
}
int RGBLED_LP55231::read(uint8_t address, uint8_t &value)
{
return transfer(&address, 1, (uint8_t *)&value, 1);
}
int RGBLED_LP55231::enable()
{
int ret = write(REG_CNTRL1, 0x40);
// enable charge pump, internal oscillator, auto increment
ret = write(REG_MISC, 0x53);
return ret;
}
int RGBLED_LP55231::reset()
{
return write(REG_RESET, 0xFF);
}
int RGBLED_LP55231::init()
{
int ret = I2C::init();
if (ret != OK) {
return ret;
}
ret = reset();
ret = enable();
// kick off work queue
ScheduleNow();
return OK;
}
int RGBLED_LP55231::setChannelPWM(uint8_t channel, uint8_t value)
{
assert(channel < 9);
return write(REG_D1_PWM + channel, value);
}
int RGBLED_LP55231::setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
const LedControlDataSingle &led)
{
uint8_t r{0}, g{0}, b{0};
uint8_t brightness = led.brightness;
switch (led.color) {
case led_control_s::COLOR_RED:
r = brightness; g = 0; b = 0;
break;
case led_control_s::COLOR_GREEN:
r = 0; g = brightness; b = 0;
break;
case led_control_s::COLOR_BLUE:
r = 0; g = 0; b = brightness;
break;
case led_control_s::COLOR_AMBER: //make it the same as yellow
case led_control_s::COLOR_YELLOW:
r = brightness / 2; g = brightness / 2; b = 0;
break;
case led_control_s::COLOR_PURPLE:
r = brightness / 2; g = 0; b = brightness / 2;
break;
case led_control_s::COLOR_CYAN:
r = 0; g = brightness / 2; b = brightness / 2;
break;
case led_control_s::COLOR_WHITE:
r = brightness / 3; g = brightness / 3; b = brightness / 3;
break;
default: // led_control_s::COLOR_OFF
r = 0; g = 0; b = 0;
break;
}
int ret = 0;
ret = setChannelPWM(channel_red, r);
ret = setChannelPWM(channel_green, g);
ret = setChannelPWM(channel_blue, b);
return ret;
}
int RGBLED_LP55231::probe()
{
// try read output on / off (should be 0xff)
uint8_t on_off;
int ret = read(REG_OUTPUT_ONOFF_LSB, on_off);
if (on_off != 0xFF) {
return -1;
}
// try read current control (should be 0xAF)
uint8_t current_control;
ret = read(REG_D1_I_CTL, current_control);
if (current_control != 0xAF) {
return -1;
}
_retries = 1;
return ret;
}
void
RGBLED_LP55231::print_status()
{
PX4_INFO("No status implemented");
}
void RGBLED_LP55231::RunImpl()
{
LedControlData led_control_data;
if (_led_controller.update(led_control_data) == 1) {
setLed(CHANNEL_L0_RED, CHANNEL_L0_GREEN, CHANNEL_L0_BLUE, led_control_data.leds[0]);
setLed(CHANNEL_L1_RED, CHANNEL_L1_GREEN, CHANNEL_L1_BLUE, led_control_data.leds[1]);
setLed(CHANNEL_L2_RED, CHANNEL_L2_GREEN, CHANNEL_L2_BLUE, led_control_data.leds[2]);
}
/* re-queue ourselves to run again later */
ScheduleDelayed(_led_controller.maximum_update_interval());
// setChannelPWM(_channel, 0);
// _channel++;
// _channel = _channel % 9;
// setChannelPWM(_channel, 255);
// ScheduleDelayed(500_ms);
}
void
RGBLED_LP55231::print_usage()
{
PRINT_MODULE_USAGE_NAME("rgbled_lp55231", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x32);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" __EXPORT int rgbled_lp55231_main(int argc, char *argv[])
{
using ThisDriver = RGBLED_LP55231;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
cli.i2c_address = ADDRESS;
const char *verb = cli.parseDefaultArguments(argc, argv);
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP55231);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}
-5
View File
@@ -1,5 +0,0 @@
menuconfig EXAMPLES_FAKE_GYRO
bool "fake_gyro"
default n
---help---
Enable support for fake_gyro
+6 -17
View File
@@ -278,25 +278,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_control_setpoint;
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
vehicle_attitude_s att{};
vehicle_rates_setpoint_s rates_sp{};
manual_control_setpoint_s manual_control_setpoint{};
vehicle_status_s vstatus{};
position_setpoint_s global_sp{};
/* output structs - this is what is sent to the mixer */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
actuator_controls_s actuators{};
/* publish actuator controls with zero values */
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
+9
View File
@@ -42,6 +42,15 @@
bool param_modify_on_import(bson_node_t node)
{
// migrate MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME (2020-12-03). This can be removed after the next release (current release=1.11)
if (node->type == BSON_DOUBLE) {
if (strcmp("MPC_SPOOLUP_TIME", node->name) == 0) {
strcpy(node->name, "COM_SPOOLUP_TIME");
PX4_INFO("param migrating MPC_SPOOLUP_TIME (removed) -> COM_SPOOLUP_TIME: value=%.3f", node->d);
return true;
}
}
// migrate COM_ARM_AUTH -> COM_ARM_AUTH_ID, COM_ARM_AUTH_MET and COM_ARM_AUTH_TO (2020-11-06). This can be removed after the next release (current release=1.11)
if (node->type == BSON_INT32) {
if (strcmp("COM_ARM_AUTH", node->name) == 0) {
+2 -2
View File
@@ -2721,8 +2721,8 @@ Commander::run()
if (_arm_state_machine.isArmed()) {
if (fd_status_flags.arm_escs) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&_vehicle_status.armed_time) < 500_ms) {
// Checks have to pass within the spool up time
if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) {
disarm(arm_disarm_reason_t::failure_detector);
mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t");
events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request");
+3 -1
View File
@@ -258,7 +258,9 @@ private:
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
)
// optional parameters
+16
View File
@@ -1023,6 +1023,22 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
*/
PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
/**
* Enforced delay between arming and further navigation
*
* The minimal time from arming the motors until moving the vehicle is possible is COM_SPOOLUP_TIME seconds.
* Goal:
* - Motors and propellers spool up to idle speed before getting commanded to spin faster
* - Timeout for ESCs and smart batteries to successfulyy do failure checks
* e.g. for stuck rotors before the vehicle is off the ground
*
* @group Commander
* @min 0
* @max 5
* @unit s
*/
PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
/**
* Wind speed warning threshold
*
@@ -269,7 +269,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
bool is_esc_failure = !is_all_escs_armed;
for (int i = 0; i < limited_esc_count; i++) {
is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
@@ -131,15 +131,15 @@ private:
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
actuator_controls_s _actuator_controls {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
vehicle_local_position_s _local_pos {}; /**< local position */
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_local_position_s _local_pos{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf;
hrt_abstime _last_run{0};
@@ -185,12 +185,12 @@ private:
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
vehicle_control_mode_s _control_mode {};
vehicle_local_position_s _local_pos {}; // vehicle local position
vehicle_status_s _vehicle_status {}; // vehicle status
manual_control_setpoint_s _manual_control_setpoint{};
position_setpoint_triplet_s _pos_sp_triplet{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _control_mode{};
vehicle_local_position_s _local_pos{};
vehicle_status_s _vehicle_status{};
double _current_latitude{0};
double _current_longitude{0};
@@ -212,8 +212,8 @@ private:
float _min_current_sp_distance_xy{FLT_MAX};
position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started
position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies
// [us] Last absolute time position control has been called
hrt_abstime _last_time_position_control_called{0};
@@ -234,7 +234,7 @@ void MulticopterPositionControl::parameters_update(bool force)
_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
_takeoff.setSpoolupTime(_param_com_spoolup_time.get());
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
}
@@ -147,7 +147,7 @@ private:
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
// Takeoff / Land
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
@@ -94,7 +94,7 @@ public:
private:
TakeoffState _takeoff_state = TakeoffState::disarmed;
systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed
systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true COM_SPOOLUP_TIME seconds after the vehicle was armed
float _takeoff_ramp_time{0.f};
float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust
@@ -780,22 +780,6 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f);
*/
PARAM_DEFINE_INT32(MPC_POS_MODE, 4);
/**
* Enforced delay between arming and takeoff
*
* For altitude controlled modes the time from arming the motors until
* a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds
* to ensure the motors and propellers can sppol up and reach idle speed before
* getting commanded to spin faster. This delay is particularly useful for vehicles
* with slow motor spin-up e.g. because of large propellers.
*
* @min 0
* @max 10
* @unit s
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
/**
* Yaw mode.
*
@@ -117,13 +117,13 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _attitude_setpoint {}; /**< vehicle attitude setpoint */
vehicle_rates_setpoint_s _rates_setpoint {}; /**< vehicle bodyrates setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
actuator_controls_s _actuators{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _attitude_setpoint{};
vehicle_rates_setpoint_s _rates_setpoint{};
vehicle_control_mode_s _vcontrol_mode{};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
@@ -195,16 +195,6 @@ void UUVPOSControl::Run()
}
}
/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */
}
}
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_manual_enabled ||
_vcontrol_mode.flag_control_attitude_enabled) {
@@ -60,7 +60,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -103,20 +102,17 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
//actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */
trajectory_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
vehicle_attitude_s _vehicle_attitude{};
trajectory_setpoint_s _trajectory_setpoint{};
vehicle_control_mode_s _vcontrol_mode{};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::UUV_GAIN_X_P>) _param_pose_gain_x,