fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 12:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-29 19:12:35 +02:00
parent 164f19176e 94c69e11ae
commit ecee13861c
59 changed files with 1415 additions and 807 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/3031_phantom
+4
View File
@@ -37,3 +37,7 @@ then
fi
set MIXER FMU_Q
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_DISARMED 1000
ROMFS/px4fmu_common/init.d/rc.usb
+1 -1
View File
@@ -3,7 +3,7 @@
# USB MAVLink start
#
mavlink start -r 10000 -d /dev/ttyACM0
mavlink start -r 10000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
usleep 100000
makefiles/config_aerocore_default.mk
+1 -1
View File
@@ -80,8 +80,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
makefiles/config_px4fmu-v1_default.mk
+1 -1
View File
@@ -107,8 +107,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
makefiles/config_px4fmu-v2_default.mk
+1 -1
View File
@@ -120,8 +120,8 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/geo_lookup
MODULES += lib/conversion
MODULES += lib/launchdetection
mavlink/include/mavlink/v1.0/checksum.h
+1 -1
View File
@@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
* @param data new bytes to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
{
const uint8_t *p = (const uint8_t *)pBuffer;
while (length--) {
mavlink/include/mavlink/v1.0/common/common.h
+4
View File
@@ -5,6 +5,10 @@
#ifndef COMMON_H
#define COMMON_H
#ifndef MAVLINK_H
#error Wrong include order: common.h MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set all defines from mavlink.h manually.
#endif
#ifdef __cplusplus
extern "C" {
#endif
mavlink/include/mavlink/v1.0/mavlink_helpers.h
+14 -7
View File
@@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
#endif
{
// This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX;
msg->len = length;
msg->sysid = system_id;
@@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
// One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
crc_accumulate(crc_extra, &msg->checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
@@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
buf[4] = mavlink_system.compid;
buf[5] = msgid;
status->current_tx_seq++;
checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
@@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
// XXX use the right sequence here
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
@@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}
mavlink/include/mavlink/v1.0/mavlink_types.h
+2 -2
View File
@@ -28,6 +28,7 @@
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#pragma pack(push, 1)
typedef struct param_union {
union {
float param_float;
@@ -62,13 +63,12 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
#pragma pack(pop)
typedef enum {
MAVLINK_TYPE_CHAR = 0,
src/drivers/px4io/px4io.cpp
+21 -17
View File
@@ -197,8 +197,10 @@ public:
* Print IO status.
*
* Print all relevant IO status information
*
* @param extended_status Shows more verbose information (in particular RC config)
*/
void print_status();
void print_status(bool extended_status);
/**
* Fetch and print debug console output.
@@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
}
void
PX4IO::print_status()
PX4IO::print_status(bool extended_status)
{
/* basic configuration */
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -2013,19 +2015,21 @@ PX4IO::print_status()
printf("\n");
}
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
i,
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
options,
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
if (extended_status) {
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
i,
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
options,
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
}
}
printf("failsafe");
@@ -2853,7 +2857,7 @@ monitor(void)
if (g_dev != nullptr) {
printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_status(false);
(void)g_dev->print_debug();
printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
@@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
printf("[px4io] loaded\n");
g_dev->print_status();
g_dev->print_status(true);
exit(0);
}
src/drivers/px4io/px4io_uploader.cpp
+3 -3
View File
@@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[])
close(_io_fd);
_io_fd = -1;
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
return ret;
}
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
-3
View File
@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
-3
View File
@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0; //xxx: param
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+1 -6
View File
@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
if(fabsf(denumerator) > FLT_EPSILON) {
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/geo/geo.h
+1 -1
View File
@@ -50,7 +50,7 @@
__BEGIN_DECLS
#include "geo/geo_mag_declination.h"
#include "geo_lookup/geo_mag_declination.h"
#include <stdbool.h>
src/lib/geo/module.mk
+2 -3
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,5 +35,4 @@
# Geo library
#
SRCS = geo.c \
geo_mag_declination.c
SRCS = geo.c
src/lib/geo/geo_mag_declination.c → src/lib/geo_lookup/geo_mag_declination.c
View File
src/lib/geo/geo_mag_declination.h → src/lib/geo_lookup/geo_mag_declination.h
View File
src/lib/geo_lookup/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Geo lookup table / data library
#
SRCS = geo_mag_declination.c
MAXOPTIMIZATION = -Os
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+21 -5
View File
@@ -65,6 +65,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
@@ -287,6 +288,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
/* magnetic declination, in radians */
float mag_decl = 0.0f;
/* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl;
R_decl.identity();
@@ -325,9 +329,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
}
/* only run filter if sensor values changed */
@@ -340,6 +341,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_check(sub_gps, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
}
}
bool global_pos_updated;
@@ -469,7 +477,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
} else {
mag_decl = ekf_params.mag_decl;
}
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
@@ -515,7 +531,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + ekf_params.mag_decl;
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
-13
View File
@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
@@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
-1
View File
@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
param_t mag_decl;
param_t acc_comp;
};
src/modules/commander/commander.cpp
+34 -15
View File
@@ -980,7 +980,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1179,18 +1179,16 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 1");
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 2");
arming_state_changed = true;
}
}
@@ -1202,7 +1200,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1261,7 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1286,7 +1284,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1638,10 +1636,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL");
}
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@@ -1652,7 +1650,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
// fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@@ -1664,28 +1662,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_LOITER");
} else {
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_MISSION");
}
// else fallback to ALTCTL (POSCTL likely will not work too)
// fallback to POSCTL
res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to MANUAL
// fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@@ -1941,8 +1961,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2005,7 +2024,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
break;
}
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -59,7 +59,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+2 -2
View File
@@ -1368,8 +1368,8 @@ FixedwingEstimator::task_main()
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
src/modules/fw_att_control/fw_att_control_main.cpp
+9 -2
View File
@@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral)
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+29 -59
View File
@@ -43,8 +43,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
* Implementation for total energy control class:
* Thomas Gubler
*
* More details and acknowledgements in the referenced library headers.
*
@@ -88,7 +88,6 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
@@ -199,7 +198,6 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
@@ -565,23 +563,6 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@@ -835,10 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -863,15 +837,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* Initialize attitude controller integrator reset flags to 0 */
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
/* previous waypoint */
math::Vector<2> prev_wp;
@@ -1061,15 +1036,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
// warnx("Launch detection running");
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
@@ -1218,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
@@ -1259,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
_att_sp.pitch_body = _mTecs.getPitchSetpoint();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1445,29 +1424,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+15 -15
View File
@@ -58,6 +58,7 @@ mTecs::mTecs() :
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* time measurement */
updateTimeMeasurement();
/* Filter arispeed */
/* Filter airspeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
}
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
@@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
/* update parameters first */
updateParams();
/* Filter flightpathangle */
float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
/* calculate values (energies) */
float flightPathAngleError = flightPathAngleSp - flightPathAngle;
float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@@ -199,10 +201,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == TECS_MODE_LAND) {
// only limit pitch but do not limit throttle
@@ -218,14 +220,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
_controlTotalEnergy.getOutputLimiter() :
*outputLimiterThrottle,
outputLimiterPitch == NULL ?
_controlEnergyDistribution.getOutputLimiter() :
*outputLimiterPitch);
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+20 -14
View File
@@ -104,12 +104,17 @@ protected:
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control:
output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight
path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
@@ -118,21 +123,22 @@ protected:
float _pitchSp; /**< Pitch Setpoint from -pi to pi */
/* Output Limits in special modes */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in
last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
/* Time measurements */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
int _counter;
bool _debug; ///< Set true to enable debug output
bool _debug; ///< Set true to enable debug output
static void debug_print(const char *fmt, va_list args);
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+7
View File
@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
/**
* P gain for the altitude control
* Maps the altitude error to the flight path angle setpoint
src/modules/mavlink/mavlink_ftp.cpp
+415
View File
@@ -0,0 +1,415 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <crc32.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include "mavlink_ftp.h"
MavlinkFTP *MavlinkFTP::_server;
MavlinkFTP *
MavlinkFTP::getServer()
{
// XXX this really cries out for some locking...
if (_server == nullptr) {
_server = new MavlinkFTP;
}
return _server;
}
MavlinkFTP::MavlinkFTP()
{
// initialise the request freelist
dq_init(&_workFree);
sem_init(&_lock, 0, 1);
// initialize session list
for (size_t i=0; i<kMaxSession; i++) {
_session_fds[i] = -1;
}
// drop work entries onto the free list
for (unsigned i = 0; i < kRequestQueueSize; i++) {
_qFree(&_workBufs[i]);
}
}
void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
{
// get a free request
auto req = _dqFree();
// if we couldn't get a request slot, just drop it
if (req != nullptr) {
// decode the request
if (req->decode(mavlink, msg)) {
// and queue it for the worker
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
} else {
_qFree(req);
}
}
}
void
MavlinkFTP::_workerTrampoline(void *arg)
{
auto req = reinterpret_cast<Request *>(arg);
auto server = MavlinkFTP::getServer();
// call the server worker with the work item
server->_worker(req);
}
void
MavlinkFTP::_worker(Request *req)
{
auto hdr = req->header();
ErrorCode errorCode = kErrNone;
uint32_t messageCRC;
// basic sanity checks; must validate length before use
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
errorCode = kErrNoRequest;
goto out;
}
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
printf("ftp: bad crc\n");
}
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
switch (hdr->opcode) {
case kCmdNone:
break;
case kCmdTerminate:
errorCode = _workTerminate(req);
break;
case kCmdReset:
errorCode = _workReset();
break;
case kCmdList:
errorCode = _workList(req);
break;
case kCmdOpen:
errorCode = _workOpen(req, false);
break;
case kCmdCreate:
errorCode = _workOpen(req, true);
break;
case kCmdRead:
errorCode = _workRead(req);
break;
case kCmdWrite:
errorCode = _workWrite(req);
break;
case kCmdRemove:
errorCode = _workRemove(req);
break;
default:
errorCode = kErrNoRequest;
break;
}
out:
// handle success vs. error
if (errorCode == kErrNone) {
hdr->opcode = kRspAck;
printf("FTP: ack\n");
} else {
printf("FTP: nak %u\n", errorCode);
hdr->opcode = kRspNak;
hdr->size = 1;
hdr->data[0] = errorCode;
}
// respond to the request
_reply(req);
// free the request buffer back to the freelist
_qFree(req);
}
void
MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
hdr->crc32 = crc32(req->rawData(), req->dataSize());
// then pack and send the reply back to the request source
req->reply();
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(Request *req)
{
auto hdr = req->header();
DIR *dp = opendir(req->dataAsCString());
if (dp == nullptr) {
printf("FTP: can't open path '%s'\n", req->dataAsCString());
return kErrNotDir;
}
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr;
unsigned offset = 0;
// move to the requested offset
seekdir(dp, hdr->offset);
for (;;) {
// read the directory entry
if (readdir_r(dp, &entry, &result)) {
errorCode = kErrIO;
break;
}
// no more entries?
if (result == nullptr) {
if (hdr->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
// name too big to fit?
if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
break;
}
// store the type marker
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
break;
default:
hdr->data[offset++] = kDirentUnknown;
break;
}
// copy the name, which we know will fit
strcpy((char *)&hdr->data[offset], entry.d_name);
offset += strlen(entry.d_name) + 1;
printf("FTP: list %s\n", entry.d_name);
}
closedir(dp);
hdr->size = offset;
return errorCode;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(Request *req, bool create)
{
auto hdr = req->header();
int session_index = _findUnusedSession();
if (session_index < 0) {
return kErrNoSession;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
if (fd < 0) {
return create ? kErrPerm : kErrNotFile;
}
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(Request *req)
{
auto hdr = req->header();
int session_index = hdr->session;
if (!_validSession(session_index)) {
return kErrNoSession;
}
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
return kErrEOF;
}
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
return kErrIO;
}
printf("Read success %d\n", bytes_read);
hdr->size = bytes_read;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(Request *req)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
// look up session
auto session = getSession(hdr->session);
if (session == nullptr) {
return kErrNoSession;
}
// append to file
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
if (result < 0) {
// XXX might also be no space, I/O, etc.
return kErrNotAppend;
}
hdr->size = result;
return kErrNone;
#else
return kErrPerm;
#endif
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemove(Request *req)
{
auto hdr = req->header();
// for now, send error reply
return kErrPerm;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
::close(_session_fds[hdr->session]);
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
}
bool
MavlinkFTP::_validSession(unsigned index)
{
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
return false;
}
return true;
}
int
MavlinkFTP::_findUnusedSession(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
}
char *
MavlinkFTP::Request::dataAsCString()
{
// guarantee nul termination
if (header()->size < kMaxDataLength) {
requestData()[header()->size] = '\0';
} else {
requestData()[kMaxDataLength - 1] = '\0';
}
// and return data
return (char *)&(header()->data[0]);
}
src/modules/mavlink/mavlink_ftp.h
+226
View File
@@ -0,0 +1,226 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file mavlink_ftp.h
*
* MAVLink remote file server.
*
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
* a session ID and sequence number.
*
* A limited number of requests (currently 2) may be outstanding at a time.
* Additional messages will be discarded.
*
* Messages consist of a fixed header, followed by a data area.
*
*/
#include <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
class MavlinkFTP
{
public:
MavlinkFTP();
static MavlinkFTP *getServer();
// static interface
void handle_message(Mavlink* mavlink,
mavlink_message_t *msg);
private:
static const unsigned kRequestQueueSize = 2;
static MavlinkFTP *_server;
struct RequestHeader
{
uint8_t magic;
uint8_t session;
uint8_t opcode;
uint8_t size;
uint32_t crc32;
uint32_t offset;
uint8_t data[];
};
enum Opcode : uint8_t
{
kCmdNone, // ignored, always acked
kCmdTerminate, // releases sessionID, closes file
kCmdReset, // terminates all sessions
kCmdList, // list files in <path> from <offset>
kCmdOpen, // opens <path> for reading, returns <session>
kCmdRead, // reads <size> bytes from <offset> in <session>
kCmdCreate, // creates <path> for writing, returns <session>
kCmdWrite, // appends <size> bytes at <offset> in <session>
kCmdRemove, // remove file (only if created by server?)
kRspAck,
kRspNak
};
enum ErrorCode : uint8_t
{
kErrNone,
kErrNoRequest,
kErrNoSession,
kErrSequence,
kErrNotDir,
kErrNotFile,
kErrEOF,
kErrNotAppend,
kErrTooBig,
kErrIO,
kErrPerm
};
int _findUnusedSession(void);
bool _validSession(unsigned index);
static const unsigned kMaxSession = 2;
int _session_fds[kMaxSession];
class Request
{
public:
union {
dq_entry_t entry;
work_s work;
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
_mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
}
return false;
}
void reply() {
// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
// flat memory architecture, as we're operating between threads here.
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence(), rawData());
_mavlink->lockMessageBufferMutex();
bool fError = _mavlink->message_buffer_write(&msg, len);
_mavlink->unlockMessageBufferMutex();
if (!fError) {
warnx("FTP TX ERR");
} else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
_mavlink->get_system_id(),
_mavlink->get_component_id(),
_mavlink->get_channel(),
len,
msg.checksum);
}
}
uint8_t *rawData() { return &_message.data[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
};
static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.
///
static void _workerTrampoline(void *arg);
void _worker(Request *req);
/// Reply to a request (XXX should be a Request method)
///
void _reply(Request *req);
ErrorCode _workList(Request *req);
ErrorCode _workOpen(Request *req, bool create);
ErrorCode _workRead(Request *req);
ErrorCode _workWrite(Request *req);
ErrorCode _workRemove(Request *req);
ErrorCode _workTerminate(Request *req);
ErrorCode _workReset();
// work freelist
Request _workBufs[kRequestQueueSize];
dq_queue_t _workFree;
sem_t _lock;
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
void _qUnlock() { sem_post(&_lock); }
void _qFree(Request *req) {
_qLock();
dq_addlast(&req->entry, &_workFree);
_qUnlock();
}
Request *_dqFree() {
_qLock();
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
_qUnlock();
return req;
}
};
src/modules/mavlink/mavlink_main.cpp
+96 -41
View File
@@ -80,6 +80,10 @@
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#endif
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -113,6 +117,7 @@ static uint64_t last_write_try_times[6] = {0};
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
Mavlink *instance;
switch (channel) {
@@ -197,7 +202,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
warnx("TX FAIL");
// XXX overflow perf
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
@@ -227,6 +232,7 @@ Mavlink::Mavlink() :
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
@@ -455,7 +461,7 @@ Mavlink::get_instance_id()
return _instance_id;
}
mavlink_channel_t
const mavlink_channel_t
Mavlink::get_channel()
{
return _channel;
@@ -532,6 +538,16 @@ void Mavlink::mavlink_update_system(void)
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::get_system_id()
{
return mavlink_system.sysid;
}
int Mavlink::get_component_id()
{
return mavlink_system.compid;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
/* process baud rate */
@@ -950,32 +966,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
/* delete stream */
LL_DELETE(_streams, stream);
delete stream;
warnx("deleted stream %s", stream->get_name());
}
return OK;
}
}
if (interval > 0) {
/* search for stream with specified name in supported streams list */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream->set_interval(interval);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
}
}
} else {
/* stream not found, nothing to disable */
if (interval == 0) {
/* stream was not active and is requested to be disabled, do nothing */
return OK;
}
/* search for stream with specified name in supported streams list */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream->set_interval(interval);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
}
}
/* if we reach here, the stream list does not contain the stream */
warnx("stream %s not found", stream_name);
return ERROR;
}
@@ -1010,11 +1030,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int
Mavlink::message_buffer_init(int size)
{
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
return (_message_buffer.data == 0) ? ERROR : OK;
int ret;
if (_message_buffer.data == 0) {
ret = ERROR;
_message_buffer.size = 0;
} else {
ret = OK;
}
return ret;
}
void
@@ -1146,7 +1176,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1202,6 +1232,10 @@ Mavlink::task_main(int argc, char *argv[])
_wait_to_transmit = true;
break;
case 'x':
_ftp_on = true;
break;
default:
err_flag = true;
break;
@@ -1265,9 +1299,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
if (OK != message_buffer_init(300)) {
if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1408,32 +1445,50 @@ Mavlink::task_main(int argc, char *argv[])
}
}
/* pass messages from other UARTs */
if (_passing_on) {
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
bool is_part;
void *read_ptr;
uint8_t *read_ptr;
uint8_t *write_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
/* write first part of buffer */
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t*)&msg;
// Pull a single message from the buffer
int read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
@@ -1483,7 +1538,7 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
if (_passing_on) {
if (_passing_on || _ftp_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
@@ -1625,7 +1680,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
}
int mavlink_main(int argc, char *argv[])
src/modules/mavlink/mavlink_main.h
+132 -57
View File
@@ -83,27 +83,41 @@ public:
/**
* Display the mavlink status.
*/
void status();
void status();
static int stream(int argc, char *argv[]);
static int stream(int argc, char *argv[]);
static int instance_count();
static int instance_count();
static Mavlink *new_instance();
static Mavlink *new_instance();
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance_for_device(const char *device_name);
static Mavlink *get_instance_for_device(const char *device_name);
static int destroy_all_instances();
static int destroy_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
static int get_uart_fd(unsigned index);
int get_uart_fd();
int get_uart_fd();
/**
* Get the MAVLink system id.
*
* @return The system ID of this vehicle
*/
int get_system_id();
/**
* Get the MAVLink component id.
*
* @return The component ID of this vehicle
*/
int get_component_id();
const char *_device_name;
@@ -113,26 +127,33 @@ public:
MAVLINK_MODE_CAMERA
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
bool get_forwarding_on() { return _forwarding_on; }
<<<<<<< HEAD
=======
/**
* Handle waypoint related messages.
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
static int start_helper(int argc, char *argv[]);
static int start_helper(int argc, char *argv[]);
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -142,28 +163,32 @@ public:
* requested change could not be made or was
* redundant.
*/
int set_hil_enabled(bool hil_enabled);
int set_hil_enabled(bool hil_enabled);
<<<<<<< HEAD
void send_message(const mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
=======
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
int get_instance_id();
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
const mavlink_channel_t get_channel();
bool _task_should_exit; /**< if true, mavlink task should exit */
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
int get_mavlink_fd() { return _mavlink_fd; }
int send_statustext(const char *string);
int send_statustext(enum MAV_SEVERITY severity, const char *string);
@@ -171,33 +196,39 @@ public:
float get_rate_mult();
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
protected:
Mavlink *next;
Mavlink *next;
private:
int _instance_id;
int _instance_id;
int _mavlink_fd;
bool _task_running;
int _mavlink_fd;
bool _task_running;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
<<<<<<< HEAD
MavlinkMissionManager *_mission_manager;
orb_advert_t _mission_pub;
@@ -205,32 +236,54 @@ private:
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
=======
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
unsigned int _total_counter;
pthread_t _receive_thread;
pthread_t _receive_thread;
<<<<<<< HEAD
bool _verbose;
bool _forwarding_on;
bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
=======
/* Allocate storage space for waypoints */
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate;
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
* to len - 1, the end of the param list.
*/
unsigned int _mavlink_param_queue_index;
unsigned int _mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
bool mavlink_link_termination_allowed;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
bool _flow_control_enabled;
struct mavlink_message_buffer {
int write_ptr;
@@ -238,11 +291,13 @@ private:
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
@@ -255,7 +310,7 @@ private:
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
@@ -263,7 +318,7 @@ private:
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
@@ -271,14 +326,14 @@ private:
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
@@ -288,13 +343,30 @@ private:
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_pm_start_queued_send();
void mavlink_update_system();
void mavlink_update_system();
<<<<<<< HEAD
uint8_t get_system_id();
uint8_t get_component_id();
=======
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
void mavlink_wpm_send_waypoint_current(uint16_t seq);
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
void mavlink_wpm_init(mavlink_wpm_storage *state);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
void publish_mission();
void mavlink_missionlib_send_message(mavlink_message_t *msg);
int mavlink_missionlib_send_gcs_string(const char *string);
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
@@ -309,8 +381,11 @@ private:
int message_buffer_is_empty();
<<<<<<< HEAD
bool message_buffer_write(const void *ptr, int size);
=======
>>>>>>> 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
src/modules/mavlink/mavlink_messages.cpp
+23 -14
View File
@@ -253,19 +253,28 @@ protected:
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
/* always send the heartbeat, independent of the update status of the topics */
if (!status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
};
@@ -1302,6 +1311,7 @@ protected:
// New message
mavlink_msg_rc_channels_send(_channel,
rc.timestamp_publication / 1000,
rc.channel_count,
((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
@@ -1320,7 +1330,6 @@ protected:
((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
rc.channel_count,
rc.rssi);
}
}
@@ -1652,7 +1661,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
src/modules/mavlink/mavlink_receiver.cpp
+7
View File
@@ -113,6 +113,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_alt0(0.0)
{
memset(&hil_local_pos, 0, sizeof(hil_local_pos));
// make sure the FTP server is started
(void)MavlinkFTP::getServer();
}
MavlinkReceiver::~MavlinkReceiver()
@@ -155,6 +158,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
default:
break;
}
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -68,6 +68,8 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include "mavlink_ftp.h"
class Mavlink;
class MavlinkReceiver
src/modules/mavlink/module.mk
+2 -1
View File
@@ -44,7 +44,8 @@ SRCS += mavlink_main.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
mavlink_commands.cpp
mavlink_commands.cpp \
mavlink_ftp.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/navigator/loiter.cpp
+2 -4
View File
@@ -53,8 +53,7 @@
#include "loiter.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator)
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
@@ -70,11 +69,10 @@ bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(false, pos_sp_triplet);;
return set_loiter_item(pos_sp_triplet);
}
void
Loiter::on_inactive()
{
}
src/modules/navigator/loiter.h
+1 -1
View File
@@ -47,7 +47,7 @@
#include "navigator_mode.h"
#include "mission_block.h"
class Loiter : public NavigatorMode, MissionBlock
class Loiter : public MissionBlock
{
public:
/**
src/modules/navigator/mission.cpp
+6 -17
View File
@@ -58,8 +58,7 @@
#include "mission.h"
Mission::Mission(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission({0}),
_offboard_mission({0}),
@@ -231,7 +230,7 @@ Mission::advance_mission()
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
set_previous_pos_setpoint(pos_sp_triplet);
/* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -241,7 +240,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_is_in_loiter(false);
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -251,7 +250,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_is_in_loiter(false);
_navigator->set_can_loiter_at_sp(false);
} else {
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -261,24 +260,14 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: no mission available");
}
_mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
set_loiter_item(pos_sp_triplet);
reset_mission_item_reached();
report_mission_finished();
}
}
void
Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp)
{
/* reuse current setpoint as previous setpoint */
if (current_pos_sp->valid) {
memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
}
}
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
src/modules/navigator/mission.h
+1 -7
View File
@@ -62,7 +62,7 @@
class Navigator;
class Mission : public NavigatorMode, MissionBlock
class Mission : public MissionBlock
{
public:
/**
@@ -106,12 +106,6 @@ private:
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set previous position setpoint
*/
void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp);
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set
src/modules/navigator/mission_block.cpp
+67 -50
View File
@@ -52,13 +52,13 @@
#include "mission_block.h"
MissionBlock::MissionBlock(Navigator *navigator) :
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false),
_navigator_priv(navigator)
_mission_item_valid(false)
{
}
@@ -69,20 +69,15 @@ MissionBlock::~MissionBlock()
bool
MissionBlock::is_mission_item_reached()
{
/* don't check landed WPs */
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return false;
return _navigator->get_vstatus()->condition_landed;
}
/* TODO: count turns */
#if 0
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
/* TODO: count turns */
if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
return false;
}
#endif
hrt_abstime now = hrt_absolute_time();
@@ -93,24 +88,24 @@ MissionBlock::is_mission_item_reached()
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator_priv->get_home_position()->alt
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator_priv->get_global_position()->lat,
_navigator_priv->get_global_position()->lon,
_navigator_priv->get_global_position()->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator_priv->get_global_position()->alt >
altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
@@ -124,10 +119,10 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
@@ -172,54 +167,76 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
break;
} else if (item->nav_cmd == NAV_CMD_LAND) {
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
break;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
break;
} else {
default:
sp->type = SETPOINT_TYPE_POSITION;
break;
}
}
void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* reuse current setpoint as previous setpoint */
if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
}
}
bool
MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
if (_navigator_priv->get_is_in_loiter()) {
/* already loitering, bail out */
return false;
}
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
/* leave position setpoint as is */
} else {
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator->set_can_loiter_at_sp(true);
}
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
_navigator_priv->set_is_in_loiter(true);
return true;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
return true;
}
return false;
}
src/modules/navigator/mission_block.h
+10 -9
View File
@@ -47,17 +47,17 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator_mode.h"
class Navigator;
class MissionBlock
class MissionBlock : public NavigatorMode
{
public:
/**
* Constructor
*
* @param pointer to parent class
*/
MissionBlock(Navigator *navigator);
MissionBlock(Navigator *navigator, const char *name);
/**
* Destructor
@@ -82,14 +82,18 @@ public:
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Set previous position setpoint to current setpoint
*/
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param true if the current position setpoint should be re-used
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/
bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@@ -97,9 +101,6 @@ public:
mission_item_s _mission_item;
bool _mission_item_valid;
private:
Navigator *_navigator_priv;
};
#endif
src/modules/navigator/navigator.h
+5 -5
View File
@@ -102,7 +102,7 @@ public:
/**
* Setters
*/
void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
/**
* Getters
@@ -113,9 +113,9 @@ public:
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
bool get_is_in_loiter() { return _is_in_loiter; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
@@ -161,11 +161,11 @@ private:
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/
src/modules/navigator/navigator_main.cpp
+4 -326
View File
@@ -123,7 +123,7 @@ Navigator::Navigator() :
_rtl(this, "RTL"),
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@@ -347,7 +347,7 @@ Navigator::task_main()
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
_navigation_mode = nullptr;
_is_in_loiter = false;
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
_navigation_mode = &_mission;
@@ -365,7 +365,7 @@ Navigator::task_main()
case NAVIGATION_STATE_TERMINATION:
default:
_navigation_mode = nullptr;
_is_in_loiter = false;
_can_loiter_at_sp = false;
break;
}
@@ -386,7 +386,7 @@ Navigator::task_main()
_update_triplet = true;
}
if (_update_triplet ) {
if (_update_triplet) {
publish_position_setpoint_triplet();
_update_triplet = false;
}
@@ -446,329 +446,7 @@ Navigator::status()
warnx("Geofence not set");
}
}
#if 0
bool
Navigator::start_none_on_ground()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::start_none_in_air()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::start_auto_on_ground()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
_update_triplet = true;
return true;
}
bool
Navigator::start_loiter()
{
/* if no existing item available, use current position */
if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
_pos_sp_triplet.current.alt = _global_pos.alt;
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
_update_triplet = true;
return true;
}
bool
Navigator::start_mission()
{
/* start fresh */
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
return set_mission_items();
}
bool
Navigator::advance_mission()
{
/* tell mission to move by one */
_mission.move_to_next();
/* now try to set the new mission items, if it fails, it will dispatch loiter */
return set_mission_items();
}
bool
Navigator::set_mission_items()
{
if (_pos_sp_triplet.current.valid) {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_pos_sp_triplet.previous.valid = true;
}
bool onboard;
int index;
/* if we fail to set the current mission, continue to loiter */
if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
return false;
}
/* if we got an RTL mission item, switch to RTL mode and give up */
if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
return false;
}
_mission_item_valid = true;
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
mission_item_s next_mission_item;
bool last_wp = false;
/* now try to set the next mission item as well, if there is no more next
* this means we're heading to the last waypoint */
if (_mission.get_next_mission_item(&next_mission_item)) {
/* convert the next mission item and set it valid */
mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
_pos_sp_triplet.next.valid = true;
} else {
last_wp = true;
}
/* notify user about what happened */
mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
_update_triplet = true;
reset_reached();
return true;
}
bool
Navigator::start_rtl()
{
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
reset_reached();
_update_triplet = true;
return true;
}
/* if RTL doesn't work, fallback to loiter */
return false;
}
bool
Navigator::advance_rtl()
{
/* tell mission to move by one */
_rtl.move_to_next();
/* now try to set the new mission items, if it fails, it will dispatch loiter */
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
reset_reached();
_update_triplet = true;
return true;
}
return false;
}
bool
Navigator::start_land()
{
/* TODO: verify/test */
reset_reached();
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::check_mission_item_reached()
{
/* only check if there is actually a mission item to check */
if (!_mission_item_valid) {
return false;
}
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _vstatus.condition_landed;
}
/* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
// return false;
// }
uint64_t now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float acceptance_radius;
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
acceptance_radius = _mission_item.acceptance_radius;
} else {
acceptance_radius = _parameters.acceptance_radius;
}
if (_do_takeoff) {
/* require only altitude for takeoff */
if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
_waypoint_position_reached = true;
}
} else {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
/* calculate AMSL altitude for this waypoint */
float wp_alt_amsl = _mission_item.altitude;
if (_mission_item.altitude_is_relative)
wp_alt_amsl += _home_pos.alt;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true;
}
}
}
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
/* check if the current waypoint was reached */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
if (_mission_item.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
(double)_mission_item.time_inside);
}
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return true;
}
}
return false;
}
void
Navigator::reset_reached()
{
_time_first_inside_orbit = 0;
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
}
#endif
void
Navigator::publish_position_setpoint_triplet()
{
src/modules/navigator/navigator_params.c
+3 -3
View File
@@ -55,12 +55,12 @@
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Takeoff Acceptance Radius (FW only)
* Acceptance Radius
*
* Acceptance radius for fixedwing.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
src/modules/navigator/rtl.cpp
+113 -75
View File
@@ -44,6 +44,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -52,14 +53,14 @@
#include "navigator.h"
#include "rtl.h"
#define DELAY_SIGMA 0.01f
RTL::RTL(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_param_acceptance_radius(this, "ACCEPT_RAD")
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
@@ -75,7 +76,11 @@ void
RTL::on_inactive()
{
_first_run = true;
_rtl_state = RTL_STATE_NONE;
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
}
bool
@@ -84,14 +89,33 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
bool updated = false;
if (_first_run) {
_first_run = false;
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item(pos_sp_triplet);
updated = true;
_first_run = false;
}
if ((_rtl_state == RTL_STATE_CLIMB
|| _rtl_state == RTL_STATE_RETURN)
&& is_mission_item_reached()) {
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
@@ -106,31 +130,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
/* make sure we have the latest params */
updateParams();
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_FINISHED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
_rtl_state = RTL_STATE_RETURN;
}
}
/* if switching directly to return state, set altitude setpoint to current altitude */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
set_previous_pos_setpoint(pos_sp_triplet);
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
@@ -141,50 +145,49 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
/* TODO: add this again */
// don't change altitude
// if (_pos_sp_triplet.previous.valid) {
// /* if previous setpoint is valid then use it to calculate heading to home */
// _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
// } else {
// /* else use current position */
// _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
// }
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_DESCEND: {
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
@@ -193,21 +196,46 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(true);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LAND: {
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
}
break;
}
case RTL_STATE_LAND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
@@ -216,36 +244,44 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
case RTL_STATE_FINISHED: {
/* nothing to do, report fail */
case RTL_STATE_LANDED: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
break;
}
default:
break;
}
if (_rtl_state == RTL_STATE_FINISHED) {
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
} else {
/* if not finished, convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
void
@@ -262,18 +298,20 @@ RTL::advance_rtl()
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < 0) {
_rtl_state = RTL_STATE_FINISHED;
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_FINISHED;
case RTL_STATE_LOITER:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_FINISHED:
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_LANDED;
break;
default:
src/modules/navigator/rtl.h
+3 -3
View File
@@ -54,7 +54,7 @@
class Navigator;
class RTL : public NavigatorMode, MissionBlock
class RTL : public MissionBlock
{
public:
/**
@@ -97,14 +97,14 @@ private:
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_LAND,
RTL_STATE_FINISHED,
RTL_STATE_LANDED,
} _rtl_state;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
control::BlockParamFloat _param_acceptance_radius;
};
#endif
src/modules/navigator/rtl_params.c
-12
View File
@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
/**
* RTL acceptance radius
*
* Acceptance radius for waypoints set for RTL
*
* @unit meters
* @min 1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);
src/modules/sdlog2/sdlog2.c
+1
View File
@@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -353,6 +353,7 @@ struct log_TECS_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
@@ -431,7 +432,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
src/modules/sensors/sensor_params.c
+30
View File
@@ -242,6 +242,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
*/
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
/**
* Board rotation Y (Pitch) offset
*
* This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
/**
* Board rotation X (Roll) offset
*
* This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
/**
* Board rotation Z (YAW) offset
*
* This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
/**
* External magnetometer rotation
*
src/modules/sensors/sensors.cpp
+22 -1
View File
@@ -229,7 +229,7 @@ private:
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -252,6 +252,8 @@ private:
int board_rotation;
int external_mag_rotation;
float board_offset[3];
int rc_map_roll;
int rc_map_pitch;
@@ -341,6 +343,8 @@ private:
param_t board_rotation;
param_t external_mag_rotation;
param_t board_offset[3];
} _parameter_handles; /**< handles for interesting parameters */
@@ -587,6 +591,11 @@ Sensors::Sensors() :
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* fetch initial parameter values */
parameters_update();
@@ -791,6 +800,18 @@ Sensors::parameters_update()
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
return OK;
}
src/modules/systemlib/perf_counter.c
+2 -2
View File
@@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
handle->name,
pce->event_count,
pce->time_total,
@@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
handle->name,
pci->event_count,
(pci->time_last - pci->time_first) / pci->event_count,
src/modules/systemlib/perf_counter.h
+1 -1
View File
@@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle);
/**
* Count a performance event.
*
* This call only affects counters that take single events; PC_COUNT etc.
* This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
*
* @param handle The handle returned from perf_alloc.
*/
src/modules/uORB/topics/mission.h
+1
View File
@@ -50,6 +50,7 @@
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
NAV_CMD_IDLE=0,
NAV_CMD_WAYPOINT=16,
NAV_CMD_LOITER_UNLIMITED=17,
NAV_CMD_LOITER_TURN_COUNT=18,
src/modules/uORB/topics/tecs_status.h
+1
View File
@@ -68,6 +68,7 @@ struct tecs_status_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
src/modules/uORB/topics/vehicle_attitude_setpoint.h
+2
View File
@@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s {
float thrust; /**< Thrust in Newton the power system should generate */
bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */
bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */
bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */
};