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

Merge remote-tracking branch 'upstream/master' into mtecs

This commit is contained in:
Thomas Gubler 2014-06-11 19:44:17 +02:00
parent e03411fc89 251760679a
commit e548eeb589
16 changed files with 427 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

14
src/drivers/gps/ubx.cpp
View File

@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_gps_position(gps_position),
_configured(false),
_waiting_for_ack(false),
_got_posllh(false),
_got_velned(false),
_got_timeutc(false),
_disable_cmd_last(0)
{
decode_init();
@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
bool handled = false;
while (true) {
bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
if (ret < 0) {
/* something went wrong when polling */
@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
if (handled) {
if (ready_to_return) {
_got_posllh = false;
_got_velned = false;
_got_timeutc = false;
return 1;
} else {
@ -438,6 +445,7 @@ UBX::handle_message()
_rate_count_lat_lon++;
_got_posllh = true;
ret = 1;
break;
}
@ -482,6 +490,7 @@ UBX::handle_message()
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
_gps_position->timestamp_time = hrt_absolute_time();
_got_timeutc = true;
ret = 1;
break;
}
@ -557,6 +566,7 @@ UBX::handle_message()
_rate_count_vel++;
_got_velned = true;
ret = 1;
break;
}

3
src/drivers/gps/ubx.h
View File

@ -397,6 +397,9 @@ private:
struct vehicle_gps_position_s *_gps_position;
bool _configured;
bool _waiting_for_ack;
bool _got_posllh;
bool _got_velned;
bool _got_timeutc;
uint8_t _message_class_needed;
uint8_t _message_id_needed;
ubx_decode_state_t _decode_state;

2
src/drivers/lsm303d/lsm303d.cpp
View File

@ -880,7 +880,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
/* manual measurement */
_mag_reports->flush();
measure();
_mag->measure();
/* measurement will have generated a report, copy it out */
if (_mag_reports->get(mrb))

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()

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);

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

@ -235,8 +235,6 @@ private:
float throttle_land_max;
float loiter_hold_radius;
float heightrate_p;
float speedrate_p;
@ -280,8 +278,6 @@ private:
param_t throttle_land_max;
param_t loiter_hold_radius;
param_t heightrate_p;
param_t speedrate_p;
@ -459,7 +455,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@ -531,7 +526,6 @@ FixedwingPositionControl::parameters_update()
/* L1 control parameters */
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));

11
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -71,17 +71,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
*/
PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
/**
* Default Loiter Radius
*
* This radius is used when no other loiter radius is set.
*
* @min 10.0
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
/**
* Cruise throttle
*

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

@ -790,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
mavlink_param_request_list_t req;
mavlink_msg_param_request_list_decode(msg, &req);
if (req.target_system == mavlink_system.sysid &&
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
}
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
@ -954,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
state->current_partner_compid = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timestamp_last_send_request = 0;
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->current_dataman_id = 0;
}
@ -1054,6 +1060,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
}
@ -1069,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
_wpm->timestamp_last_send_request = hrt_absolute_time();
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
@ -1111,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* try to get WP again after short timeout */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
@ -1173,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
}
break;
@ -1210,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
}
break;
@ -1303,12 +1305,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 from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
@ -1367,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
@ -1440,6 +1430,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
@ -1471,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@ -1511,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;

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

@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_last_send_request;
uint32_t timeout;
int current_dataman_id;
};

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

@ -274,7 +274,7 @@ protected:
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_current * 100.0f,
status->battery_remaining * 100.0f,
status->drop_rate_comm,
status->errors_comm,
@ -938,14 +938,14 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
/* always send this message, even if it has not been updated */
pos_sp_triplet_sub->update(t);
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
};

128
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -71,19 +71,20 @@
#include "inertial_filter.h"
#define MIN_VALID_W 0.00001f
#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz
#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false;
static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s
static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static inline int min(int val1, int val2)
{
return (val1 < val2) ? val1 : val2;
}
static inline int max(int val1, int val2)
{
return (val1 > val2) ? val1 : val2;
}
/**
* Print the correct usage.
*/
@ -135,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL);
exit(0);
@ -199,8 +210,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float eph = 1.0;
float epv = 1.0;
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
float R_gps[3][3]; // rotation matrix for GPS correction moment
memset(est_buf, 0, sizeof(est_buf));
memset(R_buf, 0, sizeof(R_buf));
memset(R_gps, 0, sizeof(R_gps));
int buf_ptr = 0;
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;
float eph_flow = 1.0f;
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev));
@ -257,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation
float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
@ -536,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
w_flow *= 0.05f;
}
flow_valid = true;
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1 / w_flow;
eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm
flow_valid = true;
} else {
w_flow = 0.0f;
@ -597,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@ -657,16 +679,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
y_est[1] = gps.vel_e_m_s;
}
/* calculate index of estimated values in buffer */
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
if (est_i < 0) {
est_i += EST_BUF_SIZE;
}
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
} else {
corr_gps[0][1] = 0.0f;
@ -674,8 +702,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_gps[2][1] = 0.0f;
}
eph = fminf(eph, gps.eph_m);
epv = fminf(epv, gps.epv_m);
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
@ -717,8 +745,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* increase EPH/EPV on each step */
eph *= 1.0 + dt;
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
if (eph < max_eph_epv) {
eph *= 1.0 + dt;
}
if (epv < max_eph_epv) {
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
@ -731,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
xy_src_time = t;
}
bool can_estimate_xy = eph < max_eph_epv * 1.5;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@ -763,7 +795,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_baro += offs_corr;
}
/* accelerometer bias correction */
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
@ -777,6 +809,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += R_gps[j][i] * accel_bias_corr[j];
}
if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
@ -807,7 +857,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
if (use_gps_z) {
epv = fminf(epv, gps.epv_m);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
}
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
@ -832,11 +887,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter correction for position */
if (use_flow) {
eph = fminf(eph, eph_flow);
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph_m);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
@ -910,8 +969,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
if (t > pub_last + pub_interval) {
if (t > pub_last + PUB_INTERVAL) {
pub_last = t;
/* push current estimate to buffer */
est_buf[buf_ptr][0][0] = x_est[0];
est_buf[buf_ptr][0][1] = x_est[1];
est_buf[buf_ptr][1][0] = y_est[0];
est_buf[buf_ptr][1][1] = y_est[1];
est_buf[buf_ptr][2][0] = z_est[0];
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;

3
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->land_t = param_find("INAV_LAND_T");
h->land_disp = param_find("INAV_LAND_DISP");
h->land_thr = param_find("INAV_LAND_THR");
h->delay_gps = param_find("INAV_DELAY_GPS");
return OK;
}
@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->land_t, &(p->land_t));
param_get(h->land_disp, &(p->land_disp));
param_get(h->land_thr, &(p->land_thr));
param_get(h->delay_gps, &(p->delay_gps));
return OK;
}

2
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -56,6 +56,7 @@ struct position_estimator_inav_params {
float land_t;
float land_disp;
float land_thr;
float delay_gps;
};
struct position_estimator_inav_param_handles {
@ -74,6 +75,7 @@ struct position_estimator_inav_param_handles {
param_t land_t;
param_t land_disp;
param_t land_thr;
param_t delay_gps;
};
/**