aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.sensors9
-rw-r--r--src/modules/commander/gyro_calibration.cpp93
-rw-r--r--src/modules/commander/mag_calibration.cpp66
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp50
-rw-r--r--src/modules/mavlink/mavlink_receiver.h1
-rw-r--r--src/modules/navigator/rtl.cpp10
-rw-r--r--src/modules/sensors/sensors.cpp1
-rw-r--r--src/modules/systemlib/mixer/mixer_multirotor.cpp125
8 files changed, 246 insertions, 109 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index b8a704b90..474db36ef 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -3,8 +3,13 @@
# Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers.
#
-ms5611 start
-adc start
+if ms5611 start
+then
+fi
+
+if adc start
+then
+fi
if ver hwcmp PX4FMU_V2
then
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index bdef8771e..fc6398bd6 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -74,7 +74,7 @@ typedef struct {
struct gyro_report gyro_report_0;
} gyro_worker_data_t;
-static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
+static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
{
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
unsigned calibration_counter[max_gyros] = { 0 };
@@ -89,6 +89,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
}
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
+ memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
@@ -149,7 +150,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
int do_gyro_calibration(int mavlink_fd)
{
int res = OK;
- gyro_worker_data_t worker_data;
+ gyro_worker_data_t worker_data = {};
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@@ -196,51 +197,63 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
+
+ int cancel_sub = calibrate_cancel_subscribe();
+
+ unsigned try_count = 0;
+ unsigned max_tries = 20;
+ res = ERROR;
- // Calibrate right-side up
-
- bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
-
- int cancel_sub = calibrate_cancel_subscribe();
- calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
- cancel_sub, // Subscription to vehicle_command for cancel support
- side_collected, // Sides to calibrate
- gyro_calibration_worker, // Calibration worker
- &worker_data, // Opaque data for calibration worked
- true); // true: lenient still detection
+ do {
+ // Calibrate gyro and ensure user didn't move
+ calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
+
+ if (cal_return == calibrate_return_cancelled) {
+ // Cancel message already sent, we are done here
+ res = ERROR;
+ break;
+
+ } else if (cal_return == calibrate_return_error) {
+ res = ERROR;
+
+ } else {
+ /* check offsets */
+ float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
+ float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
+ float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
+
+ /* maximum allowable calibration error in radians */
+ const float maxoff = 0.0055f;
+
+ if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
+ !isfinite(worker_data.gyro_scale[0].y_offset) ||
+ !isfinite(worker_data.gyro_scale[0].z_offset) ||
+ fabsf(xdiff) > maxoff ||
+ fabsf(ydiff) > maxoff ||
+ fabsf(zdiff) > maxoff) {
+
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
+ res = ERROR;
+
+ } else {
+ res = OK;
+ }
+ }
+ try_count++;
+
+ } while (res == ERROR && try_count <= max_tries);
+
+ if (try_count >= max_tries) {
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
+ res = ERROR;
+ }
+
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
close(worker_data.gyro_sensor_sub[s]);
}
- if (cal_return == calibrate_return_cancelled) {
- // Cancel message already sent, we are done here
- return ERROR;
- } else if (cal_return == calibrate_return_error) {
- res = ERROR;
- }
-
- if (res == OK) {
- /* check offsets */
- float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
- float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
- float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
-
- /* maximum allowable calibration error in radians */
- const float maxoff = 0.0055f;
-
- if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
- !isfinite(worker_data.gyro_scale[0].y_offset) ||
- !isfinite(worker_data.gyro_scale[0].z_offset) ||
- fabsf(xdiff) > maxoff ||
- fabsf(ydiff) > maxoff ||
- fabsf(zdiff) > maxoff) {
- mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
- res = ERROR;
- }
- }
-
if (res == OK) {
/* set offset parameters to new values */
bool failed = false;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 8a8d85818..1a8de86c4 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -50,6 +50,7 @@
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
@@ -193,7 +194,70 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
- sleep(2);
+
+ /*
+ * Detect if the system is rotating.
+ *
+ * We're detecting this as a general rotation on any axis, not necessary on the one we
+ * asked the user for. This is because we really just need two roughly orthogonal axes
+ * for a good result, so we're not constraining the user more than we have to.
+ */
+
+ hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+ hrt_abstime last_gyro = 0;
+ float gyro_x_integral = 0.0f;
+ float gyro_y_integral = 0.0f;
+ float gyro_z_integral = 0.0f;
+
+ const float gyro_int_thresh_rad = 1.0f;
+
+ int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+
+ while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
+ fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
+ fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
+
+ /* abort on request */
+ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+ result = calibrate_return_cancelled;
+ break;
+ }
+
+ /* abort with timeout */
+ if (hrt_absolute_time() > detection_deadline) {
+ result = calibrate_return_error;
+ warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
+ mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
+ break;
+ }
+
+ /* Wait clocking for new data on all gyro */
+ struct pollfd fds[1];
+ fds[0].fd = sub_gyro;
+ fds[0].events = POLLIN;
+ size_t fd_count = 1;
+
+ int poll_ret = poll(fds, fd_count, 1000);
+
+ if (poll_ret > 0) {
+ struct gyro_report gyro;
+ orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
+
+ /* ensure we have a valid first timestamp */
+ if (last_gyro > 0) {
+
+ /* integrate */
+ float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
+ gyro_x_integral += gyro.x * delta_t;
+ gyro_y_integral += gyro.y * delta_t;
+ gyro_z_integral += gyro.z * delta_t;
+ }
+
+ last_gyro = gyro.timestamp;
+ }
+ }
+
+ close(sub_gyro);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
unsigned poll_errcount = 0;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4d96f389d..0cd163a0c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -129,6 +129,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_alt0(0.0f),
_hil_local_proj_ref{},
_offboard_control_mode{},
+ _att_sp{},
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0)
@@ -778,16 +779,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
* throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
* body rates to keep the controllers running
*/
- bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
- bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+ bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7);
+ bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
- if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) {
+ if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
- _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate;
- _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
+ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
+ _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
} else {
- _offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
- _offboard_control_mode.ignore_attitude = ignore_attitude;
+ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
+ _offboard_control_mode.ignore_attitude = ignore_attitude_msg;
}
_offboard_control_mode.ignore_position = true;
@@ -816,22 +817,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(_offboard_control_mode.ignore_attitude)) {
- struct vehicle_attitude_setpoint_s att_sp = {};
- att_sp.timestamp = hrt_absolute_time();
- mavlink_quaternion_to_euler(set_attitude_target.q,
- &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
- mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
- att_sp.R_valid = true;
+ _att_sp.timestamp = hrt_absolute_time();
+ if (!ignore_attitude_msg) { // only copy att sp if message contained new data
+ mavlink_quaternion_to_euler(set_attitude_target.q,
+ &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body);
+ mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body);
+ _att_sp.R_valid = true;
+ _att_sp.yaw_sp_move_rate = 0.0;
+ memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d));
+ _att_sp.q_d_valid = true;
+ }
+
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
- att_sp.thrust = set_attitude_target.thrust;
+ _att_sp.thrust = set_attitude_target.thrust;
}
- att_sp.yaw_sp_move_rate = 0.0;
- memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
- att_sp.q_d_valid = true;
+
if (_att_sp_pub < 0) {
- _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
} else {
- orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
}
}
@@ -839,9 +843,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
///XXX add support for ignoring individual axes
if (!(_offboard_control_mode.ignore_bodyrate)) {
_rates_sp.timestamp = hrt_absolute_time();
- _rates_sp.roll = set_attitude_target.body_roll_rate;
- _rates_sp.pitch = set_attitude_target.body_pitch_rate;
- _rates_sp.yaw = set_attitude_target.body_yaw_rate;
+ if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
+ _rates_sp.roll = set_attitude_target.body_roll_rate;
+ _rates_sp.pitch = set_attitude_target.body_pitch_rate;
+ _rates_sp.yaw = set_attitude_target.body_yaw_rate;
+ }
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
_rates_sp.thrust = set_attitude_target.thrust;
}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 2b6174f8f..887d2f88e 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -187,6 +187,7 @@ private:
float _hil_local_alt0;
struct map_projection_reference_s _hil_local_proj_ref;
struct offboard_control_mode_s _offboard_control_mode;
+ struct vehicle_attitude_setpoint_s _att_sp;
struct vehicle_rates_setpoint_s _rates_sp;
double _time_offset_avg_alpha;
uint64_t _time_offset;
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1ed8cb7c..b75b7fa22 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -103,6 +103,7 @@ RTL::on_activation()
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
+
}
set_rtl_item();
@@ -146,7 +147,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
- mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home",
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
+ (int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
@@ -177,7 +179,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
- mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home",
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
+ (int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
@@ -197,7 +200,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
- mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home",
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
+ (int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4fbc0416e..153e70480 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -633,6 +633,7 @@ Sensors::Sensors() :
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("SYS_PARAM_VER");
+ (void)param_find("SYS_AUTOSTART");
/* fetch initial parameter values */
parameters_update();
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index e9a8a87ca..4d9d60c2d 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
unsigned
MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
{
+ /* Summary of mixing strategy:
+ 1) mix roll, pitch and thrust without yaw.
+ 2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
+ increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
+ (max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
+ In case there is violation at the lower and upper bound then try to shift such that violation is equal
+ on both sides.
+ 3) mix in yaw and scale if it leads to limit violation.
+ 4) scale all outputs to range [idle_speed,1]
+ */
+
float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
- //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = constrain(get_control(0, 3), 0.0f, 1.0f);
- //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float min_out = 0.0f;
float max_out = 0.0f;
+ // clean register for saturation status flags
if (status_reg != NULL) {
(*status_reg) = 0;
}
+ // thrust boost parameters
+ float thrust_increase_factor = 1.5f;
+ float thrust_decrease_factor = 0.6f;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
@@ -221,14 +234,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
out *= _rotors[i].out_scale;
- /* limit yaw if it causes outputs clipping */
- if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
- yaw = -out / _rotors[i].yaw_scale;
- if(status_reg != NULL) {
- (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
- }
- }
-
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
@@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
outputs[i] = out;
}
- /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
- if (min_out < 0.0f) {
- float scale_in = thrust / (thrust - min_out);
-
- max_out = 0.0f;
-
- /* mix again with adjusted controls */
- for (unsigned i = 0; i < _rotor_count; i++) {
- float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
-
- /* update max output value */
- if (out > max_out) {
- max_out = out;
- }
+ float boost = 0.0f; // value added to demanded thrust (can also be negative)
+ float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
- outputs[i] = out;
+ if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
+ float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+ if(max_thrust_diff >= -min_out) {
+ boost = -min_out;
}
+ else {
+ boost = max_thrust_diff;
+ roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+ }
+ }
+ else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
+ float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+ if(max_thrust_diff >= max_out - 1.0f) {
+ boost = -(max_out - 1.0f);
+ } else {
+ boost = -max_thrust_diff;
+ roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+ }
+ }
+ else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
+ float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+ boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff);
+ roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+ }
+ else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
+ float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+ boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f);
+ roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+ }
+ else if (min_out < 0.0f && max_out > 1.0f) {
+ boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
+ roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+ }
+ // notify if saturation has occurred
+ if(min_out < 0.0f) {
if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
}
-
- } else {
- /* roll/pitch mixed without lower side limiting, add yaw control */
- for (unsigned i = 0; i < _rotor_count; i++) {
- outputs[i] += yaw * _rotors[i].yaw_scale;
- }
}
-
- /* scale down all outputs if some outputs are too large, reduce total thrust */
- float scale_out;
- if (max_out > 1.0f) {
- scale_out = 1.0f / max_out;
-
+ if(max_out > 0.0f) {
if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
}
+ }
- } else {
- scale_out = 1.0f;
+ // mix again but now with thrust boost, scale roll/pitch and also add yaw
+ for(unsigned i = 0; i < _rotor_count; i++) {
+ float out = (roll * _rotors[i].roll_scale +
+ pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+ yaw * _rotors[i].yaw_scale +
+ thrust + boost;
+
+ out *= _rotors[i].out_scale;
+
+ // scale yaw if it violates limits. inform about yaw limit reached
+ if(out < 0.0f) {
+ yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+ roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale;
+ if(status_reg != NULL) {
+ (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+ }
+ }
+ else if(out > 1.0f) {
+ yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+ roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
+ if(status_reg != NULL) {
+ (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+ }
+ }
}
- /* scale outputs to range _idle_speed..1, and do final limiting */
+ /* last mix, add yaw and scale outputs to range idle_speed...1 */
for (unsigned i = 0; i < _rotor_count; i++) {
- outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
+ outputs[i] = (roll * _rotors[i].roll_scale +
+ pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+ yaw * _rotors[i].yaw_scale +
+ thrust + boost;
+
+ outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
}
return _rotor_count;
generated by cgit v1.2.3 (git 2.39.1) at 2024-05-22 20:47:43 +0000