aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/ardrone_interface/ardrone_interface.c2
-rw-r--r--apps/ardrone_interface/ardrone_motor_control.c3
-rw-r--r--apps/ardrone_interface/ardrone_motor_control.h3
-rw-r--r--apps/commander/commander.c57
-rw-r--r--apps/multirotor_att_control/multirotor_att_control_main.c14
-rw-r--r--apps/uORB/topics/vehicle_status.h1
6 files changed, 49 insertions, 31 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index f12f9cb47..f0bc0b1e7 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 89ed183cc..e410d3a71 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
return errcounter;
}
-/*
+/**
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
@@ -373,7 +373,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
-
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h
index 664419707..78b603b63 100644
--- a/apps/ardrone_interface/ardrone_motor_control.h
+++ b/apps/ardrone_interface/ardrone_motor_control.h
@@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio);
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
+/**
+ * Mix motors and output actuators
+ */
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index a66885b06..66a981a68 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1050,6 +1050,8 @@ int commander_thread_main(int argc, char *argv[])
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
current_status.flag_system_armed = false;
+ current_status.offboard_control_signal_found_once = false;
+ current_status.rc_signal_found_once = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1129,8 +1131,16 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
/* Get current values */
- orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
- orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+ bool new_data;
+ orb_check(sp_man_sub, &new_data);
+ if (new_data) {
+ orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+ }
+
+ orb_check(sp_offboard_sub, &new_data);
+ if (new_data) {
+ orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+ }
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
@@ -1300,9 +1310,8 @@ int commander_thread_main(int argc, char *argv[])
/* ignore RC signals if in offboard control mode */
- if (!current_status.offboard_control_signal_found_once) {
+ if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* Start RC state check */
- bool prev_lost = current_status.rc_signal_lost;
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
@@ -1355,38 +1364,33 @@ int commander_thread_main(int argc, char *argv[])
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
}
+ current_status.rc_signal_cutting_off = false;
current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0;
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
- if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
+ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the offboard control is NOT active */
+ current_status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
- current_status.rc_signal_cutting_off = true;
current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (current_status.rc_signal_lost_interval > 1000000) {
current_status.rc_signal_lost = true;
current_status.failsave_lowlevel = true;
+ state_changed = true;
}
// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
// publish_armed_status(&current_status);
// }
}
-
- /* Check if this is the first loss or first gain*/
- if ((!prev_lost && current_status.rc_signal_lost) ||
- (prev_lost && !current_status.rc_signal_lost)) {
- /* publish change */
- publish_armed_status(&current_status);
- }
}
@@ -1398,23 +1402,29 @@ int commander_thread_main(int argc, char *argv[])
/* State machine update for offboard control */
- if (!current_status.rc_signal_found_once) {
+ if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
+ /* decide about attitude control flag, enable in att/pos/vel */
+ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
+ sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
+ sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
+
+ /* decide about rate control flag, enable it always XXX (for now) */
+ bool rates_ctrl_enabled = true;
+
/* set up control mode */
- if (current_status.flag_control_attitude_enabled !=
- (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) {
- current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE);
+ if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
+ current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
state_changed = true;
}
- if (current_status.flag_control_rates_enabled !=
- (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) {
- current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES);
+ if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
+ current_status.flag_control_rates_enabled = rates_ctrl_enabled;
state_changed = true;
}
- /* handle the case where RC signal was regained */
+ /* handle the case where offboard control signal was regained */
if (!current_status.offboard_control_signal_found_once) {
current_status.offboard_control_signal_found_once = true;
/* enable offboard control, disable manual input */
@@ -1430,6 +1440,7 @@ int commander_thread_main(int argc, char *argv[])
}
}
+ current_status.offboard_control_signal_weak = false;
current_status.offboard_control_signal_lost = false;
current_status.offboard_control_signal_lost_interval = 0;
@@ -1445,8 +1456,8 @@ int commander_thread_main(int argc, char *argv[])
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
- if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
- /* only complain if the RC control is NOT active */
+ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
+ current_status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!");
last_print_time = hrt_absolute_time();
}
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 904872dde..8d648c194 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -79,6 +79,8 @@ static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
+static struct vehicle_status_s state;
+
/**
* Perform rate control right after gyro reading
*/
@@ -120,7 +122,7 @@ static void *rate_control_thread_main(void *arg)
/* perform local lowpass */
/* apply controller */
- // if (state.flag_control_rates_enabled) {
+ if (state.flag_control_rates_enabled) {
/* lowpass gyros */
// XXX
gyro_lp[0] = gyro_report.x;
@@ -129,7 +131,7 @@ static void *rate_control_thread_main(void *arg)
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
- // }
+ }
}
}
@@ -140,7 +142,6 @@ static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
- struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@@ -205,7 +206,11 @@ mc_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of system state */
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+ bool updated;
+ orb_check(state_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+ }
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
@@ -213,7 +218,6 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* get a local copy of rates setpoint */
- bool updated;
orb_check(setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index c727527b1..23172d7cf 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -143,6 +143,7 @@ struct vehicle_status_s
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
+ bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 21:47:14 +0000