aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/pca9685/pca9685.cpp26
-rw-r--r--src/modules/mavlink/mavlink_main.cpp12
-rw-r--r--src/modules/mavlink/mavlink_main.h2
-rw-r--r--src/modules/mavlink/mavlink_mission.cpp21
-rw-r--r--src/modules/mavlink/mavlink_mission.h2
-rw-r--r--src/modules/navigator/navigator_main.cpp4
-rw-r--r--src/modules/sensors/sensors.cpp1
7 files changed, 38 insertions, 30 deletions
diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp
index c97d1e9ab..6f69ce8a1 100644
--- a/src/drivers/pca9685/pca9685.cpp
+++ b/src/drivers/pca9685/pca9685.cpp
@@ -98,13 +98,15 @@
#define PCA9685_PWMFREQ 60.0f
#define PCA9685_NCHANS 16 // total amount of pwm outputs
-#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
-#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
+#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
+#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
-#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2)
-#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE)
-#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees
-#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG))
+#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
+#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
+ PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
+ PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
+ */
+#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -300,7 +302,7 @@ void
PCA9685::i2cpwm()
{
if (_mode == IOX_MODE_TEST_OUT) {
- setPin(0, PCA9685_CENTER);
+ setPin(0, PCA9685_PWMCENTER);
_should_run = true;
} else if (_mode == IOX_MODE_OFF) {
_should_run = false;
@@ -320,14 +322,16 @@ PCA9685::i2cpwm()
if (updated) {
orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
- uint16_t new_value = PCA9685_CENTER +
- (_actuator_controls.control[i] * PCA9685_SCALE);
+ /* Scale the controls to PWM, first multiply by pi to get rad,
+ * the control[i] values are on the range -1 ... 1 */
+ uint16_t new_value = PCA9685_PWMCENTER +
+ (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
(double)_actuator_controls.control[i]);
if (new_value != _current_values[i] &&
isfinite(new_value) &&
- new_value >= 0 &&
- new_value <= PCA9685_SERVOMAX) {
+ new_value >= PCA9685_PWMMIN &&
+ new_value <= PCA9685_PWMMAX) {
/* This value was updated, send the command to adjust the PWM value */
setPin(i, new_value);
_current_values[i] = new_value;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 976b6d4d5..ed7e879d3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1229,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
- _mode = MAVLINK_MODE_CAMERA;
+ // left in here for compatibility
+ _mode = MAVLINK_MODE_ONBOARD;
+ } else if (strcmp(optarg, "onboard") == 0) {
+ _mode = MAVLINK_MODE_ONBOARD;
}
break;
@@ -1289,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
- case MAVLINK_MODE_CAMERA:
- warnx("mode: CAMERA");
+ case MAVLINK_MODE_ONBOARD:
+ warnx("mode: ONBOARD");
break;
default:
@@ -1393,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
+ configure_stream("OPTICAL_FLOW", 0.5f);
break;
- case MAVLINK_MODE_CAMERA:
+ case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1e2e94cef..7f9d225bb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,7 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
- MAVLINK_MODE_CAMERA
+ MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 7a761588a..d436c95e9 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -58,6 +58,10 @@
#endif
static const int ERROR = -1;
+#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
+ ((_msg.target_component == mavlink_system.compid) || \
+ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
+ (_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(_interval / 10.0f),
- _verbose(false),
- _comp_id(MAV_COMP_ID_MISSIONPLANNER)
+ _verbose(false)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
- if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
- if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
- if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wprl)) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
- if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpr)) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
- if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+ if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
- if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+ if (CHECK_SYSID_COMPID_MISSION(wp)) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
- if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+ if (CHECK_SYSID_COMPID_MISSION(wpca)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 8ff27f87d..a7bb94c22 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -126,8 +126,6 @@ private:
bool _verbose;
- uint8_t _comp_id;
-
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d77825715..bc9951bf2 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -396,6 +396,8 @@ Navigator::task_main()
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
+ case NAVIGATION_STATE_LAND:
+ case NAVIGATION_STATE_TERMINATION:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
@@ -426,8 +428,6 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_navigation_mode = &_gpsFailure;
break;
- case NAVIGATION_STATE_LAND:
- case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
_navigation_mode = &_offboard;
break;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index aac297ef8..f40034d79 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -857,7 +857,6 @@ Sensors::parameters_update()
errx(1, "FATAL: no barometer found");
} else {
- warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100));
int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (ret) {
warnx("qnh could not be set");
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 07:39:29 +0000