aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp2
-rw-r--r--src/modules/commander/commander.cpp2
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp6
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c11
-rw-r--r--src/modules/mavlink/mavlink_main.cpp91
-rw-r--r--src/modules/mavlink/mavlink_main.h1
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp2
-rw-r--r--src/modules/navigator/navigator_main.cpp42
-rw-r--r--src/modules/uORB/topics/actuator_armed.h20
9 files changed, 79 insertions, 98 deletions
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 4ca8b5e42..8bf76dcc3 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -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))
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c0628a16..65922b2a5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
home.alt = global_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
- mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 5b877cd77..116d3cc63 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -232,8 +232,6 @@ private:
float throttle_land_max;
- float loiter_hold_radius;
-
float heightrate_p;
float speedrate_p;
@@ -277,8 +275,6 @@ private:
param_t throttle_land_max;
- param_t loiter_hold_radius;
-
param_t heightrate_p;
param_t speedrate_p;
@@ -441,7 +437,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");
@@ -513,7 +508,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));
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index f258f77da..52128e1b7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -72,17 +72,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
*
* This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 28cdf65a3..e300be074 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -105,7 +105,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
-static uint64_t last_write_times[6] = {0};
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
@@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
- if (buf_free == 0) {
-
- if (last_write_times[(unsigned)channel] != 0 &&
- hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
-
- warnx("DISABLING HARDWARE FLOW CONTROL");
- instance->enable_flow_control(false);
- }
-
- } else {
-
- /* apparently there is space left, although we might be
- * partially overflooding the buffer already */
- last_write_times[(unsigned)channel] = hrt_absolute_time();
+ /* Disable hardware flow control:
+ * if no successful write since a defined time
+ * and if the last try was not the last successful write
+ */
+ if (last_write_try_times[(unsigned)channel] != 0 &&
+ hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+ last_write_success_times[(unsigned)channel] !=
+ last_write_try_times[(unsigned)channel])
+ {
+ warnx("DISABLING HARDWARE FLOW CONTROL");
+ instance->enable_flow_control(false);
}
+
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
+ last_write_try_times[(unsigned)channel] = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
@@ -199,6 +199,8 @@ 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");
+ } else {
+ last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
@@ -788,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: {
@@ -894,6 +901,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
+ mission_item->time_inside = mavlink_mission_item->param1;
break;
}
@@ -902,7 +910,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
- mission_item->time_inside = mavlink_mission_item->param1;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
@@ -921,11 +928,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
- mavlink_mission_item->param2 = mission_item->pitch_min;
+ mavlink_mission_item->param1 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
+ mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
@@ -936,7 +944,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
- mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
@@ -952,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;
}
@@ -1052,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); }
}
@@ -1067,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); }
@@ -1109,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);
}
}
@@ -1171,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;
@@ -1208,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;
@@ -1301,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"); }
}
}
@@ -1365,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;
@@ -1438,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;
}
@@ -1469,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;
@@ -1509,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;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 25c0da820..40edc4b85 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -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;
};
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 933478f56..fd41b723a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -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,
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 401d50f7e..06e0c5904 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
/* XXX TODO count turns */
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) {
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
acceptance_radius = _parameters.acceptance_radius;
}
- 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 (_do_takeoff) {
- if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
- /* require only altitude for 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;
}
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
}
if (_mission.current_mission_available()) {
- set_mission_item();
+ if (_mission_item.autocontinue) {
+ /* continue mission */
+ set_mission_item();
+
+ } else {
+ /* autocontinue disabled for this item */
+ request_loiter_or_ready();
+ }
} else {
/* if no more mission items available then finish mission */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 6e944ffee..a98d3fc3a 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013 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
@@ -44,15 +44,25 @@
#include <stdint.h>
#include "../uORB.h"
+/**
+ * @addtogroup topics
+ * @{
+ */
+
/** global 'actuator output is live' control. */
struct actuator_armed_s {
- uint64_t timestamp;
- bool armed; /**< Set to true if system is armed */
- bool ready_to_arm; /**< Set to true if system is ready to be armed */
- bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+ uint64_t timestamp; /**< Microseconds since system boot */
+ bool armed; /**< Set to true if system is armed */
+ bool ready_to_arm; /**< Set to true if system is ready to be armed */
+ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
ORB_DECLARE(actuator_armed);
#endif \ No newline at end of file
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-09 19:27:21 +0000