diff options
-rw-r--r-- | apps/commander/commander.c | 94 | ||||
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 10 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 108 | ||||
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 55 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 24 | ||||
-rw-r--r-- | apps/px4io/comms.c | 40 | ||||
-rw-r--r-- | apps/px4io/controls.c | 36 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 14 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 9 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 2 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 31 | ||||
-rw-r--r-- | apps/px4io/safety.c | 13 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 7 |
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), ¤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); 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; }; /** |