diff options
30 files changed, 1037 insertions, 1089 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7277e9fa4..52412e20a 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 @@ -1496,14 +1578,20 @@ int commander_thread_main(int argc, char *argv[]) } //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + if (sp_man.aux1_cam_pan_flaps > STICK_ON_OFF_LIMIT) { update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + } else { + if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + //update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + // XXX hack + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } else { + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } } /* handle the case where RC signal was regained */ @@ -1669,7 +1757,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/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 46793c89b..aa916dafa 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -565,6 +565,28 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ { printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.") + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { @@ -587,19 +609,6 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ } } - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.") - } - /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 61dd418f8..dac0b5e84 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,6 +64,7 @@ #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> @@ -96,6 +97,7 @@ private: int _t_actuators; int _t_armed; orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,6 +163,7 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_armed(-1), _t_outputs(0), + _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -315,6 +318,13 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -331,8 +341,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; @@ -360,6 +378,11 @@ PX4FMU::task_main() /* do mixing */ _mixers->mix(&outputs.output[0], num_outputs); + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { @@ -371,7 +394,7 @@ PX4FMU::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } } @@ -388,6 +411,7 @@ PX4FMU::task_main() } ::close(_t_actuators); + ::close(_t_actuators_effective); ::close(_t_armed); /* make sure servos are off */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba7..1fb65413a 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> @@ -71,7 +72,9 @@ #include <systemlib/systemlib.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.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 +91,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 @@ -101,8 +113,13 @@ private: int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + 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,13 +192,17 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), + _update_rate(50), _serial_fd(-1), _io_stream(nullptr), _task(-1), _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), + _t_vstatus(-1), _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), @@ -256,6 +277,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 +298,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 +336,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 +360,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,16 +397,34 @@ 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) { /* XXX is this the right count? */ _mixers->mix(&_outputs.output[0], _max_actuators); + // XXX output actual limited values + memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); + + /* 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 +436,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 */ @@ -478,6 +544,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) _send_needed = true; + /* if monitoring, dump the received info */ + if (dump_one) { + dump_one = false; + + printf("IO: %s armed ", rep->armed ? "" : "not"); + for (unsigned i = 0; i < rep->channel_count; i++) + printf("%d: %d ", i, rep->rc_channel[i]); + printf("\n"); + } + out: unlock(); } @@ -495,12 +571,18 @@ PX4IO::io_send() cmd.servo_command[i] = _outputs.output[i]; /* publish as we send */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); // 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 +747,31 @@ 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 +793,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); } @@ -740,8 +857,11 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_sbus") || !strcmp(argv[1], "rx_ppm")) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + if (!strcmp(argv[1], "test")) test(); + if (!strcmp(argv[1], "monitor")) + monitor(); - errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'"); } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 334b95226..957036b41 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -113,6 +113,7 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp) { static int counter = 0; @@ -145,12 +146,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; - rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); + + /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ + float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); + /* set pitch plus feedforward roll compensation */ + rates_sp->pitch = pid_calculate(&pitch_controller, + att_sp->pitch_body + pitch_sp_rollcompensation, + att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ - //TODO + rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) + / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); + +// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/apps/fixedwing_att_control/fixedwing_att_control_att.h index ca7c14b43..11c932800 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.h +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.h @@ -41,9 +41,11 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_global_position.h> int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp); #endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 38f58ac33..3e1fc4952 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -58,40 +58,16 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> - #include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_att.h> -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - /* Prototypes */ /** * Deamon management function. @@ -126,7 +102,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing att control] started\n"); /* declare and safely initialize all structs */ struct vehicle_attitude_s att; @@ -135,14 +111,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&att_sp, 0, sizeof(att_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct manual_control_setpoint_s manual_sp; memset(&manual_sp, 0, sizeof(manual_sp)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); -// static struct debug_key_value_s debug_output; -// memset(&debug_output, 0, sizeof(debug_output)); - /* output structs */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -153,18 +128,18 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); -// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); -// debug_output.key[0] = '1'; - + orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); /* subscribe */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; + float speed_body[3] = {0.0f, 0.0f, 0.0f}; struct pollfd fds = { .fd = att_sub, .events = POLLIN }; while(!thread_should_exit) @@ -172,9 +147,35 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* wait for a sensor update, check for exit condition every 500 ms */ poll(&fds, 1, 500); + /* Check if there is a new position measurement or attitude setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool att_sp_updated; + orb_check(att_sp_sub, &att_sp_updated); + /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + if(att_sp_updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + if(pos_updated) + { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + if(att.R_valid) + { + speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; + speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; + speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; + } + else + { + speed_body[0] = 0; + speed_body[1] = 0; + speed_body[2] = 0; + + printf("FW ATT CONTROL: Did not get a valid R\n"); + } + } + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); @@ -182,33 +183,69 @@ 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); - /* Attitude Rate Control */ + /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - //REMOVEME XXX - actuators.control[3] = 0.7f; + /* 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(); + } + + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; - // debug_output.value = rates_sp.pitch; - // orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { /* directly pass through values */ actuators.control[0] = manual_sp.roll; /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = -manual_sp.pitch; + actuators.control[1] = manual_sp.pitch; actuators.control[2] = manual_sp.yaw; 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"); @@ -224,6 +261,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) close(att_sub); close(actuator_pub); + close(rates_pub); fflush(stdout); exit(0); @@ -268,7 +306,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_att_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index b81d50168..f3e36c0ec 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,8 @@ /** * @file fixedwing_att_control_rate.c * Implementation of a fixed wing attitude controller. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include <fixedwing_att_control_rate.h> @@ -59,9 +61,33 @@ #include <systemlib/geo/geo.h> #include <systemlib/systemlib.h> - - - +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* feedforward compensation */ +PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ struct fw_rate_control_params { float rollrate_p; @@ -73,7 +99,7 @@ struct fw_rate_control_params { float yawrate_p; float yawrate_i; float yawrate_awu; - + float pitch_thr_ff; }; struct fw_rate_control_param_handles { @@ -86,7 +112,7 @@ struct fw_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_awu; - + param_t pitch_thr_ff; }; @@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); return OK; } @@ -124,7 +151,7 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_awu, &(p->yawrate_awu)); - + param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); return OK; } @@ -167,12 +194,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } - /* Roll Rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); - - - actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... - actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now + /* roll rate (PI) */ + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + /* pitch rate (PI) */ + actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ + actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; + /* yaw rate (PI) */ + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); counter++; diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile deleted file mode 100644 index 02d4463dd..000000000 --- a/apps/fixedwing_control/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# fixedwing_control Application -# - -APPNAME = fixedwing_control -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -CSRCS = fixedwing_control.c - -include $(APPDIR)/mk/app.mk diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c deleted file mode 100644 index e04033481..000000000 --- a/apps/fixedwing_control/fixedwing_control.c +++ /dev/null @@ -1,566 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov <oivan@ethz.ch> - * Modifications: Doug Weibel <douglas.weibel@colorado.edu> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fixedwing_control.c - * Implementation of a fixed wing attitude and position controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> -#include <uORB/topics/debug_key_value.h> - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - -struct fw_att_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float rollrate_lim; - float roll_p; - float roll_lim; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float pitchrate_lim; - float pitch_p; - float pitch_lim; -}; - -struct fw_att_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t rollrate_lim; - param_t roll_p; - param_t roll_lim; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t pitchrate_lim; - param_t pitch_p; - param_t pitch_lim; -}; - - -// TO_DO - Navigation control will be moved to a separate app -// Attitude control will just handle the inner angle and rate loops -// to control pitch and roll, and turn coordination via rudder and -// possibly throttle compensation for battery voltage sag. - -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); - -struct fw_pos_control_params { - float heading_p; - float heading_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t heading_lim; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int att_parameters_init(struct fw_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); - -/** - * Initialize all parameter handles and values - * - */ -static int pos_parameters_init(struct fw_pos_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - - -/** - * The fixed wing control main thread. - * - * The main loop executes continously and calculates the control - * response. - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -int fixedwing_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct vehicle_attitude_s att; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* subscribe to attitude, motor setpoints and system state */ - struct vehicle_global_position_s global_pos; - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_setpoint_s global_setpoint; - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - - uint64_t last_run = 0; - uint64_t last_run_pos = 0; - - bool global_sp_updated_set_once = false; - - struct fw_att_control_params p; - struct fw_att_control_param_handles h; - - struct fw_pos_control_params ppos; - struct fw_pos_control_param_handles hpos; - - /* initialize the pid controllers */ - att_parameters_init(&h); - att_parameters_update(&h, &p); - - pos_parameters_init(&hpos); - pos_parameters_update(&hpos, &ppos); - -// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere - PID_t roll_rate_controller; - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, - p.rollrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t roll_angle_controller; - pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, - p.roll_lim,PID_MODE_DERIVATIV_NONE); - - PID_t pitch_rate_controller; - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, - p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t pitch_angle_controller; - pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, - p.pitch_lim,PID_MODE_DERIVATIV_NONE); - - PID_t heading_controller; - pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit - - // XXX remove in production - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - // This is the top of the main loop - while(!thread_should_exit) { - - struct pollfd fds[1] = { - { .fd = att_sub, .events = POLLIN }, - }; - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); - } else { - - // FIXME SUBSCRIBE - if (loopcounter % 100 == 0) { - att_parameters_update(&h, &p); - pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, - p.rollrate_awu, p.rollrate_lim); - pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, - 0.0f, p.roll_lim); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, - p.pitchrate_awu, p.pitchrate_lim); - pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, - 0.0f, p.pitch_lim); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); -//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", -//p.rollrate_p, p.rollrate_i, p.rollrate_lim); - } - - /* if position updated, run position control loop */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - if (global_sp_updated) { - global_sp_updated_set_once = true; - } - /* checking has to happen before the read, as the read clears the changed flag */ - - /* get a local copy of system state */ - 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 */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - //orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - // XXX update to switch between external attitude reference and the - // attitude calculated here - - char name[10]; - - if (pos_updated) { - - /* get position */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (global_sp_updated_set_once) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - - /* calculate delta T */ - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* calculate bearing error */ - float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, - global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); - - /* shift error to prevent wrapping issues */ - float bearing_error = target_bearing - att.yaw; - - if (loopcounter % 2 == 0) { - sprintf(name, "hdng err1"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - if (bearing_error < M_PI_F) { - bearing_error += 2.0f * M_PI_F; - } - - if (bearing_error > M_PI_F) { - bearing_error -= 2.0f * M_PI_F; - } - - if (loopcounter % 2 != 0) { - sprintf(name, "hdng err2"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - /* calculate roll setpoint, do this artificially around zero */ - att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, - 0.0f, att.yawspeed, deltaT); - - /* limit roll angle output */ - if (att_sp.roll_body > ppos.heading_lim) { - att_sp.roll_body = ppos.heading_lim; - heading_controller.saturated = 1; - } - - if (att_sp.roll_body < -ppos.heading_lim) { - att_sp.roll_body = -ppos.heading_lim; - heading_controller.saturated = 1; - } - - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - - } else { - /* no setpoint, maintain level flight */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - } - - att_sp.thrust = 0.7f; - } - - /* calculate delta T */ - const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; - last_run_pos = hrt_absolute_time(); - - if (verbose && (loopcounter % 20 == 0)) { - printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); - } - - // actuator control[0] is aileron (or elevon roll control) - // Commanded roll rate from P controller on roll angle - float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, - att.roll, 0.0f, deltaTpos); - // actuator control from PI controller on roll rate - actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, - att.rollspeed, 0.0f, deltaTpos); - - // actuator control[1] is elevator (or elevon pitch control) - // Commanded pitch rate from P controller on pitch angle - float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, - att.pitch, 0.0f, deltaTpos); - // actuator control from PI controller on pitch rate - actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, - att.pitchspeed, 0.0f, deltaTpos); - - // actuator control[3] is throttle - actuators.control[3] = att_sp.thrust; - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - } - } - - printf("[fixedwing_control] exiting.\n"); - thread_running = false; - - return 0; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 4096, - fixedwing_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_control is running\n"); - } else { - printf("\tfixedwing_control not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int att_parameters_init(struct fw_att_control_param_handles *h) -{ - /* PID parameters */ - - h->rollrate_p = param_find("FW_ROLLRATE_P"); - h->rollrate_i = param_find("FW_ROLLRATE_I"); - h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); - h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); - h->roll_p = param_find("FW_ROLL_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitchrate_p = param_find("FW_PITCHRATE_P"); - h->pitchrate_i = param_find("FW_PITCHRATE_I"); - h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); - h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->roll_p, &(p->roll_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - -static int pos_parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->heading_lim = param_find("FW_HEADING_LIM"); - - return OK; -} - -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->heading_lim, &(p->heading_lim)); - - return OK; -} diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index a70b9bd30..fbd6138de 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -57,24 +57,31 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> /* * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - +PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ struct fw_pos_control_params { float heading_p; + float headingr_p; + float headingr_i; + float headingr_lim; float xtrack_p; float altitude_p; float roll_lim; @@ -83,11 +90,13 @@ struct fw_pos_control_params { struct fw_pos_control_param_handles { param_t heading_p; + param_t headingr_p; + param_t headingr_i; + param_t headingr_lim; param_t xtrack_p; param_t altitude_p; param_t roll_lim; param_t pitch_lim; - }; @@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - + h->heading_p = param_find("FW_HEAD_P"); + h->headingr_p = param_find("FW_HEADR_P"); + h->headingr_i = param_find("FW_HEADR_I"); + h->headingr_lim = param_find("FW_HEADR_LIM"); + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } @@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h) static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { param_get(h->heading_p, &(p->heading_p)); + param_get(h->headingr_p, &(p->headingr_p)); + param_get(h->headingr_i, &(p->headingr_i)); + param_get(h->headingr_lim, &(p->headingr_lim)); param_get(h->xtrack_p, &(p->xtrack_p)); param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); @@ -171,7 +185,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing pos control] started\n"); /* declare and safely initialize all structs */ struct vehicle_global_position_s global_pos; @@ -184,6 +198,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct crosstrack_error_s xtrack_err; memset(&xtrack_err, 0, sizeof(xtrack_err)); + struct parameter_update_s param_update; + memset(¶m_update, 0, sizeof(param_update)); /* output structs */ struct vehicle_attitude_setpoint_s attitude_setpoint; @@ -193,129 +209,196 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) attitude_setpoint.roll_body = 0.0f; attitude_setpoint.pitch_body = 0.0f; attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.thrust = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + struct pollfd fds[2] = { + { .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN } + }; bool global_sp_updated_set_once = false; float psi_track = 0.0f; - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - static int counter = 0; - static bool initialized = false; - - static struct fw_pos_control_params p; - static struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t altitude_controller; - - if(!initialized) - { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - - } - - /* Check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* Load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(pos_updated) - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - if(global_sp_updated) { - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", (double)psi_track); - } - - /* Control */ - - - /* Simple Horizontal Control */ - if(global_sp_updated_set_once) - { -// if (counter % 100 == 0) -// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - if(!(distance_res != OK || xtrack_err.past_end)) { + int counter = 0; - float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + struct fw_pos_control_params p; + struct fw_pos_control_param_handles h; - if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) - delta_psi_c = 60.0f*M_DEG_TO_RAD_F; + PID_t heading_controller; + PID_t heading_rate_controller; + PID_t offtrack_controller; + PID_t altitude_controller; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) - delta_psi_c = -60.0f*M_DEG_TO_RAD_F; + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value - float psi_c = psi_track + delta_psi_c; + /* error and performance monitoring */ + perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); + perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - float psi_e = psi_c - att.yaw; - - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); - - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - -// if (counter % 100 == 0) -// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { - if (counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + while(!thread_should_exit) + { + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(fw_err_perf); + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); + + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value } - } - - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + printf("next wp direction: %0.4f\n", (double)psi_track); + } + + /* Simple Horizontal Control */ + if(global_sp_updated_set_once) + { + // if (counter % 100 == 0) + // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + // XXX what is xtrack_err.past_end? + if(distance_res == OK /*&& !xtrack_err.past_end*/) { + + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + + float psi_c = psi_track + delta_psi_c; + float psi_e = psi_c - att.yaw; + + /* wrap difference back onto -pi..pi range */ + psi_e = _wrap_pi(psi_e); + + if (verbose) { + printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); + printf("delta_psi_c %.4f ", (double)delta_psi_c); + printf("psi_c %.4f ", (double)psi_c); + printf("att.yaw %.4f ", (double)att.yaw); + printf("psi_e %.4f ", (double)psi_e); + } + + /* calculate roll setpoint, do this artificially around zero */ + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following + float psi_rate_c = delta_psi_rate_c + psi_rate_track; + + /* limit turn rate */ + if(psi_rate_c > p.headingr_lim) { + psi_rate_c = p.headingr_lim; + } else if(psi_rate_c < -p.headingr_lim) { + psi_rate_c = -p.headingr_lim; + } + + float psi_rate_e = psi_rate_c - att.yawspeed; + + // XXX sanity check: Assume 10 m/s stall speed and no stall condition + float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + + if (ground_speed < 10.0f) { + ground_speed = 10.0f; + } + + float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g + + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + + if (verbose) { + printf("psi_rate_c %.4f ", (double)psi_rate_c); + printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); + printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); + } + + if (verbose && counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } else { + if (verbose && counter % 100 == 0) + printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + } + + /* Very simple Altitude Control */ + if(pos_updated) + { + + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + + } + + // XXX need speed control + attitude_setpoint.thrust = 0.7f; + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + + counter++; + + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - counter++; } printf("[fixedwing_pos_control] exiting.\n"); @@ -367,7 +450,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 81b907bcd..575b42b37 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -189,10 +189,6 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -231,11 +227,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) case SYSTEM_STATE_STABILIZED: *mavlink_state = MAV_STATE_ACTIVE; *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case SYSTEM_STATE_AUTO: *mavlink_state = MAV_STATE_ACTIVE; *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case SYSTEM_STATE_MISSION_ABORT: @@ -745,6 +744,7 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = true; while (thread_running) { usleep(200000); + printf("."); } warnx("terminated."); exit(0); diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 7cb1582b4..9d9b9914a 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0; */ void mavlink_pm_callback(void *arg, param_t param); -/** - * Save parameters to EEPROM. - * - * Stores the parameters to /eeprom/parameters - */ -static int mavlink_pm_save_eeprom(void); - -/** - * Load parameters from EEPROM. - * - * Loads the parameters from /eeprom/parameters - */ -static int mavlink_pm_load_eeprom(void); - void mavlink_pm_callback(void *arg, param_t param) { mavlink_pm_send_param(param); @@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } break; - - // case MAVLINK_MSG_ID_COMMAND_LONG: { - // mavlink_command_long_t cmd_mavlink; - // mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - // uint8_t result = MAV_RESULT_UNSUPPORTED; - - // if (cmd_mavlink.target_system == mavlink_system.sysid && - // ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - - // // XXX move this to LOW PRIO THREAD of commander app - - // /* preflight parameter load / store */ - // if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { - // /* Read all parameters from EEPROM to RAM */ - - // if (((int)(cmd_mavlink.param1)) == 0) { - - // /* read all parameters from EEPROM to RAM */ - // int read_ret = param_load_default(); - // if (read_ret == OK) { - // //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); - // mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - // } else if (read_ret == 1) { - // mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - // } else { - // if (read_ret < -1) { - // mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file()); - // } else { - // mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file()); - // } - // result = MAV_RESULT_FAILED; - // } - - // } else if (((int)(cmd_mavlink.param1)) == 1) { - - // /* write all parameters from RAM to EEPROM */ - // int write_ret = param_save_default(); - // if (write_ret == OK) { - // mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - - // } else { - // if (write_ret < -1) { - // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); - // } else { - // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); - // } - // result = MAV_RESULT_FAILED; - // } - - // } else { - // //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); - // mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request"); - // result = MAV_RESULT_UNSUPPORTED; - // } - // } - // } - - // /* send back command result */ - // //mavlink_msg_command_ack_send(chan, cmd.command, result); - // } break; } } diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed..58761e89c 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; @@ -305,6 +325,7 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; + /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index a067460d8..b0aa401fc 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -114,6 +114,7 @@ static void l_manual_control_setpoint(struct listener *l); static void l_vehicle_attitude_controls(struct listener *l); static void l_debug_key_value(struct listener *l); static void l_optical_flow(struct listener *l); +static void l_vehicle_rates_setpoint(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -136,6 +137,7 @@ struct listener listeners[] = { {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, + {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -409,6 +411,23 @@ l_attitude_setpoint(struct listener *l) } void +l_vehicle_rates_setpoint(struct listener *l) +{ + struct vehicle_rates_setpoint_s rates_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + rates_sp.timestamp/1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); +} + +void l_actuator_outputs(struct listener *l) { struct actuator_outputs_s act_outputs; @@ -547,28 +566,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_controls_effective_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); + "eff ctrl0 ", + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); + "eff ctrl1 ", + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); + "eff ctrl2 ", + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + "eff ctrl3 ", + actuators.control_effective[3]); } } @@ -695,6 +714,10 @@ uorb_receive_start(void) mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + /* --- ACTUATOR OUTPUTS --- */ mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); @@ -716,7 +739,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 61e664401..4650c4991 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -79,6 +80,7 @@ struct mavlink_subscriptions { int debug_key_value; int input_rc_sub; int optical_flow; + int rates_setpoint_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..0fcf952ab 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -100,8 +100,8 @@ comms_main(void) debug("FMU: ready"); for (;;) { - /* wait for serial data, but no more than 100ms */ - poll(&fds, 1, 100); + /* wait for serial data, but no more than 10ms */ + poll(&fds, 1, 10); /* * Pull bytes from FMU and feed them to the HX engine. @@ -130,15 +130,11 @@ 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 +170,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..9db9a0fa5 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,19 +55,27 @@ #include <drivers/drv_hrt.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> +#include <systemlib/ppm_decode.h> #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1600 +#define RC_CHANNEL_LOW_THRESH 1400 + +static void ppm_input(void); + void controls_main(void) { struct pollfd fds[2]; + /* DSM input */ fds[0].fd = dsm_init("/dev/ttyS0"); fds[0].events = POLLIN; - + /* S.bus input */ fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; @@ -75,14 +83,84 @@ controls_main(void) /* run this loop at ~100Hz */ poll(fds, 2, 10); + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + bool locked = false; + if (fds[0].revents & POLLIN) - dsm_input(); + locked |= dsm_input(); if (fds[1].revents & POLLIN) - 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; - /* XXX do ppm processing, bypass mode, etc. here */ + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } /* do PWM output updates */ mixer_tick(); } } + +static void +ppm_input(void) +{ + /* + * Look for new PPM input. + */ + if (ppm_last_valid_decode != 0) { + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* PPM data exists, copy it */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + + /* copy the timestamp and clear it */ + system_state.rc_channels_timestamp = ppm_last_valid_decode; + ppm_last_valid_decode = 0; + + irqrestore(state); + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 744dac3d6..2611f3a03 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -104,7 +104,7 @@ dsm_init(const char *device) return dsm_fd; } -void +bool dsm_input(void) { ssize_t ret; @@ -141,7 +141,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -153,7 +153,7 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -161,6 +161,12 @@ dsm_input(void) */ dsm_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } static bool @@ -275,10 +281,13 @@ dsm_decode(hrt_abstime frame_time) */ if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) dsm_guess_format(true); + + /* we have received something we think is a frame */ last_frame_time = frame_time; + + /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - system_state.dsm_input_ok = false; return; } @@ -293,10 +302,6 @@ dsm_decode(hrt_abstime frame_time) * seven channels are being transmitted. */ - const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS; - - uint16_t dsm_channels[dsm_chancount]; - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; @@ -311,31 +316,40 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel > ppm_decoded_channels) - ppm_decoded_channels = channel; + if (channel >= system_state.rc_channels) + system_state.rc_channels = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; - - /* stuff the decoded channel into the PPM input buffer */ - dsm_channels[channel] = 988 + value; + value += 998; + + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; + case 1: + channel = 0; + break; + case 2: + channel = 1; + default: + break; + } + system_state.rc_channel_data[channel] = value; } - /* DSM input is valid */ - system_state.dsm_input_ok = true; - - /* check if no S.BUS data is available */ - if (!system_state.sbus_input_ok) { + /* and note that we have received data from the R/C controller */ + /* XXX failsafe will cause problems here - need a strategy for detecting it */ + system_state.rc_channels_timestamp = frame_time; - for (unsigned i = 0; i < dsm_chancount; i++) { - system_state.rc_channel_data[i] = dsm_channels[i]; - } - - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - system_state.rc_channels = dsm_chancount; - system_state.fmu_report_due = true; - } + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d..5a644906b 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -50,8 +50,6 @@ #include <drivers/drv_pwm_output.h> -#include <systemlib/ppm_decode.h> - #include "px4io.h" /* @@ -59,10 +57,6 @@ */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Collect RC input data from the controller source(s). - */ -static void mixer_get_rc_input(void); /* * Update a mixer based on the current control signals. @@ -89,15 +83,9 @@ mixer_tick(void) bool should_arm; /* - * Start by looking for R/C control inputs. - * This updates system_state with any control inputs received. - */ - mixer_get_rc_input(); - - /* * Decide which set of inputs we're using. */ - if (system_state.mixer_use_fmu) { + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { /* we have recent control data from the FMU */ control_count = PX4IO_OUTPUT_CHANNELS; control_values = &system_state.fmu_channel_data[0]; @@ -108,22 +96,25 @@ 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 */ + + // XXX builtin failsafe would activate here control_count = 0; } + /* * Tickle each mixer, if we have control data. */ @@ -141,9 +132,10 @@ mixer_tick(void) /* * Decide whether the servos should be armed right now. + * A sufficient reason is armed state and either FMU or RC control inputs */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -166,30 +158,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) mixers[mixer].current_value = 0; } } - -static void -mixer_get_rc_input(void) -{ - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ - if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - - /* input was ok and timed out, mark as update */ - if (system_state.ppm_input_ok) { - system_state.ppm_input_ok = false; - system_state.fmu_report_due = true; - } - return; - } - - /* mark PPM as valid */ - system_state.ppm_input_ok = true; - - /* check if no DSM and S.BUS data is available */ - if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) { - /* otherwise, copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - system_state.fmu_report_due = true; - } -} 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..4f68b3d2d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include <debug.h> #include <stdlib.h> #include <errno.h> +#include <string.h> #include <nuttx/clock.h> @@ -60,6 +61,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..84f5ba029 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,50 +69,72 @@ struct sys_state_s { - 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 */ - bool ppm_input_ok; /* valid PPM input data */ - bool dsm_input_ok; /* valid Spektrum DSM data */ - bool sbus_input_ok; /* valid Futaba S.Bus data */ - - /* + /** * Data from the remote control input(s) */ - int rc_channels; + unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * If true, we are using the FMU controls, else RC input if available. + */ + bool mixer_manual_override; + + /** + * If true, FMU input is available. */ - bool mixer_use_fmu; + bool mixer_fmu_available; - /* + /** * If true, state that should be reported to FMU has been updated. */ bool fmu_report_due; - /* + /** * If true, new control data from the FMU has been received. */ bool fmu_data_received; - /* + /** * Current serial interface mode, per the serial_rx_mode parameter * in the config packet. */ uint8_t serial_rx_mode; + + /** + * 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; }; extern struct sys_state_s system_state; @@ -169,9 +191,9 @@ extern void comms_main(void) __attribute__((noreturn)); */ extern void controls_main(void); extern int dsm_init(const char *device); -extern void dsm_input(void); +extern bool dsm_input(void); extern int sbus_init(const char *device); -extern void sbus_input(void); +extern bool sbus_input(void); /* * Assertion codes diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 60d20905a..9ce4589b7 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,20 +58,21 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +static unsigned counter = 0; /* * 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; #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 2 +#define DISARM_COUNTER_THRESHOLD 4 static bool safety_button_pressed; @@ -101,10 +102,6 @@ safety_check_button(void *arg) */ safety_button_pressed = BUTTON_SAFETY; - if(safety_button_pressed) { - //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); - } - /* Keep pressed for a while to arm */ if (safety_button_pressed && !system_state.armed) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -139,6 +136,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 +164,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/px4io/sbus.c b/apps/px4io/sbus.c index c3949f2b0..a8f628a84 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -58,6 +58,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -94,7 +95,7 @@ sbus_init(const char *device) return sbus_fd; } -void +bool sbus_input(void) { ssize_t ret; @@ -131,7 +132,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -143,7 +144,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -151,6 +152,12 @@ sbus_input(void) */ sbus_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } /* @@ -195,17 +202,19 @@ sbus_decode(hrt_abstime frame_time) /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - system_state.sbus_input_ok = false; return; } - /* if the failsafe bit is set, we consider that a loss of RX signal */ + /* if the failsafe bit is set, we consider the frame invalid */ if (frame[23] & (1 << 4)) { - system_state.sbus_input_ok = false; return; } - unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + /* we have received something we think is a frame */ + last_frame_time = frame_time; + + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -228,14 +237,16 @@ sbus_decode(hrt_abstime frame_time) } if (PX4IO_INPUT_CHANNELS >= 18) { - /* decode two switch channels */ chancount = 18; + /* XXX decode the two switch channels */ } + /* note the number of channels decoded */ system_state.rc_channels = chancount; - system_state.sbus_input_ok = true; - system_state.fmu_report_due = true; /* and note that we have received data from the R/C controller */ system_state.rc_channels_timestamp = frame_time; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index cc74ae705..37ba0ec3e 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -122,6 +122,8 @@ PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA +PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */ + /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); @@ -136,5 +138,5 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 07a9015fe..b63bfb195 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -95,6 +95,12 @@ #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ +enum RC_DEMIX { + RC_DEMIX_NONE = 0, + RC_DEMIX_AUTO = 1, + RC_DEMIX_DELTA = 2 +}; + /** * Sensor app start / stop handling function * @@ -178,12 +184,18 @@ private: int rc_type; + int rc_demix; /**< enabled de-mixing of RC mixers */ + int rc_map_roll; int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; int rc_map_mode_sw; + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + float rc_scale_roll; float rc_scale_pitch; float rc_scale_yaw; @@ -200,6 +212,8 @@ private: param_t ex[_rc_max_chan_count]; param_t rc_type; + param_t rc_demix; + param_t gyro_offset[3]; param_t accel_offset[3]; param_t accel_scale[3]; @@ -212,6 +226,10 @@ private: param_t rc_map_throttle; param_t rc_map_mode_sw; + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_scale_roll; param_t rc_scale_pitch; param_t rc_scale_yaw; @@ -384,11 +402,16 @@ Sensors::Sensors() : _parameter_handles.rc_type = param_find("RC_TYPE"); + _parameter_handles.rc_demix = param_find("RC_DEMIX"); + _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); @@ -480,20 +503,21 @@ Sensors::parameters_update() _parameters.scaling_factor[i] = 0; } - } + /* handle wrong values */ + // XXX TODO - /* update RC function mappings */ - _rc.function[0] = _parameters.rc_map_throttle - 1; - _rc.function[1] = _parameters.rc_map_roll - 1; - _rc.function[2] = _parameters.rc_map_pitch - 1; - _rc.function[3] = _parameters.rc_map_yaw - 1; - _rc.function[4] = _parameters.rc_map_mode_sw - 1; + } /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { warnx("Failed getting remote control type"); } + /* de-mixing */ + if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) { + warnx("Failed getting demix setting"); + } + /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -510,6 +534,15 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx("Failed getting mode sw chan index"); } + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { + warnx("Failed getting mode aux 1 index"); + } + if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { + warnx("Failed getting mode aux 2 index"); + } + if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { + warnx("Failed getting mode aux 3 index"); + } if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { warnx("Failed getting rc scaling for roll"); @@ -521,6 +554,16 @@ Sensors::parameters_update() warnx("Failed getting rc scaling for yaw"); } + /* update RC function mappings */ + _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[ROLL] = _parameters.rc_map_roll - 1; + _rc.function[PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[YAW] = _parameters.rc_map_yaw - 1; + _rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1; + _rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1; + _rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1; + _rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1; + /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); @@ -879,7 +922,7 @@ Sensors::ppm_poll() */ if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned int i = 0; i < ppm_decoded_channels; i++) { raw.values[i] = ppm_buffer[i]; } @@ -953,35 +996,54 @@ Sensors::ppm_poll() manual_control.timestamp = rc_input.timestamp; - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + /* check if input needs to be de-mixed */ + if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) { + // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS + + /* roll input - mixed roll and pitch channels */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled; + /* pitch input - mixed roll and pitch channels */ + manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + + /* direct pass-through of manual input */ + } else { + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + } + + /* limit outputs */ if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { manual_control.roll *= _parameters.rc_scale_roll; } - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { manual_control.pitch *= _parameters.rc_scale_pitch; } - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { manual_control.yaw *= _parameters.rc_scale_yaw; } - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; @@ -990,6 +1052,21 @@ Sensors::ppm_poll() if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* aux functions */ + manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled; + if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f; + if (manual_control.aux1_cam_pan_flaps > 1.0f) manual_control.aux1_cam_pan_flaps = 1.0f; + + /* aux inputs */ + manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled; + if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f; + if (manual_control.aux2_cam_tilt > 1.0f) manual_control.aux2_cam_tilt = 1.0f; + + /* aux inputs */ + manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled; + if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f; + if (manual_control.aux3_cam_zoom > 1.0f) manual_control.aux3_cam_zoom = 1.0f; + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index aad2c4d9b..40278c56c 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_controls_effective.h * * Actuator control topics - mixer inputs. * diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 23172d7cf..738ca644f 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, VEHICLE_MODE_FLAG_TEST_ENABLED = 2, @@ -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; }; /** diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 0959687de..5e2a32a46 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -77,7 +77,6 @@ CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -#CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator |