2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
|
|
|
|
2014-11-05 06:18:04 -04:00
|
|
|
/*
|
|
|
|
handle creation of PX4 mixer file, for failover to direct RC control
|
|
|
|
on failure of FMU
|
|
|
|
|
|
|
|
This will create APM/MIXER.MIX on the microSD card. The user may
|
|
|
|
also create APM/CUSTOM.MIX, and if it exists that will be used
|
|
|
|
instead. That allows the user to setup more complex failsafe mixes
|
|
|
|
that include flaps, landing gear, ignition cut etc
|
|
|
|
*/
|
|
|
|
|
2016-09-10 19:19:22 -03:00
|
|
|
#if HAVE_PX4_MIXER
|
2014-11-05 06:18:04 -04:00
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <fcntl.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <drivers/drv_pwm_output.h>
|
|
|
|
#include <systemlib/mixer/mixer.h>
|
2015-06-21 05:19:16 -03:00
|
|
|
#include <modules/px4iofirmware/protocol.h>
|
2016-04-07 03:30:51 -03:00
|
|
|
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
|
2014-11-05 06:18:04 -04:00
|
|
|
|
2015-06-02 08:48:18 -03:00
|
|
|
#define PX4_LIM_RC_MIN 900
|
|
|
|
#define PX4_LIM_RC_MAX 2100
|
2014-11-05 06:18:04 -04:00
|
|
|
|
|
|
|
/*
|
2015-06-02 08:48:18 -03:00
|
|
|
formatted print to a buffer with buffer advance. Returns true on
|
|
|
|
success, false on fail
|
2014-11-05 06:18:04 -04:00
|
|
|
*/
|
2015-06-02 08:48:18 -03:00
|
|
|
bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...)
|
2014-11-05 06:18:04 -04:00
|
|
|
{
|
2015-06-02 08:48:18 -03:00
|
|
|
va_list arg_list;
|
|
|
|
va_start(arg_list, fmt);
|
|
|
|
int n = ::vsnprintf(buf, buf_size, fmt, arg_list);
|
|
|
|
va_end(arg_list);
|
|
|
|
if (n <= 0 || n >= buf_size) {
|
2014-11-05 06:18:04 -04:00
|
|
|
return false;
|
|
|
|
}
|
2015-06-02 08:48:18 -03:00
|
|
|
buf += n;
|
|
|
|
buf_size -= n;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2016-01-21 22:48:28 -04:00
|
|
|
create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used
|
2015-06-02 08:48:18 -03:00
|
|
|
*/
|
2016-01-21 22:48:28 -04:00
|
|
|
uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
|
2015-06-02 08:48:18 -03:00
|
|
|
{
|
|
|
|
char *buf0 = buf;
|
|
|
|
uint16_t buf_size0 = buf_size;
|
2014-11-05 06:18:04 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
this is the equivalent of channel_output_mixer()
|
|
|
|
*/
|
|
|
|
const int8_t mixmul[5][2] = { { 0, 0 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }};
|
2014-11-06 19:20:37 -04:00
|
|
|
// these are the internal clipping limits. Use scale_max1 when
|
|
|
|
// clipping to user specified min/max is wanted. Use scale_max2
|
|
|
|
// when no clipping is wanted (simulated by setting a very large
|
|
|
|
// clipping value)
|
|
|
|
const float scale_max1 = 10000;
|
|
|
|
const float scale_max2 = 1000000;
|
|
|
|
// range for mixers
|
|
|
|
const uint16_t mix_max = scale_max1 * g.mixing_gain;
|
|
|
|
// scaling factors used by PX4IO between pwm and internal values,
|
|
|
|
// as configured in setup_failsafe_mixing() below
|
2015-06-02 08:48:18 -03:00
|
|
|
const float pwm_min = PX4_LIM_RC_MIN;
|
|
|
|
const float pwm_max = PX4_LIM_RC_MAX;
|
2014-11-06 19:20:37 -04:00
|
|
|
const float pwm_scale = 2*scale_max1/(pwm_max - pwm_min);
|
2014-11-05 06:18:04 -04:00
|
|
|
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
2014-11-06 19:20:37 -04:00
|
|
|
int32_t c1, c2, mix=0;
|
2014-11-05 06:18:04 -04:00
|
|
|
bool rev = false;
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i);
|
2014-11-05 06:18:04 -04:00
|
|
|
if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
|
|
|
|
// first channel of VTAIL mix
|
|
|
|
c1 = rcmap.yaw()-1;
|
|
|
|
c2 = i;
|
|
|
|
rev = false;
|
|
|
|
mix = -mix_max*mixmul[g.vtail_output][0];
|
|
|
|
} else if (i == rcmap.yaw()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
|
|
|
|
// second channel of VTAIL mix
|
|
|
|
c1 = rcmap.pitch()-1;
|
|
|
|
c2 = i;
|
|
|
|
rev = true;
|
|
|
|
mix = mix_max*mixmul[g.vtail_output][1];
|
2014-11-06 19:20:37 -04:00
|
|
|
} else if (i == rcmap.roll()-1 && g.elevon_output > MIXING_DISABLED &&
|
|
|
|
g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// first channel of ELEVON mix
|
|
|
|
c1 = i;
|
|
|
|
c2 = rcmap.pitch()-1;
|
|
|
|
rev = true;
|
|
|
|
mix = mix_max*mixmul[g.elevon_output][1];
|
2014-11-06 19:20:37 -04:00
|
|
|
} else if (i == rcmap.pitch()-1 && g.elevon_output > MIXING_DISABLED &&
|
|
|
|
g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// second channel of ELEVON mix
|
|
|
|
c1 = i;
|
|
|
|
c2 = rcmap.roll()-1;
|
|
|
|
rev = false;
|
|
|
|
mix = mix_max*mixmul[g.elevon_output][0];
|
2016-10-22 07:27:57 -03:00
|
|
|
} else if (function == SRV_Channel::k_aileron ||
|
|
|
|
function == SRV_Channel::k_flaperon1 ||
|
|
|
|
function == SRV_Channel::k_flaperon2) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// a secondary aileron. We don't mix flap input in yet for flaperons
|
|
|
|
c1 = rcmap.roll()-1;
|
2016-10-22 07:27:57 -03:00
|
|
|
} else if (function == SRV_Channel::k_elevator) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// a secondary elevator
|
|
|
|
c1 = rcmap.pitch()-1;
|
2016-10-22 07:27:57 -03:00
|
|
|
} else if (function == SRV_Channel::k_rudder ||
|
|
|
|
function == SRV_Channel::k_steering) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// a secondary rudder or wheel
|
|
|
|
c1 = rcmap.yaw()-1;
|
|
|
|
} else if (g.flapin_channel > 0 &&
|
2016-10-22 07:27:57 -03:00
|
|
|
(function == SRV_Channel::k_flap ||
|
|
|
|
function == SRV_Channel::k_flap_auto)) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// a flap output channel, and we have a manual flap input channel
|
|
|
|
c1 = g.flapin_channel-1;
|
|
|
|
} else if (i < 4 ||
|
2016-10-22 07:27:57 -03:00
|
|
|
function == SRV_Channel::k_elevator_with_input ||
|
|
|
|
function == SRV_Channel::k_aileron_with_input ||
|
|
|
|
function == SRV_Channel::k_manual) {
|
2014-11-05 06:18:04 -04:00
|
|
|
// a pass-thru channel
|
|
|
|
c1 = i;
|
|
|
|
} else {
|
|
|
|
// a empty output
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "Z:\n")) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (mix == 0) {
|
2016-05-12 14:10:49 -03:00
|
|
|
// pass through channel, possibly with reversal. We also
|
2014-11-05 06:18:04 -04:00
|
|
|
// adjust the gain based on the range of input and output
|
2014-11-06 19:20:37 -04:00
|
|
|
// channels and adjust for trims
|
2016-10-22 07:27:57 -03:00
|
|
|
const RC_Channel *chan1 = RC_Channels::rc_channel(i);
|
|
|
|
const RC_Channel *chan2 = RC_Channels::rc_channel(c1);
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim());
|
|
|
|
int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim());
|
2015-06-02 08:48:18 -03:00
|
|
|
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
|
|
|
|
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
|
2014-11-06 19:20:37 -04:00
|
|
|
// if the input and output channels are the same then we
|
|
|
|
// apply clipping. This allows for direct pass-thru
|
|
|
|
int32_t limit = (c1==i?scale_max2:scale_max1);
|
2014-11-08 00:38:39 -04:00
|
|
|
int32_t in_scale_low;
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
if (chan2_trim <= chan2->get_radio_min()) {
|
2014-11-08 00:38:39 -04:00
|
|
|
in_scale_low = scale_max1;
|
|
|
|
} else {
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min());
|
2014-11-08 00:38:39 -04:00
|
|
|
}
|
|
|
|
int32_t in_scale_high;
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
if (chan2->get_radio_max() <= chan2_trim) {
|
2014-11-08 00:38:39 -04:00
|
|
|
in_scale_high = scale_max1;
|
|
|
|
} else {
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim);
|
2014-11-08 00:38:39 -04:00
|
|
|
}
|
2014-11-06 19:20:37 -04:00
|
|
|
if (chan1->get_reverse() != chan2->get_reverse()) {
|
|
|
|
in_scale_low = -in_scale_low;
|
|
|
|
in_scale_high = -in_scale_high;
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "M: 1\n") ||
|
|
|
|
!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
(int)(pwm_scale*(chan1_trim - chan1->get_radio_min())),
|
|
|
|
(int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)),
|
2015-06-02 08:48:18 -03:00
|
|
|
(int)(pwm_scale*(chan1_trim - 1500)),
|
|
|
|
(int)-scale_max2, (int)scale_max2) ||
|
|
|
|
!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1,
|
|
|
|
in_scale_low,
|
|
|
|
in_scale_high,
|
|
|
|
0,
|
|
|
|
-limit, limit)) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
} else {
|
2016-10-22 07:27:57 -03:00
|
|
|
const RC_Channel *chan1 = RC_Channels::rc_channel(c1);
|
|
|
|
const RC_Channel *chan2 = RC_Channels::rc_channel(c2);
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim());
|
|
|
|
int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim());
|
2015-06-02 08:48:18 -03:00
|
|
|
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
|
|
|
|
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
|
2014-11-06 19:20:37 -04:00
|
|
|
// mix of two input channels to give an output channel. To
|
|
|
|
// make the mixer match the behaviour of APM we need to
|
|
|
|
// scale and offset the input channels to undo the affects
|
|
|
|
// of the PX4IO input processing
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "M: 2\n") ||
|
|
|
|
!print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-08 00:38:39 -04:00
|
|
|
int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min);
|
|
|
|
int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim);
|
|
|
|
int32_t offset = pwm_scale*(chan1_trim - 1500);
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
|
|
|
|
c1, in_scale_low, in_scale_high, offset,
|
|
|
|
(int)-scale_max2, (int)scale_max2)) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-08 00:38:39 -04:00
|
|
|
in_scale_low = pwm_scale*(chan2_trim - pwm_min);
|
|
|
|
in_scale_high = pwm_scale*(pwm_max - chan2_trim);
|
|
|
|
offset = pwm_scale*(chan2_trim - 1500);
|
2014-11-05 06:18:04 -04:00
|
|
|
if (rev) {
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
|
|
|
|
c2, in_scale_low, in_scale_high, offset,
|
|
|
|
(int)-scale_max2, (int)scale_max2)) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
} else {
|
2015-06-02 08:48:18 -03:00
|
|
|
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
|
|
|
|
c2, -in_scale_low, -in_scale_high, -offset,
|
|
|
|
(int)-scale_max2, (int)scale_max2)) {
|
2016-01-21 22:48:28 -04:00
|
|
|
return 0;
|
2015-06-02 08:48:18 -03:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2015-06-02 08:48:18 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
if possible, also write to a file for debugging purposes
|
|
|
|
*/
|
|
|
|
int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
|
|
|
|
if (mix_fd != -1) {
|
|
|
|
write(mix_fd, buf0, buf_size0 - buf_size);
|
|
|
|
close(mix_fd);
|
|
|
|
}
|
2016-01-21 22:48:28 -04:00
|
|
|
return buf_size0 - buf_size;
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
setup mixer on PX4 so that if FMU dies the pilot gets manual control
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::setup_failsafe_mixing(void)
|
2014-11-05 06:18:04 -04:00
|
|
|
{
|
|
|
|
const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
|
|
|
|
bool ret = false;
|
2016-10-28 19:10:03 -03:00
|
|
|
char *buf = nullptr;
|
2015-01-06 00:15:20 -04:00
|
|
|
const uint16_t buf_size = 2048;
|
2016-06-05 19:47:55 -03:00
|
|
|
uint16_t fileSize, new_crc;
|
|
|
|
int px4io_fd = -1;
|
|
|
|
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
|
|
|
|
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
|
2016-09-04 22:38:42 -03:00
|
|
|
unsigned mixer_status = 0;
|
2014-11-05 06:18:04 -04:00
|
|
|
|
2015-06-02 08:48:18 -03:00
|
|
|
buf = (char *)malloc(buf_size);
|
2016-10-28 19:10:03 -03:00
|
|
|
if (buf == nullptr) {
|
2016-06-05 19:47:55 -03:00
|
|
|
goto failed;
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
|
2016-06-05 19:47:55 -03:00
|
|
|
fileSize = create_mixer(buf, buf_size, mixer_filename);
|
2016-01-21 22:48:28 -04:00
|
|
|
if (!fileSize) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("Unable to create mixer");
|
2016-06-05 19:47:55 -03:00
|
|
|
goto failed;
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
|
2016-06-05 19:47:55 -03:00
|
|
|
new_crc = crc_calculate((uint8_t *)buf, fileSize);
|
2016-01-21 22:48:28 -04:00
|
|
|
|
|
|
|
if ((int32_t)new_crc == last_mixer_crc) {
|
2016-06-06 02:21:17 -03:00
|
|
|
free(buf);
|
2016-01-21 22:48:28 -04:00
|
|
|
return true;
|
|
|
|
} else {
|
|
|
|
last_mixer_crc = new_crc;
|
|
|
|
}
|
|
|
|
|
2016-06-05 19:47:55 -03:00
|
|
|
px4io_fd = open("/dev/px4io", 0);
|
2014-11-05 06:18:04 -04:00
|
|
|
if (px4io_fd == -1) {
|
|
|
|
// px4io isn't started, no point in setting up a mixer
|
2016-06-05 19:47:55 -03:00
|
|
|
goto failed;
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
|
2014-11-08 00:38:39 -04:00
|
|
|
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
|
|
|
|
// make sure the throttle has a non-zero failsafe value before we
|
|
|
|
// disable safety. This prevents sending zero PWM during switch over
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
2014-11-08 00:38:39 -04:00
|
|
|
}
|
|
|
|
|
2015-06-02 08:48:18 -03:00
|
|
|
// we need to force safety on to allow us to load a mixer. We call
|
|
|
|
// it twice as there have been reports that this call can fail
|
|
|
|
// with a small probability
|
|
|
|
hal.rcout->force_safety_on();
|
2016-05-27 03:41:29 -03:00
|
|
|
hal.rcout->force_safety_no_wait();
|
2014-11-05 06:18:04 -04:00
|
|
|
|
|
|
|
/* reset any existing mixer in px4io. This shouldn't be needed,
|
|
|
|
* but is good practice */
|
|
|
|
if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("Unable to reset mixer");
|
2014-11-05 06:18:04 -04:00
|
|
|
goto failed;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* pass the buffer to the device */
|
|
|
|
if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("Unable to send mixer to IO");
|
2014-11-05 06:18:04 -04:00
|
|
|
goto failed;
|
|
|
|
}
|
|
|
|
|
2014-11-06 19:38:15 -04:00
|
|
|
// setup RC config for each channel based on user specified
|
|
|
|
// mix/max/trim. We only do the first 8 channels due to
|
|
|
|
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
2016-10-22 07:27:57 -03:00
|
|
|
RC_Channel *ch = RC_Channels::rc_channel(i);
|
2016-10-28 19:10:03 -03:00
|
|
|
if (ch == nullptr) {
|
2014-11-05 06:18:04 -04:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
struct pwm_output_rc_config config;
|
2014-11-06 19:20:37 -04:00
|
|
|
/*
|
|
|
|
we use a min/max of 900/2100 to allow for pass-thru of
|
|
|
|
larger values than the RC min/max range. This mimics the APM
|
|
|
|
behaviour of pass-thru in manual, which allows for dual-rate
|
|
|
|
transmitter setups in manual mode to go beyond the ranges
|
|
|
|
used in stabilised modes
|
|
|
|
*/
|
2014-11-05 06:18:04 -04:00
|
|
|
config.channel = i;
|
|
|
|
config.rc_min = 900;
|
|
|
|
config.rc_max = 2100;
|
2014-11-08 00:38:39 -04:00
|
|
|
if (rcmap.throttle()-1 == i) {
|
|
|
|
// throttle uses a trim of 1500, so we don't get division
|
|
|
|
// by small numbers near RC3_MIN
|
|
|
|
config.rc_trim = 1500;
|
|
|
|
} else {
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1);
|
2014-11-08 00:38:39 -04:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
config.rc_dz = 0; // zero for the purposes of manual takeover
|
2015-06-21 05:19:16 -03:00
|
|
|
|
2014-11-05 06:18:04 -04:00
|
|
|
// we set reverse as false, as users of ArduPilot will have
|
|
|
|
// input reversed on transmitter, so from the point of view of
|
|
|
|
// the mixer the input is never reversed. The one exception is
|
|
|
|
// the 2nd channel, which is reversed inside the PX4IO code,
|
|
|
|
// so needs to be unreversed here to give sane behaviour.
|
|
|
|
if (i == 1) {
|
|
|
|
config.rc_reverse = true;
|
|
|
|
} else {
|
|
|
|
config.rc_reverse = false;
|
|
|
|
}
|
2015-06-21 05:19:16 -03:00
|
|
|
|
|
|
|
if (i+1 == g.override_channel.get()) {
|
|
|
|
/*
|
|
|
|
This is an OVERRIDE_CHAN channel. We want IO to trigger
|
|
|
|
override with a channel input of over 1750. The px4io
|
2016-05-22 21:00:04 -03:00
|
|
|
code is setup for triggering below 80% of the range below
|
|
|
|
trim. To map this to values above 1750 we need to reverse
|
|
|
|
the direction and set the rc range for this channel to 1000
|
|
|
|
to 1813 (1812.5 = 1500 + 250/0.8)
|
2015-06-21 05:19:16 -03:00
|
|
|
*/
|
|
|
|
config.rc_assignment = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
|
|
|
|
config.rc_reverse = true;
|
2016-05-22 21:00:04 -03:00
|
|
|
config.rc_max = 1813; // round 1812.5 up to grant > 1750
|
2015-06-21 05:19:16 -03:00
|
|
|
config.rc_min = 1000;
|
|
|
|
config.rc_trim = 1500;
|
|
|
|
} else {
|
|
|
|
config.rc_assignment = i;
|
|
|
|
}
|
|
|
|
|
2014-11-08 00:38:39 -04:00
|
|
|
if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("SET_RC_CONFIG failed");
|
2015-05-29 22:17:04 -03:00
|
|
|
goto failed;
|
2014-11-08 00:38:39 -04:00
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
|
2016-10-22 07:27:57 -03:00
|
|
|
if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 &&
|
|
|
|
SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) {
|
2016-08-11 00:19:00 -03:00
|
|
|
pwm_values.values[i] = quadplane.thr_min_pwm;
|
|
|
|
} else {
|
|
|
|
pwm_values.values[i] = 900;
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
2015-06-02 08:48:18 -03:00
|
|
|
if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("SET_MIN_PWM failed");
|
2015-06-02 08:48:18 -03:00
|
|
|
goto failed;
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
|
|
|
|
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
|
2016-10-22 07:27:57 -03:00
|
|
|
if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 &&
|
|
|
|
SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) {
|
2016-08-11 00:19:00 -03:00
|
|
|
hal.rcout->write(i, quadplane.thr_min_pwm);
|
|
|
|
pwm_values.values[i] = quadplane.thr_min_pwm;
|
|
|
|
} else {
|
|
|
|
pwm_values.values[i] = 2100;
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
2015-06-02 08:48:18 -03:00
|
|
|
if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("SET_MAX_PWM failed");
|
2015-06-02 08:48:18 -03:00
|
|
|
goto failed;
|
|
|
|
}
|
|
|
|
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("SET_OVERRIDE_OK failed");
|
2015-06-02 08:48:18 -03:00
|
|
|
goto failed;
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
|
2015-06-02 08:48:18 -03:00
|
|
|
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) {
|
2016-11-25 19:11:36 -04:00
|
|
|
hal.console->println("SET_OVERRIDE_IMMEDIATE failed");
|
2015-06-02 08:48:18 -03:00
|
|
|
goto failed;
|
|
|
|
}
|
2014-11-06 20:30:30 -04:00
|
|
|
|
2016-09-04 22:38:42 -03:00
|
|
|
if (ioctl(px4io_fd, PWM_IO_GET_STATUS, (unsigned long)&mixer_status) != 0 ||
|
|
|
|
(mixer_status & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
|
|
|
|
hal.console->printf("Mixer failed: 0x%04x\n", mixer_status);
|
|
|
|
goto failed;
|
|
|
|
}
|
|
|
|
|
2014-11-05 06:18:04 -04:00
|
|
|
ret = true;
|
|
|
|
|
|
|
|
failed:
|
2016-10-28 19:10:03 -03:00
|
|
|
if (buf != nullptr) {
|
2015-01-06 00:15:20 -04:00
|
|
|
free(buf);
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
if (px4io_fd != -1) {
|
|
|
|
close(px4io_fd);
|
|
|
|
}
|
|
|
|
// restore safety state if it was previously armed
|
|
|
|
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
|
|
|
|
hal.rcout->force_safety_off();
|
2016-08-11 01:11:03 -03:00
|
|
|
hal.rcout->force_safety_no_wait();
|
2014-11-05 06:18:04 -04:00
|
|
|
}
|
2016-06-06 02:12:23 -03:00
|
|
|
if (!ret) {
|
|
|
|
// clear out the mixer CRC so that we will attempt to send it again
|
|
|
|
last_mixer_crc = -1;
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#endif // CONFIG_HAL_BOARD
|