dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

merged master

This commit is contained in:
Lorenz Meier 2014-06-26 13:31:33 +02:00
parent 0ef6605498 3e76be0d69
commit 6e597a66de
26 changed files with 1116 additions and 651 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/3031_phantom
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

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
usleep 100000
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
usleep 100000
mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
usleep 100000
# Exit shell to make it available to MAVLink
exit

38
src/drivers/px4io/px4io.cpp
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);
}

6
src/drivers/px4io/px4io_uploader.cpp
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;
}

4
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() :
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
_bodyrate_setpoint(0.0f),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
{
perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
}
ECL_PitchController::~ECL_PitchController()

4
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() :
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
_bodyrate_setpoint(0.0f),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
{
perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
}
ECL_RollController::~ECL_RollController()

4
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() :
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_coordinated_min_speed(1.0f)
_coordinated_min_speed(1.0f),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
{
perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
}
ECL_YawController::~ECL_YawController()

2
src/lib/geo/geo.h
View File

@ -50,7 +50,7 @@
__BEGIN_DECLS
#include "geo/geo_mag_declination.h"
#include "geo_mag_declination.h"
#include <stdbool.h>

26
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
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_m < 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_m < 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];

20
src/modules/commander/commander.cpp
View File

@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@ -945,8 +945,8 @@ 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)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch");
}
}
}
@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
}
status_changed = true;
@ -1163,7 +1163,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) {
// XXX check for sensors
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
} else {
// XXX: Add emergency stuff if sensors are lost
@ -1217,7 +1217,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_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
stick_off_counter = 0;
} else {
@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
}
stick_on_counter = 0;
@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
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;
}
@ -2004,7 +2004,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;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -57,7 +57,7 @@ typedef enum {
} transition_result_t;
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);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);

11
src/modules/fw_att_control/fw_att_control_main.cpp
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

367
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -50,149 +50,322 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Controller parameters, accessible via MAVLink
*
*/
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Range 0.4 to 1.0 seconds, in tens of seconds
/**
* Attitude Time Constant
*
* This defines the latency between a step input and the achieved setpoint
* (inverse to a P gain). Half a second is a good start value and fits for
* most average systems. Smaller systems may require smaller values, but as
* this will wear out servos faster, the value should only be decreased as
* needed.
*
* @unit seconds
* @min 0.4
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Pitch rate proportional gain.
// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error.
// @Range 10 to 200, 1 increments
/**
* Pitch rate proportional gain.
*
* This defines how much the elevator input will be commanded depending on the
* current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
// @DisplayName Pitch rate integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
/**
* Pitch rate integrator gain.
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
// @DisplayName Maximum positive / up pitch rate.
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
/**
* Maximum positive / up pitch rate.
*
* This limits the maximum pitch up angular rate the controller will output (in
* degrees per second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
// @DisplayName Maximum negative / down pitch rate.
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
/**
* Maximum negative / down pitch rate.
*
* This limits the maximum pitch down up angular rate the controller will
* output (in degrees per second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
// @DisplayName Pitch rate integrator limit
// @Description The portion of the integrator part in the control surface deflection is limited to this value
// @Range 0.0 to 1
// @Increment 0.1
/**
* Pitch rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @DisplayName Roll to Pitch feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
// @Range 0.5 2.0
// @Increment 0.05
// @User User
/**
* Roll to Pitch feedforward gain.
*
* This compensates during turns and ensures the nose stays level.
*
* @min 0.0
* @max 2.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
// @DisplayName Roll rate proportional Gain.
// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error.
// @Range 10.0 200.0
// @Increment 10.0
// @User User
/**
* Roll rate proportional Gain
*
* This defines how much the aileron input will be commanded depending on the
* current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
// @DisplayName Roll rate integrator Gain
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0.0 100.0
// @Increment 5.0
// @User User
/**
* Roll rate integrator Gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 100.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
// @DisplayName Roll Integrator Anti-Windup
// @Description The portion of the integrator part in the control surface deflection is limited to this value.
// @Range 0.0 to 1.0
// @Increment 0.1
/**
* Roll Integrator Anti-Windup
*
* The portion of the integrator part in the control surface deflection is limited to this value.
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
/**
* Maximum Roll Rate
*
* This limits the maximum roll rate the controller will output (in degrees per
* second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
// @DisplayName Yaw rate proportional gain.
// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error.
// @Range 10 to 200, 1 increments
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
/**
* Yaw rate proportional gain
*
* This defines how much the rudder input will be commanded depending on the
* current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
// @DisplayName Yaw rate integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
/**
* Yaw rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
// @DisplayName Yaw rate integrator limit
// @Description The portion of the integrator part in the control surface deflection is limited to this value
// @Range 0.0 to 1
// @Increment 0.1
/**
* Yaw rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
// @DisplayName Maximum Yaw Rate
// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
/**
* Maximum Yaw Rate
*
* This limits the maximum yaw rate the controller will output (in degrees per
* second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
// @DisplayName Roll rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
/**
* Roll rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
// @DisplayName Pitch rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
/**
* Pitch rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
// @DisplayName Yaw rate feed forward
// @Description Direct feed forward from rate setpoint to control surface output
// @Range 0 to 10
// @Increment 0.1
/**
* Yaw rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
// @DisplayName Minimal speed for yaw coordination
// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
/**
* Minimal speed for yaw coordination
*
* For airspeeds above this value, the yaw rate is calculated for a coordinated
* turn. Set to a very high value to disable.
*
* @unit m/s
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
/* Airspeed parameters:
* The following parameters about airspeed are used by the attitude and the
* position controller.
* */
// @DisplayName Minimum Airspeed
// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
// @Range 0.0 to 30
/**
* Minimum Airspeed
*
* If the airspeed falls below this value, the TECS controller will try to
* increase airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
// @DisplayName Trim Airspeed
// @Description The TECS controller tries to fly at this airspeed
// @Range 0.0 to 30
/**
* Trim Airspeed
*
* The TECS controller tries to fly at this airspeed.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
// @DisplayName Maximum Airspeed
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
// @Range 0.0 to 30
/**
* Maximum Airspeed
*
* If the airspeed is above this value, the TECS controller will try to decrease
* airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
// @DisplayName Roll Setpoint Offset
// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
/**
* Roll Setpoint Offset
*
* An airframe specific offset of the roll setpoint in degrees, the value is
* added to the roll setpoint and should correspond to the typical cruise speed
* of the airframe.
*
* @unit deg
* @min -90.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @DisplayName Pitch Setpoint Offset
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
/**
* Pitch Setpoint Offset
*
* An airframe specific offset of the pitch setpoint in degrees, the value is
* added to the pitch setpoint and should correspond to the typical cruise
* speed of the airframe.
*
* @unit deg
* @min -90.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
// @DisplayName Max Manual Roll
// @Description Max roll for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
/**
* Max Manual Roll
*
* Max roll for manual control in attitude stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
// @DisplayName Max Manual Pitch
// @Description Max pitch for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
/**
* Max Manual Pitch
*
* Max pitch for manual control in attitude stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);

14
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* 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;
@ -1020,15 +1024,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;

35
src/modules/mavlink/mavlink_commands.cpp
View File

@ -40,34 +40,31 @@
#include "mavlink_commands.h"
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
}
MavlinkCommandsStream::~MavlinkCommandsStream()
{
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
struct vehicle_command_s cmd;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}

2
src/modules/mavlink/mavlink_commands.h
View File

@ -55,10 +55,10 @@ private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
~MavlinkCommandsStream();
void update(const hrt_abstime t);
};

137
src/modules/mavlink/mavlink_main.cpp
View File

@ -242,6 +242,12 @@ Mavlink::Mavlink() :
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
@ -500,44 +506,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
void Mavlink::mavlink_update_system(void)
{
static bool initialized = false;
static param_t param_system_id;
static param_t param_component_id;
static param_t param_system_type;
static param_t param_use_hil_gps;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
param_use_hil_gps = param_find("MAV_USEHILGPS");
initialized = true;
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_initialized = true;
}
/* update system and component id */
int32_t system_id;
param_get(param_system_id, &system_id);
param_get(_param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
param_get(param_component_id, &component_id);
param_get(_param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
int32_t system_type;
param_get(param_system_type, &system_type);
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(param_use_hil_gps, &use_hil_gps);
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
@ -835,7 +836,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[mavlink pm] unknown: %s", name);
sprintf(buf, "[pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
} else {
@ -1024,8 +1025,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
if (_verbose) { warnx("ERROR: index out of bounds"); }
}
}
@ -1098,8 +1097,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
}
}
@ -1130,8 +1127,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
mavlink_missionlib_send_gcs_string("Operation timeout");
if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
@ -1166,8 +1161,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
}
break;
@ -1191,15 +1184,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
}
@ -1230,8 +1218,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
}
@ -1249,8 +1235,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
break;
}
@ -1261,15 +1245,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
break;
}
@ -1277,17 +1259,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == _wpm->current_wp_id) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == _wpm->current_wp_id + 1) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
break;
}
@ -1295,8 +1275,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
break;
}
@ -1310,7 +1288,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
}
@ -1344,15 +1322,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("COUNT 0");
if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
mavlink_missionlib_send_gcs_string("WP COUNT 0");
break;
}
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
@ -1366,18 +1340,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
}
}
@ -1400,7 +1368,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
break;
}
@ -1408,12 +1375,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wp.seq >= _wpm->current_count) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
break;
}
if (wp.seq != _wpm->current_wp_id) {
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
@ -1528,11 +1494,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
@ -1606,31 +1571,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;
}
@ -1964,9 +1934,12 @@ Mavlink::task_main(int argc, char *argv[])
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
@ -2023,14 +1996,14 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
if (param_sub->update(t)) {
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
if (status_sub->update(t)) {
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status->hil_state == HIL_STATE_ON);
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */

6
src/modules/mavlink/mavlink_main.h
View File

@ -301,6 +301,12 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
/**
* Send one parameter.
*

954
src/modules/mavlink/mavlink_messages.cpp
View File

File diff suppressed because it is too large Load Diff

15
src/modules/mavlink/mavlink_messages.h
View File

@ -43,6 +43,19 @@
#include "mavlink_stream.h"
extern MavlinkStream *streams_list[];
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
~StreamListItem() {};
};
extern StreamListItem *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */

55
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -50,50 +50,55 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
_topic(topic),
_last_check(0),
next(nullptr)
{
_data = malloc(topic->o_size);
memset(_data, 0, topic->o_size);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(_fd);
free(_data);
}
orb_id_t
MavlinkOrbSubscription::get_topic()
MavlinkOrbSubscription::get_topic() const
{
return _topic;
}
void *
MavlinkOrbSubscription::get_data()
bool
MavlinkOrbSubscription::update(uint64_t *time, void* data)
{
return _data;
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
return false;
} else {
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
} else {
return false;
}
}
}
bool
MavlinkOrbSubscription::update(const hrt_abstime t)
MavlinkOrbSubscription::update(void* data)
{
if (_last_check == t) {
/* already checked right now, return result of the check */
return _updated;
} else {
_last_check = t;
orb_check(_fd, &_updated);
if (_updated) {
orb_copy(_topic, _fd, _data);
_published = true;
return true;
}
}
return false;
return !orb_copy(_topic, _fd, data);
}
bool

30
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -48,12 +48,26 @@
class MavlinkOrbSubscription
{
public:
MavlinkOrbSubscription *next; /*< pointer to next subscription in list */
MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
/**
* Check if subscription updated and get data.
*
* @return true only if topic was updated and data copied to buffer successfully.
* If topic was not updated since last check it will return false but still copy the data.
* If no data available data buffer will be filled with zeroes.
*/
bool update(uint64_t *time, void* data);
/**
* Copy topic data to given buffer.
*
* @return true only if topic data copied successfully.
*/
bool update(void* data);
/**
* Check if the topic has been published.
@ -62,16 +76,12 @@ public:
* @return true if the topic has been published at least once.
*/
bool is_published();
void *get_data();
orb_id_t get_topic();
orb_id_t get_topic() const;
private:
const orb_id_t _topic; /*< topic metadata */
int _fd; /*< subscription handle */
bool _published; /*< topic was ever published */
void *_data; /*< pointer to data buffer */
hrt_abstime _last_check; /*< time of last check */
bool _updated; /*< updated on last check */
const orb_id_t _topic; ///< topic metadata
int _fd; ///< subscription handle
bool _published; ///< topic was ever published
};

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -959,7 +959,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);

22
src/modules/mavlink/mavlink_stream.h
View File

@ -50,14 +50,6 @@ class MavlinkStream;
class MavlinkStream
{
private:
hrt_abstime _last_sent;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
public:
MavlinkStream *next;
@ -71,9 +63,19 @@ public:
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
static MavlinkStream *new_instance();
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() = 0;
virtual const char *get_name() const = 0;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
private:
hrt_abstime _last_sent;
};

4
src/modules/systemlib/perf_counter.c
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,

2
src/modules/uORB/topics/vehicle_attitude_setpoint.h
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) */
};