forked from Archive/PX4-Autopilot
Added required additional fields: If system is ok to launch (required for LED indicator), if system is ok to override fully by RC (required for multirotors which should not support this), desired PWM output rate in Hz (again required for some multirotors).
This commit is contained in:
Lorenz Meier
parent
154035279f
commit
03076a72ca
|
|
@ -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), ¤t_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(¶m_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, ¤t_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, ¶m_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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue