aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/commander/commander.c94
-rw-r--r--apps/drivers/px4fmu/fmu.cpp10
-rw-r--r--apps/drivers/px4io/px4io.cpp108
-rw-r--r--apps/fixedwing_att_control/fixedwing_att_control_main.c55
-rw-r--r--apps/mavlink/mavlink_receiver.c24
-rw-r--r--apps/px4io/comms.c40
-rw-r--r--apps/px4io/controls.c36
-rw-r--r--apps/px4io/mixer.c14
-rw-r--r--apps/px4io/protocol.h9
-rw-r--r--apps/px4io/px4io.c2
-rw-r--r--apps/px4io/px4io.h31
-rw-r--r--apps/px4io/safety.c13
-rw-r--r--apps/uORB/topics/vehicle_status.h7
13 files changed, 390 insertions, 53 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 7277e9fa4..310a707aa 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -72,10 +72,12 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
@@ -1167,6 +1169,8 @@ int commander_thread_main(int argc, char *argv[])
failsafe_lowlevel_timeout_ms = 0;
param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
+ param_t _param_sys_type = param_find("MAV_TYPE");
+
/* welcome user */
printf("[commander] I am in command now!\n");
@@ -1199,6 +1203,8 @@ int commander_thread_main(int argc, char *argv[])
/* mark all signals lost as long as they haven't been found */
current_status.rc_signal_lost = true;
current_status.offboard_control_signal_lost = true;
+ /* allow manual override initially */
+ current_status.flag_external_manual_override_ok = true;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1249,9 +1255,15 @@ int commander_thread_main(int argc, char *argv[])
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
- int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
- struct vehicle_gps_position_s gps;
- memset(&gps, 0, sizeof(gps));
+ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ struct vehicle_global_position_s global_position;
+ memset(&global_position, 0, sizeof(global_position));
+ uint64_t last_global_position_time = 0;
+
+ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+ struct vehicle_local_position_s local_position;
+ memset(&local_position, 0, sizeof(local_position));
+ uint64_t last_local_position_time = 0;
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s sensors;
@@ -1262,6 +1274,11 @@ int commander_thread_main(int argc, char *argv[])
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
+ /* Subscribe to parameters changed topic */
+ int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+ struct parameter_update_s param_changed;
+ memset(&param_changed, 0, sizeof(param_changed));
+
// uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f;
@@ -1288,7 +1305,6 @@ int commander_thread_main(int argc, char *argv[])
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);
orb_check(cmd_sub, &new_data);
@@ -1300,6 +1316,46 @@ int commander_thread_main(int argc, char *argv[])
handle_command(stat_pub, &current_status, &cmd);
}
+ /* update parameters */
+ orb_check(param_changed_sub, &new_data);
+ if (new_data) {
+ /* parameters changed */
+ orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+ /* update parameters */
+ if (!current_status.flag_system_armed) {
+ current_status.system_type = param_get(_param_sys_type, &(current_status.system_type));
+
+ /* disable manual override for all systems that rely on electronic stabilization */
+ if (current_status.system_type == MAV_TYPE_QUADROTOR ||
+ current_status.system_type == MAV_TYPE_HEXAROTOR ||
+ current_status.system_type == MAV_TYPE_OCTOROTOR) {
+ current_status.flag_external_manual_override_ok = false;
+ } else {
+ current_status.flag_external_manual_override_ok = true;
+ }
+ printf("param changed, val: %d, override: %s\n", current_status.system_type, (current_status.flag_external_manual_override_ok) ? "ON" : "OFF");
+ } else {
+ printf("ARMED, rejecting sys type change\n");
+ }
+ }
+
+ /* update global position estimate */
+ orb_check(global_position_sub, &new_data);
+ if (new_data) {
+ /* position changed */
+ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+ last_global_position_time = global_position.timestamp;
+ }
+
+ /* update local position estimate */
+ orb_check(local_position_sub, &new_data);
+ if (new_data) {
+ /* position changed */
+ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+ last_local_position_time = local_position.timestamp;
+ }
+
battery_voltage = sensors.battery_voltage_v;
battery_voltage_valid = sensors.battery_voltage_valid;
@@ -1406,6 +1462,32 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
+ bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
+ bool global_pos_valid = current_status.flag_global_position_valid;
+ bool local_pos_valid = current_status.flag_local_position_valid;
+
+ /* check for global or local position updates, set a timeout of 2s */
+ if (hrt_absolute_time() - last_global_position_time < 2000000) {
+ current_status.flag_vector_flight_mode_ok = true;
+ current_status.flag_global_position_valid = true;
+ // XXX check for controller status and home position as well
+ }
+
+ if (hrt_absolute_time() - last_local_position_time < 2000000) {
+ current_status.flag_vector_flight_mode_ok = true;
+ current_status.flag_local_position_valid = true;
+ // XXX check for controller status and home position as well
+ }
+
+ /* consolidate state change, flag as changed if required */
+ if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
+ global_pos_valid != current_status.flag_global_position_valid ||
+ local_pos_valid != current_status.flag_local_position_valid) {
+ state_changed = true;
+ }
+
+
+
/* Check if last transition deserved an audio event */
// #warning This code depends on state that is no longer? maintained
// #if 0
@@ -1669,7 +1751,7 @@ int commander_thread_main(int argc, char *argv[])
buzzer_deinit();
close(sp_man_sub);
close(sp_offboard_sub);
- close(gps_sub);
+ close(global_position_sub);
close(sensor_sub);
close(cmd_sub);
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 61dd418f8..d5c50fbf0 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -331,8 +331,16 @@ PX4FMU::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
- if (update_rate_in_ms < 2)
+ /* reject faster than 500 Hz updates */
+ if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
+ _update_rate = 500;
+ }
+ /* reject slower than 50 Hz updates */
+ if (update_rate_in_ms > 20) {
+ update_rate_in_ms = 20;
+ _update_rate = 50;
+ }
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 456564ba7..868d53cfd 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -55,6 +55,7 @@
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
+#include <math.h>
#include <arch/board/board.h>
@@ -72,6 +73,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rc_channels.h>
#include <px4io/protocol.h>
@@ -88,8 +90,17 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ /**
+ * Set the PWM via serial update rate
+ * @warning this directly affects CPU load
+ */
+ int set_pwm_rate(int hz);
+
+ bool dump_one;
+
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
+ int _update_rate; ///< serial send rate in Hz
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@@ -103,6 +114,8 @@ private:
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
+ int _t_vstatus; ///< system / vehicle status
+ vehicle_status_s _vstatus; ///< overall system state
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
@@ -175,6 +188,8 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
+ dump_one(false),
+ _update_rate(50),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
@@ -182,6 +197,7 @@ PX4IO::PX4IO() :
_connected(false),
_t_actuators(-1),
_t_armed(-1),
+ _t_vstatus(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
@@ -256,6 +272,17 @@ PX4IO::init()
return OK;
}
+int
+PX4IO::set_pwm_rate(int hz)
+{
+ if (hz > 0 && hz <= 400) {
+ _update_rate = hz;
+ return OK;
+ } else {
+ return -EINVAL;
+ }
+}
+
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@@ -266,6 +293,7 @@ void
PX4IO::task_main()
{
log("starting");
+ int update_rate_in_ms;
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -303,12 +331,20 @@ PX4IO::task_main()
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
- //int update_rate_in_ms = int(1000 / _update_rate);
- orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
+ update_rate_in_ms = int(1000 / _update_rate);
+ orb_set_interval(_t_actuators, update_rate_in_ms);
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
+ orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
+
+ /* advertise the limited control inputs */
+ memset(&_controls_effective, 0, sizeof(_controls_effective));
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
+ &_controls_effective);
+
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@@ -319,13 +355,15 @@ PX4IO::task_main()
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* poll descriptor */
- pollfd fds[3];
+ pollfd fds[4];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
+ fds[3].fd = _t_vstatus;
+ fds[3].events = POLLIN;
log("ready");
@@ -354,7 +392,8 @@ PX4IO::task_main()
if (fds[1].revents & POLLIN) {
/* get controls */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+ ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
@@ -362,8 +401,19 @@ PX4IO::task_main()
_mixers->mix(&_outputs.output[0], _max_actuators);
/* convert to PWM values */
- for (unsigned i = 0; i < _max_actuators; i++)
- _outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) {
+ _outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ _outputs.output[i] = 900;
+ }
+ }
/* and flag for update */
_send_needed = true;
@@ -375,6 +425,11 @@ PX4IO::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
+
+ if (fds[3].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
+ _send_needed = true;
+ }
}
/* send an update to IO if required */
@@ -499,8 +554,14 @@ PX4IO::io_send()
// XXX relays
- /* armed and not locked down */
+ /* armed and not locked down -> arming ok */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
+ /* indicate that full autonomous position control / vector flight mode is available */
+ cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
+ /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
+ cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
+ /* set desired PWM output rate */
+ cmd.servo_rate = _update_rate;
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
@@ -665,6 +726,29 @@ test(void)
exit(0);
}
+void
+monitor(void)
+{
+ unsigned cancels = 3;
+ printf("Hit <enter> three times to exit monitor mode\n");
+
+ for (;;) {
+ pollfd fds[1];
+
+ fds[0].fd = 0;
+ fds[0].events = POLLIN;
+ poll(fds, 1, 500);
+
+ if (fds[0].revents == POLLIN) {
+ int c;
+ read(0, &c, 1);
+ if (cancels-- == 0)
+ exit(0);
+ }
+
+ if (g_dev != nullptr)
+ g_dev->dump_one = true;
+ }
}
int
@@ -686,6 +770,16 @@ px4io_main(int argc, char *argv[])
errx(1, "driver init failed");
}
+ /* look for the optional pwm update rate for the supported modes */
+ if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+ if (argc > 2 + 1) {
+ g_dev->set_pwm_rate(atoi(argv[2 + 1]));
+ } else {
+ fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+ return 1;
+ }
+ }
+
exit(0);
}
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 38f58ac33..479ffd9a3 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -182,15 +182,45 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
- /* Control */
+ /* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
- /* Attitude Control */
- fixedwing_att_control_attitude(&att_sp,
- &att,
- &rates_sp);
+ /* attitude control */
+ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+ /* publish rate setpoint */
+ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
- /* Attitude Rate Control */
+ /* angular rate control */
+ fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+ /* pass through throttle */
+ actuators.control[3] = att_sp.thrust;
+
+ } else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+
+ /* if the RC signal is lost, try to stay level and go slowly back down to ground */
+ if(vstatus.rc_signal_lost) {
+
+ // XXX define failsafe throttle param
+ //param_get(failsafe_throttle_handle, &failsafe_throttle);
+ att_sp.roll_body = 0.3f;
+ att_sp.pitch_body = 0.0f;
+ att_sp.thrust = 0.5f;
+
+ // XXX disable yaw control, loiter
+
+ } else {
+
+ att_sp.roll_body = manual_sp.roll;
+ att_sp.pitch_body = manual_sp.pitch;
+ att_sp.yaw_body = 0;
+ att_sp.thrust = manual_sp.throttle;
+ att_sp.timestamp = hrt_absolute_time();
+ }
+
+ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+ /* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX
@@ -207,8 +237,17 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
actuators.control[3] = manual_sp.throttle;
}
- /* publish output */
- orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+ /* publish rates */
+ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
+
+ /* sanity check and publish actuator outputs */
+ if (isfinite(actuators.control[0]) &&
+ isfinite(actuators.control[1]) &&
+ isfinite(actuators.control[2]) &&
+ isfinite(actuators.control[3]))
+ {
+ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+ }
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index dd011aeed..a8894f059 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg)
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX;
- offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
- offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+ offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
+ offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
ml_armed = false;
@@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg)
mavlink_hil_state_t hil_state;
mavlink_msg_hil_state_decode(msg, &hil_state);
+ /* Calculate Rotation Matrix */
+ //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
+
+ if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
+ //TODO: assuming low pitch and roll values for now
+ hil_attitude.R[0][0] = cosf(hil_state.yaw);
+ hil_attitude.R[0][1] = sinf(hil_state.yaw);
+ hil_attitude.R[0][2] = 0.0f;
+
+ hil_attitude.R[1][0] = -sinf(hil_state.yaw);
+ hil_attitude.R[1][1] = cosf(hil_state.yaw);
+ hil_attitude.R[1][2] = 0.0f;
+
+ hil_attitude.R[2][0] = 0.0f;
+ hil_attitude.R[2][1] = 0.0f;
+ hil_attitude.R[2][2] = 1.0f;
+
+ hil_attitude.R_valid = true;
+ }
+
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..c522325dd 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -130,15 +130,10 @@ comms_main(void)
last_report_time = now;
/* populate the report */
- for (int i = 0; i < system_state.rc_channels; i++)
+ for (unsigned i = 0; i < system_state.rc_channels; i++) {
report.rc_channel[i] = system_state.rc_channel_data[i];
-
- if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
- report.channel_count = system_state.rc_channels;
- } else {
- report.channel_count = 0;
}
-
+ report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
/* and send it */
@@ -174,26 +169,41 @@ comms_handle_command(const void *buffer, size_t length)
irqstate_t flags = irqsave();
/* fetch new PWM output values */
- for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+ for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
system_state.fmu_channel_data[i] = cmd->servo_command[i];
+ }
- /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
- if(system_state.arm_ok && !cmd->arm_ok) {
+ /* if IO is armed and FMU gets disarmed, IO must also disarm */
+ if (system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
- system_state.mixer_use_fmu = true;
+ system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
+ system_state.manual_override_ok = cmd->manual_override_ok;
+ system_state.mixer_fmu_available = true;
system_state.fmu_data_received = true;
+ /* set PWM update rate if changed (after limiting) */
+ uint16_t new_servo_rate = cmd->servo_rate;
- /* handle changes signalled by FMU */
-// if (!system_state.arm_ok && system_state.armed)
-// system_state.armed = false;
+ /* reject faster than 500 Hz updates */
+ if (new_servo_rate > 500) {
+ new_servo_rate = 500;
+ }
+ /* reject slower than 50 Hz updates */
+ if (new_servo_rate < 50) {
+ new_servo_rate = 50;
+ }
+ if (system_state.servo_rate != new_servo_rate) {
+ up_pwm_servo_set_rate(new_servo_rate);
+ system_state.servo_rate = new_servo_rate;
+ }
/* XXX do relay changes here */
- for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
system_state.relays[i] = cmd->relay_state[i];
+ }
irqrestore(flags);
}
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index d4eace3df..03c8182b2 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -78,7 +78,43 @@ controls_main(void)
if (fds[0].revents & POLLIN)
dsm_input();
if (fds[1].revents & POLLIN)
+<<<<<<< Updated upstream
sbus_input();
+=======
+ locked |= sbus_input();
+
+ /*
+ * If we don't have lock from one of the serial receivers,
+ * look for PPM. It shares an input with S.bus, so there's
+ * a possibility it will mis-parse an S.bus frame.
+ *
+ * XXX each S.bus frame will cause a PPM decoder interrupt
+ * storm (lots of edges). It might be sensible to actually
+ * disable the PPM decoder completely if we have an alternate
+ * receiver lock.
+ */
+ if (!locked)
+ ppm_input();
+
+ /* check for manual override status */
+ if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
+ /* force manual input override */
+ system_state.mixer_manual_override = true;
+ } else {
+ /* override not engaged, use FMU */
+ system_state.mixer_manual_override = false;
+ }
+
+ /*
+ * If we haven't seen any new control data in 200ms, assume we
+ * have lost input and tell FMU.
+ */
+ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+
+ /* set the number of channels to zero - no inputs */
+ system_state.rc_channels = 0;
+ system_state.rc_lost = true;
+>>>>>>> Stashed changes
/* XXX do ppm processing, bypass mode, etc. here */
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 483e9fe4d..4e0aecdd7 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -97,7 +97,11 @@ mixer_tick(void)
/*
* Decide which set of inputs we're using.
*/
+<<<<<<< Updated upstream
if (system_state.mixer_use_fmu) {
+=======
+ if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
+>>>>>>> Stashed changes
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
@@ -108,20 +112,24 @@ mixer_tick(void)
/* too many frames without FMU input, time to go to failsafe */
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
- system_state.mixer_use_fmu = false;
+ system_state.mixer_manual_override = true;
+ system_state.mixer_fmu_available = false;
}
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
}
- } else if (system_state.rc_channels > 0) {
+ } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) {
/* we have control data from an R/C input */
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
-
} else {
/* we have no control input */
+<<<<<<< Updated upstream
+=======
+ // XXX builtin failsafe would activate here
+>>>>>>> Stashed changes
control_count = 0;
}
/*
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..0bf6d4997 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -52,9 +52,12 @@ struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
- uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
- bool relay_state[PX4IO_RELAY_CHANNELS];
- bool arm_ok;
+ uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */
+ uint16_t servo_rate; /**< PWM output rate in Hz */
+ bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
+ bool arm_ok; /**< FMU allows full arming */
+ bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
+ bool manual_override_ok; /**< if true, IO performs a direct manual override */
};
/* config message from FMU to IO */
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 77524797f..0eed5b011 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -60,6 +60,8 @@ int user_start(int argc, char *argv[])
{
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
+ /* default to 50 Hz PWM outputs */
+ system_state.servo_rate = 50;
/* configure the high-resolution time/callout interface */
hrt_init();
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 483b9bcc8..8a6cb48a9 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -69,8 +69,14 @@
struct sys_state_s
{
+<<<<<<< Updated upstream
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
+=======
+ bool armed; /* IO armed */
+ bool arm_ok; /* FMU says OK to arm */
+ uint16_t servo_rate; /* output rate of servos in Hz */
+>>>>>>> Stashed changes
bool ppm_input_ok; /* valid PPM input data */
bool dsm_input_ok; /* valid Spektrum DSM data */
@@ -96,7 +102,7 @@ struct sys_state_s
/*
* If true, we are using the FMU controls.
*/
- bool mixer_use_fmu;
+ bool mixer_manual_override;
/*
* If true, state that should be reported to FMU has been updated.
@@ -113,6 +119,29 @@ struct sys_state_s
* in the config packet.
*/
uint8_t serial_rx_mode;
+<<<<<<< Updated upstream
+=======
+
+ /**
+ * If true, the RC signal has been lost for more than a timeout interval
+ */
+ bool rc_lost;
+
+ /**
+ * If true, the connection to FMU has been lost for more than a timeout interval
+ */
+ bool fmu_lost;
+
+ /**
+ * If true, FMU is ready for autonomous position control. Only used for LED indication
+ */
+ bool vector_flight_mode_ok;
+
+ /**
+ * If true, IO performs an on-board manual override with the RC channel values
+ */
+ bool manual_override_ok;
+>>>>>>> Stashed changes
};
extern struct sys_state_s system_state;
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 60d20905a..52facfb61 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -63,10 +63,11 @@ static unsigned counter;
/*
* Define the various LED flash sequences for each system state.
*/
-#define LED_PATTERN_SAFE 0xffff // always on
-#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking
-#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking
-#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink
+#define LED_PATTERN_SAFE 0xffff /**< always on */
+#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0x3000 /**< always on with short break */
+#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
+#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
+#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
static unsigned blink_counter = 0;
@@ -139,6 +140,8 @@ safety_check_button(void *arg)
}
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
+ } else if (system_state.vector_flight_mode_ok) {
+ pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */
@@ -165,7 +168,7 @@ failsafe_blink(void *arg)
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
- if (!system_state.mixer_use_fmu) {
+ if (!system_state.mixer_fmu_available) {
failsafe = !failsafe;
} else {
failsafe = false;
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 23172d7cf..d69befe96 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -116,6 +116,7 @@ struct vehicle_status_s
commander_state_machine_t state_machine; /**< current flight state, main state machine */
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
+ int32_t system_type; /**< system type, inspired by MAVLinks MAV_TYPE enum */
// uint8_t mode;
@@ -165,8 +166,10 @@ struct vehicle_status_s
uint16_t errors_count4;
// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */
- bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-
+ bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+ bool flag_local_position_valid;
+ bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
+ bool flag_external_manual_override_ok;
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-04 23:46:26 +0000