diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 56ae63d271..ec4221b93a 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -29,6 +29,10 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \ $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \ + $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \ + $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \ + $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \ + $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \ $(SRCROOT)/logging/logconv.m~logging/logconv.m # diff --git a/ROMFS/mixers/FMU_hex_+.mix b/ROMFS/mixers/FMU_hex_+.mix new file mode 100644 index 0000000000..b5e38ce9ef --- /dev/null +++ b/ROMFS/mixers/FMU_hex_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the + configuration. All controls +are mixed 100%. + +R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_hex_x.mix b/ROMFS/mixers/FMU_hex_x.mix new file mode 100644 index 0000000000..8e8d122adc --- /dev/null +++ b/ROMFS/mixers/FMU_hex_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the X configuration. All controls +are mixed 100%. + +R: 6x 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_octo_+.mix b/ROMFS/mixers/FMU_octo_+.mix new file mode 100644 index 0000000000..2cb70e814c --- /dev/null +++ b/ROMFS/mixers/FMU_octo_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the + configuration. All controls +are mixed 100%. + +R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/mixers/FMU_octo_x.mix b/ROMFS/mixers/FMU_octo_x.mix new file mode 100644 index 0000000000..edc71f0139 --- /dev/null +++ b/ROMFS/mixers/FMU_octo_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the X configuration. All controls +are mixed 100%. + +R: 8x 10000 10000 10000 0 diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 72c7e34a13..cd8783673c 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -37,7 +37,6 @@ * * PX4IO is connected via serial (or possibly some other interface at a later * point). - */ #include @@ -384,7 +383,7 @@ PX4IO::task_main() if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); + orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } } @@ -538,7 +537,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case PWM_SERVO_DISARM: /* fake a disarmed transition */ - _armed.armed = true; + _armed.armed = false; _send_needed = true; break; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 29463d7447..6a83877416 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -68,10 +68,13 @@ #include #include +#include #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" +PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. + __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -142,6 +145,10 @@ mc_thread_main(int argc, char *argv[]) bool flag_system_armed = false; bool man_yaw_zero_once = false; + /* prepare the handle for the failsafe throttle */ + param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); + float failsafe_throttle = 0.0f; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -226,6 +233,17 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } att_sp.thrust = manual.throttle; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if(state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = failsafe_throttle; + } + att_sp.timestamp = hrt_absolute_time(); } /* STEP 2: publish the result to the vehicle actuators */ diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 3964d75037..f1dac433fb 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -239,13 +239,19 @@ comms_handle_command(const void *buffer, size_t length) 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) { + system_state.armed = false; + } + system_state.arm_ok = cmd->arm_ok; system_state.mixer_use_fmu = true; system_state.fmu_data_received = true; + /* handle changes signalled by FMU */ - if (!system_state.arm_ok && system_state.armed) - system_state.armed = false; +// if (!system_state.arm_ok && system_state.armed) +// system_state.armed = false; /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index b614f3fa44..6cf499c391 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -82,7 +82,7 @@ static void mixer_get_rc_input(void); static void mixer_update(int mixer, uint16_t *inputs, int input_count); /* current servo arm/disarm state */ -bool mixer_servos_armed; +bool mixer_servos_armed = false; /* * Each mixer consumes a set of inputs and produces a single output. @@ -159,7 +159,7 @@ mixer_tick(void *arg) /* * If we are armed, update the servo output. */ - if (system_state.armed) + if (system_state.armed && system_state.arm_ok) up_pwm_servo_set(i, mixers[i].current_value); } } @@ -167,7 +167,7 @@ mixer_tick(void *arg) /* * Decide whether the servos should be armed right now. */ - should_arm = system_state.armed && (control_count > 0); + 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); diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index e02d865d5f..dec2cdde67 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -133,8 +133,9 @@ int user_start(int argc, char *argv[]) /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r", + lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r", cursor[cycle++ & 3], + (system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"), (system_state.armed ? "ARMED" : "SAFE"), (system_state.rc_channels ? "RC OK" : "NO RC"), (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 536dbebf1f..f50e29252a 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -68,7 +68,7 @@ struct sys_state_s { - bool armed; /* actually armed */ + bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ /* diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index f274326643..0ad636c0b3 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -76,9 +76,9 @@ safety_check_button(void *arg) /* * Debounce the safety button, change state if it has been held for long enough. * - * Ignore the button if FMU has not said it's OK to arm yet. */ - if (BUTTON_SAFETY && system_state.arm_ok) { + + if (BUTTON_SAFETY) { if (arm_counter < ARM_COUNTER_THRESHOLD) { arm_counter++; } else if (arm_counter == ARM_COUNTER_THRESHOLD) { diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 0e714ed50a..52d79e3693 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space) float max = 0.0f; float fixup_scale; + /* use an output factor to prevent too strong control signals at low throttle */ + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float startpoint_full_control = 0.20f; + float output_factor; + + /* keep roll, pitch and yaw control to 0 below min thrust */ + if (thrust <= min_thrust) { + output_factor = 0.0f; + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust); + /* and then stay at full control */ + } else { + output_factor = max_thrust; + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + /* perform initial mix pass yielding un-bounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { float tmp = roll * _rotors[i].roll_scale +