mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 16:46:09 +08:00
stop ignoring Wmissing-field-initializers and fix (#9912)
This commit is contained in:
@@ -404,7 +404,6 @@ function(px4_add_common_flags)
|
|||||||
)
|
)
|
||||||
|
|
||||||
set(cxx_warnings
|
set(cxx_warnings
|
||||||
-Wno-missing-field-initializers
|
|
||||||
-Wno-overloaded-virtual # TODO: fix and remove
|
-Wno-overloaded-virtual # TODO: fix and remove
|
||||||
-Wreorder
|
-Wreorder
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -223,6 +223,9 @@ function(px4_os_add_flags)
|
|||||||
list(APPEND added_c_flags --sysroot=${HEXAGON_ARM_SYSROOT})
|
list(APPEND added_c_flags --sysroot=${HEXAGON_ARM_SYSROOT})
|
||||||
list(APPEND added_cxx_flags --sysroot=${HEXAGON_ARM_SYSROOT})
|
list(APPEND added_cxx_flags --sysroot=${HEXAGON_ARM_SYSROOT})
|
||||||
|
|
||||||
|
# TODO: Wmissing-field-initializers ignored on older toolchain, can be removed eventually
|
||||||
|
list(APPEND added_cxx_flags -Wno-missing-field-initializers)
|
||||||
|
|
||||||
list(APPEND added_exe_linker_flags
|
list(APPEND added_exe_linker_flags
|
||||||
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib
|
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib
|
||||||
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib
|
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib
|
||||||
@@ -254,6 +257,9 @@ function(px4_os_add_flags)
|
|||||||
list(APPEND added_c_flags ${RPI_COMPILE_FLAGS})
|
list(APPEND added_c_flags ${RPI_COMPILE_FLAGS})
|
||||||
list(APPEND added_cxx_flags ${RPI_COMPILE_FLAGS})
|
list(APPEND added_cxx_flags ${RPI_COMPILE_FLAGS})
|
||||||
|
|
||||||
|
# TODO: Wmissing-field-initializers ignored on older toolchain, can be removed eventually
|
||||||
|
list(APPEND added_cxx_flags -Wno-missing-field-initializers)
|
||||||
|
|
||||||
find_program(CXX_COMPILER_PATH ${CMAKE_CXX_COMPILER})
|
find_program(CXX_COMPILER_PATH ${CMAKE_CXX_COMPILER})
|
||||||
|
|
||||||
GET_FILENAME_COMPONENT(CXX_COMPILER_PATH ${CXX_COMPILER_PATH} DIRECTORY)
|
GET_FILENAME_COMPONENT(CXX_COMPILER_PATH ${CXX_COMPILER_PATH} DIRECTORY)
|
||||||
@@ -274,6 +280,9 @@ function(px4_os_add_flags)
|
|||||||
-L${CXX_COMPILER_PATH}/arm-linux-gnueabihf/libc/usr/lib
|
-L${CXX_COMPILER_PATH}/arm-linux-gnueabihf/libc/usr/lib
|
||||||
)
|
)
|
||||||
ENDIF()
|
ENDIF()
|
||||||
|
elseif ("${BOARD}" STREQUAL "bebop")
|
||||||
|
# TODO: Wmissing-field-initializers ignored on older toolchain, can be removed eventually
|
||||||
|
list(APPEND added_cxx_flags -Wno-missing-field-initializers)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
# output
|
# output
|
||||||
|
|||||||
@@ -211,20 +211,20 @@ protected:
|
|||||||
Device *_interface;
|
Device *_interface;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
work_s _work;
|
work_s _work{};
|
||||||
unsigned _measure_ticks;
|
unsigned _measure_ticks{0};
|
||||||
|
|
||||||
ringbuffer::RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports{nullptr};
|
||||||
bool _collect_phase;
|
bool _collect_phase{false};
|
||||||
|
|
||||||
orb_advert_t _baro_topic;
|
orb_advert_t _baro_topic{nullptr};
|
||||||
int _orb_class_instance;
|
int _orb_class_instance{-1};
|
||||||
int _class_instance;
|
int _class_instance{-1};
|
||||||
|
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
perf_counter_t _comms_errors;
|
perf_counter_t _comms_errors;
|
||||||
|
|
||||||
struct baro_report _last_report; /**< used for info() */
|
sensor_baro_s _last_report{}; /**< used for info() */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initialise the automatic measurement state machine and start it.
|
* Initialise the automatic measurement state machine and start it.
|
||||||
@@ -311,28 +311,14 @@ extern "C" __EXPORT int lps25h_main(int argc, char *argv[]);
|
|||||||
LPS25H::LPS25H(device::Device *interface, const char *path) :
|
LPS25H::LPS25H(device::Device *interface, const char *path) :
|
||||||
CDev("LPS25H", path),
|
CDev("LPS25H", path),
|
||||||
_interface(interface),
|
_interface(interface),
|
||||||
_work{},
|
|
||||||
_measure_ticks(0),
|
|
||||||
_reports(nullptr),
|
|
||||||
_collect_phase(false),
|
|
||||||
_baro_topic(nullptr),
|
|
||||||
_orb_class_instance(-1),
|
|
||||||
_class_instance(-1),
|
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors")),
|
_comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors"))
|
||||||
_last_report{0}
|
|
||||||
{
|
{
|
||||||
// set the device type from the interface
|
// set the device type from the interface
|
||||||
_device_id.devid_s.bus_type = _interface->get_device_bus_type();
|
_device_id.devid_s.bus_type = _interface->get_device_bus_type();
|
||||||
_device_id.devid_s.bus = _interface->get_device_bus();
|
_device_id.devid_s.bus = _interface->get_device_bus();
|
||||||
_device_id.devid_s.address = _interface->get_device_address();
|
_device_id.devid_s.address = _interface->get_device_address();
|
||||||
_device_id.devid_s.devtype = DRV_BARO_DEVTYPE_LPS25H;
|
_device_id.devid_s.devtype = DRV_BARO_DEVTYPE_LPS25H;
|
||||||
|
|
||||||
// enable debug() calls
|
|
||||||
_debug_enabled = false;
|
|
||||||
|
|
||||||
// work_cancel in the dtor will explode if we don't do this...
|
|
||||||
memset(&_work, 0, sizeof(_work));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
LPS25H::~LPS25H()
|
LPS25H::~LPS25H()
|
||||||
|
|||||||
@@ -483,20 +483,12 @@ CameraTrigger::stop()
|
|||||||
void
|
void
|
||||||
CameraTrigger::test()
|
CameraTrigger::test()
|
||||||
{
|
{
|
||||||
struct vehicle_command_s cmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = 1.0,
|
vcmd.param5 = 1.0;
|
||||||
.param6 = 0.0,
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
|
||||||
.param1 = 0.0f,
|
|
||||||
.param2 = 0.0f,
|
|
||||||
.param3 = 0.0f,
|
|
||||||
.param4 = 0.0f,
|
|
||||||
.param7 = 0.0f,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL
|
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
(void)orb_unadvertise(pub);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
@@ -722,16 +714,12 @@ CameraTrigger::cycle_trampoline(void *arg)
|
|||||||
|
|
||||||
// Command ACK handling
|
// Command ACK handling
|
||||||
if (updated && need_ack) {
|
if (updated && need_ack) {
|
||||||
vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = 0,
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
command_ack.command = cmd.command;
|
||||||
.command = cmd.command,
|
command_ack.result = (uint8_t)cmd_result;
|
||||||
.result = (uint8_t)cmd_result,
|
command_ack.target_system = cmd.source_system;
|
||||||
.from_external = false,
|
command_ack.target_component = cmd.source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = cmd.source_system,
|
|
||||||
.target_component = cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (trig->_cmd_ack_pub == nullptr) {
|
if (trig->_cmd_ack_pub == nullptr) {
|
||||||
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||||
@@ -739,7 +727,6 @@ CameraTrigger::cycle_trampoline(void *arg)
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -898,4 +885,3 @@ int camera_trigger_main(int argc, char *argv[])
|
|||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -207,17 +207,19 @@ protected:
|
|||||||
virtual int probe();
|
virtual int probe();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
work_s _work;
|
work_s _work{};
|
||||||
unsigned _measure_ticks;
|
unsigned _measure_ticks{0};
|
||||||
|
|
||||||
ringbuffer::RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports{nullptr};
|
||||||
struct mag_calibration_s _scale;
|
|
||||||
float _range_scale;
|
|
||||||
bool _collect_phase;
|
|
||||||
int _class_instance;
|
|
||||||
int _orb_class_instance;
|
|
||||||
|
|
||||||
orb_advert_t _mag_topic;
|
struct mag_calibration_s _scale {};
|
||||||
|
float _range_scale{0.003f}; /* default range scale from counts to gauss */
|
||||||
|
|
||||||
|
bool _collect_phase{false};
|
||||||
|
int _class_instance{-1};
|
||||||
|
int _orb_class_instance{-1};
|
||||||
|
|
||||||
|
orb_advert_t _mag_topic{nullptr};
|
||||||
|
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
perf_counter_t _comms_errors;
|
perf_counter_t _comms_errors;
|
||||||
@@ -225,16 +227,16 @@ private:
|
|||||||
perf_counter_t _conf_errors;
|
perf_counter_t _conf_errors;
|
||||||
|
|
||||||
/* status reporting */
|
/* status reporting */
|
||||||
bool _sensor_ok; /**< sensor was found and reports ok */
|
bool _sensor_ok{false}; /**< sensor was found and reports ok */
|
||||||
bool _calibrated; /**< the calibration is valid */
|
bool _calibrated{false}; /**< the calibration is valid */
|
||||||
bool _ctl_reg_mismatch; /**< control register value mismatch after checking */
|
bool _ctl_reg_mismatch{false}; /**< control register value mismatch after checking */
|
||||||
|
|
||||||
enum Rotation _rotation;
|
enum Rotation _rotation;
|
||||||
|
|
||||||
struct mag_report _last_report; /**< used for info() */
|
sensor_mag_s _last_report{}; /**< used for info() */
|
||||||
|
|
||||||
uint8_t _ctl3_reg;
|
uint8_t _ctl3_reg{0};
|
||||||
uint8_t _ctl4_reg;
|
uint8_t _ctl4_reg{0};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initialise the automatic measurement state machine and start it.
|
* Initialise the automatic measurement state machine and start it.
|
||||||
@@ -396,32 +398,14 @@ extern "C" __EXPORT int ist8310_main(int argc, char *argv[]);
|
|||||||
|
|
||||||
IST8310::IST8310(int bus_number, int address, const char *path, enum Rotation rotation) :
|
IST8310::IST8310(int bus_number, int address, const char *path, enum Rotation rotation) :
|
||||||
I2C("IST8310", path, bus_number, address, IST8310_DEFAULT_BUS_SPEED),
|
I2C("IST8310", path, bus_number, address, IST8310_DEFAULT_BUS_SPEED),
|
||||||
_work{},
|
|
||||||
_measure_ticks(0),
|
|
||||||
_reports(nullptr),
|
|
||||||
_scale{},
|
|
||||||
_range_scale(0.003), /* default range scale from counts to gauss */
|
|
||||||
_collect_phase(false),
|
|
||||||
_class_instance(-1),
|
|
||||||
_orb_class_instance(-1),
|
|
||||||
_mag_topic(nullptr),
|
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "ist8310_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "ist8310_read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "ist8310_com_err")),
|
_comms_errors(perf_alloc(PC_COUNT, "ist8310_com_err")),
|
||||||
_range_errors(perf_alloc(PC_COUNT, "ist8310_rng_err")),
|
_range_errors(perf_alloc(PC_COUNT, "ist8310_rng_err")),
|
||||||
_conf_errors(perf_alloc(PC_COUNT, "ist8310_conf_err")),
|
_conf_errors(perf_alloc(PC_COUNT, "ist8310_conf_err")),
|
||||||
_sensor_ok(false),
|
_rotation(rotation)
|
||||||
_calibrated(false),
|
|
||||||
_ctl_reg_mismatch(false),
|
|
||||||
_rotation(rotation),
|
|
||||||
_last_report{0},
|
|
||||||
_ctl3_reg(0),
|
|
||||||
_ctl4_reg(0)
|
|
||||||
{
|
{
|
||||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_IST8310;
|
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_IST8310;
|
||||||
|
|
||||||
// enable debug() calls
|
|
||||||
_debug_enabled = false;
|
|
||||||
|
|
||||||
// default scaling
|
// default scaling
|
||||||
_scale.x_offset = 0;
|
_scale.x_offset = 0;
|
||||||
_scale.x_scale = 1.0f;
|
_scale.x_scale = 1.0f;
|
||||||
@@ -429,9 +413,6 @@ IST8310::IST8310(int bus_number, int address, const char *path, enum Rotation ro
|
|||||||
_scale.y_scale = 1.0f;
|
_scale.y_scale = 1.0f;
|
||||||
_scale.z_offset = 0;
|
_scale.z_offset = 0;
|
||||||
_scale.z_scale = 1.0f;
|
_scale.z_scale = 1.0f;
|
||||||
|
|
||||||
// work_cancel in the dtor will explode if we don't do this...
|
|
||||||
memset(&_work, 0, sizeof(_work));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
IST8310::~IST8310()
|
IST8310::~IST8310()
|
||||||
@@ -1018,7 +999,7 @@ out:
|
|||||||
|
|
||||||
int IST8310::calibrate(struct file *filp, unsigned enable)
|
int IST8310::calibrate(struct file *filp, unsigned enable)
|
||||||
{
|
{
|
||||||
struct mag_report report;
|
struct mag_report report {};
|
||||||
ssize_t sz;
|
ssize_t sz;
|
||||||
int ret = 1;
|
int ret = 1;
|
||||||
float total_x = 0.0f;
|
float total_x = 0.0f;
|
||||||
@@ -1387,7 +1368,7 @@ void
|
|||||||
test(enum IST8310_BUS busid)
|
test(enum IST8310_BUS busid)
|
||||||
{
|
{
|
||||||
struct ist8310_bus_option &bus = find_bus(busid);
|
struct ist8310_bus_option &bus = find_bus(busid);
|
||||||
struct mag_report report;
|
struct mag_report report {};
|
||||||
ssize_t sz;
|
ssize_t sz;
|
||||||
int ret;
|
int ret;
|
||||||
const char *path = bus.devpath;
|
const char *path = bus.devpath;
|
||||||
|
|||||||
+11
-20
@@ -733,27 +733,18 @@ PX4IO::init()
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* send command to arm system via command API */
|
/* send command to arm system via command API */
|
||||||
struct vehicle_command_s cmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = 0.0f,
|
vcmd.param1 = 1.0f; /* request arming */
|
||||||
.param6 = 0.0f,
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
|
||||||
/* request arming */
|
vcmd.target_system = (uint8_t)sys_id;
|
||||||
.param1 = 1.0f,
|
vcmd.target_component = (uint8_t)comp_id;
|
||||||
.param2 = 0.0f,
|
vcmd.source_system = (uint8_t)sys_id;
|
||||||
.param3 = 0.0f,
|
vcmd.source_component = (uint8_t)comp_id;
|
||||||
.param4 = 0.0f,
|
vcmd.confirmation = true; /* ask to confirm command */
|
||||||
.param7 = 0.0f,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
|
|
||||||
.target_system = (uint8_t)sys_id,
|
|
||||||
.target_component = (uint8_t)comp_id,
|
|
||||||
.source_system = (uint8_t)sys_id,
|
|
||||||
.source_component = (uint8_t)comp_id,
|
|
||||||
/* ask to confirm command */
|
|
||||||
.confirmation = 1
|
|
||||||
};
|
|
||||||
|
|
||||||
/* send command once */
|
/* send command once */
|
||||||
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
|
|
||||||
/* spin here until IO's state has propagated into the system */
|
/* spin here until IO's state has propagated into the system */
|
||||||
do {
|
do {
|
||||||
@@ -774,7 +765,7 @@ PX4IO::init()
|
|||||||
|
|
||||||
/* re-send if necessary */
|
/* re-send if necessary */
|
||||||
if (!safety.armed) {
|
if (!safety.armed) {
|
||||||
orb_publish(ORB_ID(vehicle_command), pub, &cmd);
|
orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
|
||||||
DEVICE_LOG("re-sending arm cmd");
|
DEVICE_LOG("re-sending arm cmd");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -332,16 +332,13 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
|
|||||||
|
|
||||||
void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
|
void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
|
||||||
{
|
{
|
||||||
vehicle_command_ack_s vehicle_command_ack = {
|
vehicle_command_ack_s vehicle_command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
|
||||||
.result_param2 = 0,
|
vehicle_command_ack.timestamp = hrt_absolute_time();
|
||||||
.command = cmd->command,
|
vehicle_command_ack.command = cmd->command;
|
||||||
.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED,
|
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
.from_external = false,
|
vehicle_command_ack.target_system = cmd->source_system;
|
||||||
.result_param1 = 0,
|
vehicle_command_ack.target_component = cmd->source_component;
|
||||||
.target_system = cmd->source_system,
|
|
||||||
.target_component = cmd->source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_vehicle_command_ack_pub == nullptr) {
|
if (_vehicle_command_ack_pub == nullptr) {
|
||||||
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
||||||
@@ -350,7 +347,6 @@ void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
|
|||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void InputMavlinkCmdMount::print_status()
|
void InputMavlinkCmdMount::print_status()
|
||||||
|
|||||||
@@ -61,19 +61,10 @@ OutputMavlink::~OutputMavlink()
|
|||||||
|
|
||||||
int OutputMavlink::update(const ControlData *control_data)
|
int OutputMavlink::update(const ControlData *control_data)
|
||||||
{
|
{
|
||||||
vehicle_command_s vehicle_command = {
|
vehicle_command_s vehicle_command = {};
|
||||||
.timestamp = 0,
|
vehicle_command.timestamp = hrt_absolute_time();
|
||||||
.param5 = 0.0,
|
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
|
||||||
.param6 = 0.0,
|
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
|
||||||
.param1 = 0.0f,
|
|
||||||
.param2 = 0.0f,
|
|
||||||
.param3 = 0.0f,
|
|
||||||
.param4 = 0.0f,
|
|
||||||
.param7 = 0.0f,
|
|
||||||
.command = 0,
|
|
||||||
.target_system = (uint8_t)_config.mavlink_sys_id,
|
|
||||||
.target_component = (uint8_t)_config.mavlink_comp_id,
|
|
||||||
};
|
|
||||||
|
|
||||||
if (control_data) {
|
if (control_data) {
|
||||||
//got new command
|
//got new command
|
||||||
|
|||||||
@@ -88,10 +88,11 @@ int uuv_example_app_main(int argc, char *argv[])
|
|||||||
orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act);
|
orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act);
|
||||||
|
|
||||||
/* one could wait for multiple topics with this technique, just using one here */
|
/* one could wait for multiple topics with this technique, just using one here */
|
||||||
px4_pollfd_struct_t fds[] = {
|
px4_pollfd_struct_t fds[2] = {};
|
||||||
{ .fd = sensor_sub_fd, .events = POLLIN },
|
fds[0].fd = sensor_sub_fd;
|
||||||
{ .fd = vehicle_attitude_sub_fd, .events = POLLIN },
|
fds[0].events = POLLIN;
|
||||||
};
|
fds[1].fd = vehicle_attitude_sub_fd;
|
||||||
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
int error_counter = 0;
|
int error_counter = 0;
|
||||||
|
|
||||||
|
|||||||
@@ -4,7 +4,7 @@
|
|||||||
constexpr uint64_t FlightTask::_timeout;
|
constexpr uint64_t FlightTask::_timeout;
|
||||||
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
|
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
|
||||||
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
|
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
|
||||||
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN};
|
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, vehicle_constraints_s::GEAR_KEEP, {}};
|
||||||
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
|
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
|
||||||
{
|
{
|
||||||
if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
|
if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
|
||||||
|
|||||||
@@ -78,7 +78,10 @@ read : cdev_read,
|
|||||||
write : cdev_write,
|
write : cdev_write,
|
||||||
seek : cdev_seek,
|
seek : cdev_seek,
|
||||||
ioctl : cdev_ioctl,
|
ioctl : cdev_ioctl,
|
||||||
poll : cdev_poll
|
poll : cdev_poll,
|
||||||
|
#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
|
||||||
|
unlink : nullptr
|
||||||
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
static int
|
static int
|
||||||
|
|||||||
@@ -429,16 +429,12 @@ void AttitudeEstimatorQ::task_main()
|
|||||||
last_time = now;
|
last_time = now;
|
||||||
|
|
||||||
if (update(dt)) {
|
if (update(dt)) {
|
||||||
vehicle_attitude_s att = {
|
vehicle_attitude_s att = {};
|
||||||
.timestamp = sensors.timestamp,
|
att.timestamp = sensors.timestamp;
|
||||||
.rollspeed = _rates(0),
|
att.rollspeed = _rates(0);
|
||||||
.pitchspeed = _rates(1),
|
att.pitchspeed = _rates(1);
|
||||||
.yawspeed = _rates(2),
|
att.yawspeed = _rates(2);
|
||||||
|
_q.copyTo(att.q);
|
||||||
.q = {_q(0), _q(1), _q(2), _q(3)},
|
|
||||||
.delta_q_reset = {},
|
|
||||||
.quat_reset_counter = 0,
|
|
||||||
};
|
|
||||||
|
|
||||||
/* the instance count is not used here */
|
/* the instance count is not used here */
|
||||||
int att_inst;
|
int att_inst;
|
||||||
|
|||||||
@@ -78,23 +78,15 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
|
|||||||
|
|
||||||
static void arm_auth_request_msg_send()
|
static void arm_auth_request_msg_send()
|
||||||
{
|
{
|
||||||
struct vehicle_command_s cmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = 0,
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = 0,
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
|
||||||
.param6 = 0,
|
vcmd.target_system = arm_parameters.authorizer_system_id;
|
||||||
.param1 = 0,
|
|
||||||
.param2 = 0,
|
|
||||||
.param3 = 0,
|
|
||||||
.param4 = 0,
|
|
||||||
.param7 = 0,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST,
|
|
||||||
.target_system = arm_parameters.authorizer_system_id
|
|
||||||
};
|
|
||||||
|
|
||||||
if (handle_vehicle_command_pub == nullptr) {
|
if (handle_vehicle_command_pub == nullptr) {
|
||||||
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
|
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &cmd);
|
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -263,6 +263,26 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
|
||||||
|
{
|
||||||
|
vehicle_command_s vcmd = {};
|
||||||
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
|
vcmd.param1 = param1;
|
||||||
|
vcmd.param2 = param2;
|
||||||
|
vcmd.param3 = NAN;
|
||||||
|
vcmd.param4 = NAN;
|
||||||
|
vcmd.param5 = (double)NAN;
|
||||||
|
vcmd.param6 = (double)NAN;
|
||||||
|
vcmd.param7 = NAN;
|
||||||
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
|
||||||
|
vcmd.target_system = status.system_id;
|
||||||
|
vcmd.target_component = status.component_id;
|
||||||
|
|
||||||
|
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
|
|
||||||
|
return (h != nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
int commander_main(int argc, char *argv[])
|
int commander_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
@@ -273,7 +293,7 @@ int commander_main(int argc, char *argv[])
|
|||||||
if (!strcmp(argv[1], "start")) {
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
if (thread_running) {
|
if (thread_running) {
|
||||||
warnx("already running");
|
PX4_INFO("already running");
|
||||||
/* this is not an error */
|
/* this is not an error */
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@@ -299,7 +319,7 @@ int commander_main(int argc, char *argv[])
|
|||||||
if (!strcmp(argv[1], "stop")) {
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
|
||||||
if (!thread_running) {
|
if (!thread_running) {
|
||||||
warnx("commander already stopped");
|
PX4_WARN("already stopped");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -307,14 +327,14 @@ int commander_main(int argc, char *argv[])
|
|||||||
|
|
||||||
Commander::main(argc, argv);
|
Commander::main(argc, argv);
|
||||||
|
|
||||||
warnx("terminated.");
|
PX4_INFO("terminated.");
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* commands needing the app to run below */
|
/* commands needing the app to run below */
|
||||||
if (!thread_running) {
|
if (!thread_running) {
|
||||||
warnx("\tcommander not started");
|
PX4_ERR("not started");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -339,17 +359,17 @@ int commander_main(int argc, char *argv[])
|
|||||||
} else if (!strcmp(argv[2], "airspeed")) {
|
} else if (!strcmp(argv[2], "airspeed")) {
|
||||||
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
|
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
|
||||||
} else {
|
} else {
|
||||||
warnx("argument %s unsupported.", argv[2]);
|
PX4_ERR("argument %s unsupported.", argv[2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (calib_ret) {
|
if (calib_ret) {
|
||||||
warnx("calibration failed, exiting.");
|
PX4_ERR("calibration failed, exiting.");
|
||||||
return 1;
|
return 1;
|
||||||
} else {
|
} else {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
warnx("missing argument");
|
PX4_ERR("missing argument");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -365,98 +385,51 @@ int commander_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (!strcmp(argv[1], "arm")) {
|
if (!strcmp(argv[1], "arm")) {
|
||||||
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
||||||
warnx("arming failed");
|
PX4_ERR("arming failed");
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "disarm")) {
|
if (!strcmp(argv[1], "disarm")) {
|
||||||
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
|
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
|
||||||
warnx("rejected disarm");
|
PX4_ERR("rejected disarm");
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "takeoff")) {
|
if (!strcmp(argv[1], "takeoff")) {
|
||||||
|
|
||||||
|
bool ret = false;
|
||||||
|
|
||||||
/* see if we got a home position */
|
/* see if we got a home position */
|
||||||
if (status_flags.condition_local_position_valid) {
|
if (status_flags.condition_local_position_valid) {
|
||||||
|
|
||||||
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
||||||
|
ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
|
||||||
struct vehicle_command_s cmd = {
|
|
||||||
.timestamp = hrt_absolute_time(),
|
|
||||||
.param5 = (double)NAN,
|
|
||||||
.param6 = (double)NAN,
|
|
||||||
/* minimum pitch */
|
|
||||||
.param1 = NAN,
|
|
||||||
.param2 = NAN,
|
|
||||||
.param3 = NAN,
|
|
||||||
.param4 = NAN,
|
|
||||||
.param7 = NAN,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
|
|
||||||
.target_system = status.system_id,
|
|
||||||
.target_component = status.component_id
|
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
|
||||||
(void)orb_unadvertise(h);
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
warnx("arming failed");
|
PX4_ERR("arming failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
warnx("rejecting takeoff, no position lock yet. Please retry..");
|
PX4_ERR("rejecting takeoff, no position lock yet. Please retry..");
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return (ret ? 0 : 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "land")) {
|
if (!strcmp(argv[1], "land")) {
|
||||||
|
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND);
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {
|
return (ret ? 0 : 1);
|
||||||
.timestamp = 0,
|
|
||||||
.param5 = (double)NAN,
|
|
||||||
.param6 = (double)NAN,
|
|
||||||
/* minimum pitch */
|
|
||||||
.param1 = NAN,
|
|
||||||
.param2 = NAN,
|
|
||||||
.param3 = NAN,
|
|
||||||
.param4 = NAN,
|
|
||||||
.param7 = NAN,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND,
|
|
||||||
.target_system = status.system_id,
|
|
||||||
.target_component = status.component_id
|
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
|
||||||
(void)orb_unadvertise(h);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "transition")) {
|
if (!strcmp(argv[1], "transition")) {
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {
|
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
|
||||||
.timestamp = 0,
|
(float)(status.is_rotary_wing ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC));
|
||||||
.param5 = (double)NAN,
|
|
||||||
.param6 = (double)NAN,
|
|
||||||
/* transition to the other mode */
|
|
||||||
.param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
|
|
||||||
.param2 = NAN,
|
|
||||||
.param3 = NAN,
|
|
||||||
.param4 = NAN,
|
|
||||||
.param7 = NAN,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
|
|
||||||
.target_system = status.system_id,
|
|
||||||
.target_component = status.component_id
|
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
return (ret ? 0 : 1);
|
||||||
(void)orb_unadvertise(h);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "mode")) {
|
if (!strcmp(argv[1], "mode")) {
|
||||||
@@ -489,16 +462,16 @@ int commander_main(int argc, char *argv[])
|
|||||||
} else if (!strcmp(argv[2], "auto:precland")) {
|
} else if (!strcmp(argv[2], "auto:precland")) {
|
||||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
|
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
|
||||||
} else {
|
} else {
|
||||||
warnx("argument %s unsupported.", argv[2]);
|
PX4_ERR("argument %s unsupported.", argv[2]);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) {
|
if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) {
|
||||||
warnx("mode change failed");
|
PX4_ERR("mode change failed");
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
warnx("missing argument");
|
PX4_ERR("missing argument");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -509,25 +482,10 @@ int commander_main(int argc, char *argv[])
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {
|
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
|
||||||
.timestamp = 0,
|
strcmp(argv[2], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f);
|
||||||
.param5 = 0.0,
|
|
||||||
.param6 = 0.0,
|
|
||||||
/* if the comparison matches for off (== 0) set 0.0f, 2.0f (on) else */
|
|
||||||
.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f, /* lockdown */
|
|
||||||
.param2 = 0.0f,
|
|
||||||
.param3 = 0.0f,
|
|
||||||
.param4 = 0.0f,
|
|
||||||
.param7 = 0.0f,
|
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
|
|
||||||
.target_system = status.system_id,
|
|
||||||
.target_component = status.component_id
|
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
return (ret ? 0 : 1);
|
||||||
(void)orb_unadvertise(h);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
usage("unrecognized command");
|
usage("unrecognized command");
|
||||||
@@ -3700,23 +3658,18 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* publish ACK */
|
/* publish ACK */
|
||||||
vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = 0,
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
command_ack.command = cmd.command;
|
||||||
.command = cmd.command,
|
command_ack.result = (uint8_t)result;
|
||||||
.result = (uint8_t)result,
|
command_ack.target_system = cmd.source_system;
|
||||||
.from_external = false,
|
command_ack.target_component = cmd.source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = cmd.source_system,
|
|
||||||
.target_component = cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (command_ack_pub != nullptr) {
|
if (command_ack_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||||
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -194,16 +194,12 @@ void SendEvent::process_commands()
|
|||||||
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
|
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
|
||||||
{
|
{
|
||||||
/* publish ACK */
|
/* publish ACK */
|
||||||
struct vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
command_ack.command = cmd.command;
|
||||||
.command = cmd.command,
|
command_ack.result = (uint8_t)result;
|
||||||
.result = (uint8_t)result,
|
command_ack.target_system = cmd.source_system;
|
||||||
.from_external = false,
|
command_ack.target_component = cmd.source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = cmd.source_system,
|
|
||||||
.target_component = cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_command_ack_pub != nullptr) {
|
if (_command_ack_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
|
||||||
@@ -214,8 +210,6 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int SendEvent::print_usage(const char *reason)
|
int SendEvent::print_usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason) {
|
if (reason) {
|
||||||
@@ -287,20 +281,18 @@ int SendEvent::custom_command(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = 0,
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = ((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN),
|
vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
|
||||||
.param6 = (double)NAN,
|
vcmd.param2 = NAN;
|
||||||
.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
|
vcmd.param3 = NAN;
|
||||||
.param2 = NAN,
|
vcmd.param4 = NAN;
|
||||||
.param3 = NAN,
|
vcmd.param5 = ((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
|
||||||
.param4 = NAN,
|
vcmd.param6 = (double)NAN;
|
||||||
.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
|
vcmd.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
|
||||||
};
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
(void)orb_unadvertise(h);
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
print_usage("unrecognized command");
|
print_usage("unrecognized command");
|
||||||
|
|||||||
@@ -2228,25 +2228,18 @@ int Logger::remove_directory(const char *dir)
|
|||||||
|
|
||||||
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result)
|
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result)
|
||||||
{
|
{
|
||||||
vehicle_command_ack_s vehicle_command_ack = {
|
vehicle_command_ack_s vehicle_command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vehicle_command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
vehicle_command_ack.command = cmd->command;
|
||||||
.command = cmd->command,
|
vehicle_command_ack.result = (uint8_t)result;
|
||||||
.result = (uint8_t)result,
|
vehicle_command_ack.target_system = cmd->source_system;
|
||||||
.from_external = false,
|
vehicle_command_ack.target_component = cmd->source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = cmd->source_system,
|
|
||||||
.target_component = cmd->source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (vehicle_command_ack_pub == nullptr) {
|
if (vehicle_command_ack_pub == nullptr) {
|
||||||
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||||
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace logger
|
} // namespace logger
|
||||||
|
|||||||
@@ -2288,16 +2288,13 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
|
|
||||||
// send positive command ack
|
// send positive command ack
|
||||||
struct vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = vehicle_cmd.timestamp,
|
command_ack.timestamp = vehicle_cmd.timestamp;
|
||||||
.result_param2 = 0,
|
command_ack.command = vehicle_cmd.command;
|
||||||
.command = vehicle_cmd.command,
|
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||||
.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
|
command_ack.from_external = !vehicle_cmd.from_external;
|
||||||
.from_external = !vehicle_cmd.from_external,
|
command_ack.target_system = vehicle_cmd.source_system;
|
||||||
.result_param1 = 0,
|
command_ack.target_component = vehicle_cmd.source_component;
|
||||||
.target_system = vehicle_cmd.source_system,
|
|
||||||
.target_component = vehicle_cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (command_ack_pub != nullptr) {
|
if (command_ack_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
||||||
|
|||||||
@@ -1551,25 +1551,24 @@ protected:
|
|||||||
|
|
||||||
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
|
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = 0,
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = (double)NAN,
|
vcmd.param1 = 0.0f; // all cameras
|
||||||
.param6 = (double)NAN,
|
vcmd.param2 = 0.0f; // duration 0 because only taking one picture
|
||||||
.param1 = 0.0f, // all cameras
|
vcmd.param3 = 1.0f; // only take one
|
||||||
.param2 = 0.0f, // duration 0 because only taking one picture
|
vcmd.param4 = NAN;
|
||||||
.param3 = 1.0f, // only take one
|
vcmd.param5 = (double)NAN;
|
||||||
.param4 = NAN,
|
vcmd.param6 = (double)NAN;
|
||||||
.param7 = NAN,
|
vcmd.param7 = NAN;
|
||||||
.command = MAV_CMD_IMAGE_START_CAPTURE,
|
vcmd.command = MAV_CMD_IMAGE_START_CAPTURE;
|
||||||
.target_system = mavlink_system.sysid,
|
vcmd.target_system = mavlink_system.sysid;
|
||||||
.target_component = MAV_COMP_ID_CAMERA
|
vcmd.target_component = MAV_COMP_ID_CAMERA;
|
||||||
};
|
|
||||||
|
|
||||||
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
|
MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel());
|
||||||
|
|
||||||
// TODO: move this camera_trigger and publish as a vehicle_command
|
// TODO: move this camera_trigger and publish as a vehicle_command
|
||||||
/* send MAV_CMD_DO_DIGICAM_CONTROL*/
|
/* send MAV_CMD_DO_DIGICAM_CONTROL*/
|
||||||
mavlink_command_long_t digicam_ctrl_cmd;
|
mavlink_command_long_t digicam_ctrl_cmd = {};
|
||||||
|
|
||||||
digicam_ctrl_cmd.target_system = 0; // 0 for broadcast
|
digicam_ctrl_cmd.target_system = 0; // 0 for broadcast
|
||||||
digicam_ctrl_cmd.target_component = MAV_COMP_ID_CAMERA;
|
digicam_ctrl_cmd.target_component = MAV_COMP_ID_CAMERA;
|
||||||
|
|||||||
@@ -174,16 +174,12 @@ MavlinkReceiver::~MavlinkReceiver()
|
|||||||
|
|
||||||
void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
|
void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
|
||||||
{
|
{
|
||||||
vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
command_ack.command = command;
|
||||||
.command = command,
|
command_ack.result = result;
|
||||||
.result = result,
|
command_ack.target_system = sysid;
|
||||||
.from_external = false,
|
command_ack.target_component = compid;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = sysid,
|
|
||||||
.target_component = compid
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||||
@@ -490,24 +486,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||||||
mavlink_command_long_t cmd_mavlink;
|
mavlink_command_long_t cmd_mavlink;
|
||||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||||
|
|
||||||
struct vehicle_command_s vcmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = (double)cmd_mavlink.param5,
|
|
||||||
.param6 = (double)cmd_mavlink.param6,
|
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
vcmd.param1 = cmd_mavlink.param1;
|
||||||
.param1 = cmd_mavlink.param1,
|
vcmd.param2 = cmd_mavlink.param2;
|
||||||
.param2 = cmd_mavlink.param2,
|
vcmd.param3 = cmd_mavlink.param3;
|
||||||
.param3 = cmd_mavlink.param3,
|
vcmd.param4 = cmd_mavlink.param4;
|
||||||
.param4 = cmd_mavlink.param4,
|
vcmd.param5 = (double)cmd_mavlink.param5;
|
||||||
.param7 = cmd_mavlink.param7,
|
vcmd.param6 = (double)cmd_mavlink.param6;
|
||||||
.command = cmd_mavlink.command,
|
vcmd.param7 = cmd_mavlink.param7;
|
||||||
.target_system = cmd_mavlink.target_system,
|
vcmd.command = cmd_mavlink.command;
|
||||||
.target_component = cmd_mavlink.target_component,
|
vcmd.target_system = cmd_mavlink.target_system;
|
||||||
.source_system = msg->sysid,
|
vcmd.target_component = cmd_mavlink.target_component;
|
||||||
.source_component = msg->compid,
|
vcmd.source_system = msg->sysid;
|
||||||
.confirmation = cmd_mavlink.confirmation,
|
vcmd.source_component = msg->compid;
|
||||||
.from_external = true
|
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||||
};
|
vcmd.from_external = true;
|
||||||
|
|
||||||
handle_message_command_both(msg, cmd_mavlink, vcmd);
|
handle_message_command_both(msg, cmd_mavlink, vcmd);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -518,30 +515,28 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
|
|||||||
mavlink_command_int_t cmd_mavlink;
|
mavlink_command_int_t cmd_mavlink;
|
||||||
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
|
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
|
||||||
|
|
||||||
struct vehicle_command_s vcmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
|
|
||||||
.param5 = ((double)cmd_mavlink.x) / 1e7,
|
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
||||||
.param6 = ((double)cmd_mavlink.y) / 1e7,
|
vcmd.param1 = cmd_mavlink.param1;
|
||||||
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
vcmd.param2 = cmd_mavlink.param2;
|
||||||
.param1 = cmd_mavlink.param1,
|
vcmd.param3 = cmd_mavlink.param3;
|
||||||
.param2 = cmd_mavlink.param2,
|
vcmd.param4 = cmd_mavlink.param4;
|
||||||
.param3 = cmd_mavlink.param3,
|
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
|
||||||
.param4 = cmd_mavlink.param4,
|
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
|
||||||
.param7 = cmd_mavlink.z,
|
vcmd.param7 = cmd_mavlink.z;
|
||||||
.command = cmd_mavlink.command,
|
vcmd.command = cmd_mavlink.command;
|
||||||
.target_system = cmd_mavlink.target_system,
|
vcmd.target_system = cmd_mavlink.target_system;
|
||||||
.target_component = cmd_mavlink.target_component,
|
vcmd.target_component = cmd_mavlink.target_component;
|
||||||
.source_system = msg->sysid,
|
vcmd.source_system = msg->sysid;
|
||||||
.source_component = msg->compid,
|
vcmd.source_component = msg->compid;
|
||||||
.confirmation = false,
|
vcmd.confirmation = false;
|
||||||
.from_external = true
|
vcmd.from_external = true;
|
||||||
};
|
|
||||||
|
|
||||||
handle_message_command_both(msg, cmd_mavlink, vcmd);
|
handle_message_command_both(msg, cmd_mavlink, vcmd);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
template <class T>
|
template <class T>
|
||||||
void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
|
void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
|
||||||
const vehicle_command_s &vehicle_command)
|
const vehicle_command_s &vehicle_command)
|
||||||
@@ -636,16 +631,15 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
|
|||||||
|
|
||||||
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid);
|
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid);
|
||||||
|
|
||||||
vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = ack.result_param2,
|
command_ack.result_param2 = ack.result_param2;
|
||||||
.command = ack.command,
|
command_ack.command = ack.command;
|
||||||
.result = ack.result,
|
command_ack.result = ack.result;
|
||||||
.from_external = true,
|
command_ack.from_external = true;
|
||||||
.result_param1 = ack.progress,
|
command_ack.result_param1 = ack.progress;
|
||||||
.target_system = ack.target_system,
|
command_ack.target_system = ack.target_system;
|
||||||
.target_component = ack.target_component
|
command_ack.target_component = ack.target_component;
|
||||||
};
|
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||||
@@ -785,24 +779,21 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
|
|||||||
union px4_custom_mode custom_mode;
|
union px4_custom_mode custom_mode;
|
||||||
custom_mode.data = new_mode.custom_mode;
|
custom_mode.data = new_mode.custom_mode;
|
||||||
|
|
||||||
struct vehicle_command_s vcmd = {
|
vehicle_command_s vcmd = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
vcmd.timestamp = hrt_absolute_time();
|
||||||
.param5 = 0,
|
|
||||||
.param6 = 0,
|
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||||
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
vcmd.param1 = (float)new_mode.base_mode;
|
||||||
.param1 = (float)new_mode.base_mode,
|
vcmd.param2 = (float)custom_mode.main_mode;
|
||||||
.param2 = (float)custom_mode.main_mode,
|
vcmd.param3 = (float)custom_mode.sub_mode;
|
||||||
.param3 = (float)custom_mode.sub_mode,
|
|
||||||
.param4 = 0,
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
|
||||||
.param7 = 0,
|
vcmd.target_system = new_mode.target_system;
|
||||||
.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE,
|
vcmd.target_component = MAV_COMP_ID_ALL;
|
||||||
.target_system = new_mode.target_system,
|
vcmd.source_system = msg->sysid;
|
||||||
.target_component = MAV_COMP_ID_ALL,
|
vcmd.source_component = msg->compid;
|
||||||
.source_system = msg->sysid,
|
vcmd.confirmation = true;
|
||||||
.source_component = msg->compid,
|
vcmd.from_external = true;
|
||||||
.confirmation = 1,
|
|
||||||
.from_external = true
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_cmd_pub == nullptr) {
|
if (_cmd_pub == nullptr) {
|
||||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
|
|||||||
@@ -32,6 +32,7 @@
|
|||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
@@ -563,16 +564,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Acknowledge the received command
|
// Acknowledge the received command
|
||||||
struct vehicle_command_ack_s ack = {
|
vehicle_command_ack_s ack = {};
|
||||||
.timestamp = 0,
|
ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
ack.command = cmd.command;
|
||||||
.command = cmd.command,
|
ack.result = cmd_ack_result;
|
||||||
.result = cmd_ack_result,
|
ack.target_system = cmd.source_system;
|
||||||
.from_external = false,
|
ack.target_component = cmd.source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = cmd.source_system,
|
|
||||||
.target_component = cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||||
|
|||||||
@@ -363,16 +363,12 @@ VtolAttitudeControl::handle_command()
|
|||||||
// This might not be optimal but is better than no response at all.
|
// This might not be optimal but is better than no response at all.
|
||||||
|
|
||||||
if (_vehicle_cmd.from_external) {
|
if (_vehicle_cmd.from_external) {
|
||||||
vehicle_command_ack_s command_ack = {
|
vehicle_command_ack_s command_ack = {};
|
||||||
.timestamp = hrt_absolute_time(),
|
command_ack.timestamp = hrt_absolute_time();
|
||||||
.result_param2 = 0,
|
command_ack.command = _vehicle_cmd.command;
|
||||||
.command = _vehicle_cmd.command,
|
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||||
.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
|
command_ack.target_system = _vehicle_cmd.source_system;
|
||||||
.from_external = false,
|
command_ack.target_component = _vehicle_cmd.source_component;
|
||||||
.result_param1 = 0,
|
|
||||||
.target_system = _vehicle_cmd.source_system,
|
|
||||||
.target_component = _vehicle_cmd.source_component
|
|
||||||
};
|
|
||||||
|
|
||||||
if (_v_cmd_ack_pub == nullptr) {
|
if (_v_cmd_ack_pub == nullptr) {
|
||||||
_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||||
@@ -380,7 +376,6 @@ VtolAttitudeControl::handle_command()
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user