void assert_equal_float_array( float* ref, float* opt, const int nelems, const char* file, const int line )
{
// OBS: the maximum ULP difference observed is 16384 and it seems to come from difference
// optimizations applied by NVCC compiler in "stress_update" function in scell_BR
// causes an accumulation of errors.
int maxULP = 16384;
int max_diff = 0;
for (int elem = 0; elem < nelems; elem++)
{
float ref_e = ref[elem];
float opt_e = opt[elem];
const int diff = diff_in_ULPs(ref_e, opt_e);
if (max_diff < diff) max_diff = diff;
if ( !assert_float_equal_ULPs(ref_e, opt_e, maxULP) )
{
fprintf(stderr, "ERROR:%s:%d: in element %d : ref %e !=\t %e \topt by %d ULP\n",
file, line, elem, ref_e, opt_e, diff );
Float_t uref, uopt;
uref.f = ref_e;
uopt.f = opt_e;
fprintf(stderr, "REF: sign %d mantissa %023d exponent %08d\n",
is_negative(uref), raw_mantissa(uref), raw_exponent(uref));
fprintf(stderr, "OPT: sign %d mantissa %023d exponent %08d\n",
is_negative(uopt), raw_mantissa(uopt), raw_exponent(uopt));
exit(1); //UNITY_FAIL_AND_BAIL;
}
}
fprintf(stdout, "\nMAX DIFF: %d ULPs", max_diff);
}
// Proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
float correction_rate;
if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
// second order limit is zero or negative.
correction_rate = error*p;
} else if (is_zero(p)) {
// P term is zero but we have a second order limit.
if (is_positive(error)) {
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
} else if (is_negative(error)) {
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
} else {
correction_rate = 0.0f;
}
} else {
// Both the P and second order limit have been defined.
float linear_dist = second_ord_lim/sq(p);
if (error > linear_dist) {
correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
} else if (error < -linear_dist) {
correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
} else {
correction_rate = error*p;
}
}
if (!is_zero(dt)) {
// this ensures we do not get small oscillations by over shooting the error correction in the last time step.
return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
} else {
return correction_rate;
}
}
bool assert_float_equal_ULPs(float A, float B, int maxUlpsDiff)
{
Float_t uA;
Float_t uB;
uA.f = A;
uB.f = B;
// NaN or Infinite means they do not match.
if (isnan(A) || isnan(B) || (A == INFINITY) || (B == INFINITY))
return false;
// Different signs means they do not match.
if (is_negative(uA) != is_negative(uB))
{
// Check for equality to make sure +0==-0
if (A == B)
return true;
return false;
}
// Find the difference in ULPs.
int ulpsDiff = abs(uA.i - uB.i);
if (ulpsDiff <= maxUlpsDiff)
return true;
return false;
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
if (_scale_steering) {
// vectored thrust handling
if (have_vectored_thrust()) {
if (fabsf(throttle) > _vector_throttle_base) {
// scale steering down linearly as throttle increases above _vector_throttle_base
steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
}
} else {
// scale steering down as speed increase above MOT_SPD_SCA_BASE (1 m/s default)
if (is_positive(_speed_scale_base) && (fabsf(ground_speed) > _speed_scale_base)) {
steering *= (_speed_scale_base / fabsf(ground_speed));
} else {
// regular steering rover at low speed so set limits to stop I-term build-up in controllers
if (!have_skid_steering()) {
limit.steer_left = true;
limit.steer_right = true;
}
}
// reverse steering direction when backing up
if (is_negative(ground_speed)) {
steering *= -1.0f;
}
}
} else {
// reverse steering direction when backing up
if (is_negative(throttle)) {
steering *= -1.0f;
}
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
// clear and set limits based on input
// we do this here because vectored thrust or speed scaling may have reduced steering request
set_limits_from_input(armed, steering, throttle);
// constrain steering
steering = constrain_float(steering, -4500.0f, 4500.0f);
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
// steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
// throttle_out is in the range -100 ~ +100
void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out)
{
// do basic conversion
get_pilot_input(steering_out, throttle_out);
// check for special case of input and output throttle being in opposite directions
float throttle_out_limited = g2.motors.get_slew_limited_throttle(throttle_out, rover.G_Dt);
if ((is_negative(throttle_out) != is_negative(throttle_out_limited)) &&
((g.pilot_steer_type == PILOT_STEER_TYPE_DEFAULT) ||
(g.pilot_steer_type == PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING))) {
steering_out *= -1;
}
throttle_out = throttle_out_limited;
}
// decode pilot steering and return steering_out and speed_out (in m/s)
void Mode::get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out)
{
float desired_throttle;
get_pilot_input(steering_out, desired_throttle);
speed_out = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// check for special case of input and output throttle being in opposite directions
float speed_out_limited = g2.attitude_control.get_desired_speed_accel_limited(speed_out, rover.G_Dt);
if ((is_negative(speed_out) != is_negative(speed_out_limited)) &&
((g.pilot_steer_type == PILOT_STEER_TYPE_DEFAULT) ||
(g.pilot_steer_type == PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING))) {
steering_out *= -1;
}
speed_out = speed_out_limited;
}
void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out)
{
// no RC input means no throttle and centered steering
if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
steering_out = 0;
throttle_out = 0;
return;
}
// apply RC skid steer mixing
switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get())
{
case PILOT_STEER_TYPE_DEFAULT:
default: {
// by default regular and skid-steering vehicles reverse their rotation direction when backing up
// (this is the same as PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING below)
throttle_out = rover.channel_throttle->get_control_in();
steering_out = rover.channel_steer->get_control_in();
break;
}
case PILOT_STEER_TYPE_TWO_PADDLES: {
// convert the two radio_in values from skid steering values
// left paddle from steering input channel, right paddle from throttle input channel
// steering = left-paddle - right-paddle
// throttle = average(left-paddle, right-paddle)
const float left_paddle = rover.channel_steer->norm_input();
const float right_paddle = rover.channel_throttle->norm_input();
throttle_out = 0.5f * (left_paddle + right_paddle) * 100.0f;
const float steering_dir = is_negative(throttle_out) ? -1 : 1;
steering_out = steering_dir * (left_paddle - right_paddle) * 4500.0f;
break;
}
case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING:
throttle_out = rover.channel_throttle->get_control_in();
steering_out = rover.channel_steer->get_control_in();
break;
case PILOT_STEER_TYPE_DIR_UNCHANGED_WHEN_REVERSING: {
throttle_out = rover.channel_throttle->get_control_in();
const float steering_dir = is_negative(throttle_out) ? -1 : 1;
steering_out = steering_dir * rover.channel_steer->get_control_in();
break;
}
}
}
int main()
{
namespace oven = pstade::oven;
namespace bll = boost::lambda;
int a[] = { 9,1,3,7,4,31,1,76,9,2,3 };
int b[] = { -1,0,9,-3,-6,1,3,7,4,31,-2,1,-2,76,-1,9,-6,2,3 };
BOOST_TEST(oven::equals(a, b | oven::removed(is_negative())));
BOOST_TEST(oven::equals(a, b | oven::removed(oven::regular(is_negative()))));
BOOST_TEST(oven::equals(a, b | oven::removed(oven::regular(0 >= bll::_1))));
// BOOST_TEST(oven::equals(a, b | oven::removed(oven::regular([](int x) { return 0 >= x; }))));
return boost::report_errors();
}
char *ft_itoa_base(int value, int base)
{
int sign;
int count;
char *str;
count = 0;
sign = is_negative(&value);
if (base < 2 || base > 16)
return ("");
if (value == 0 || value == -0)
return ("0");
if (value == -2147483648)
return ("-2147483648");
if (value == 2147483647)
return ("2147483647");
if ((str = ft_strnew(ft_getlen(value, base) + sign))
== NULL)
return (NULL);
count = ft_getstr(value, base, str);
if (sign < 0 && base == 10)
str[count++] = '-';
str[count] = '\0';
ft_strrev(str);
return (str);
}
inline void fast_sin_cos(float x, float& s, float& c)
{
while (is_positive(x) && x >= anglef::_2pi) x -= anglef::_2pi;
while (is_negative(x) && x < 0) x += anglef::_2pi;
if (x >= anglef::pi)
{
if (x >= anglef::_3pi2)
{
//64 + anglef::_2pi
s = 1.f - fast_sin_pi2(64.f + anglef::_2pi - x);
c = fast_sin_pi2(64.f - anglef::_3pi2 + x) - 1.f;
}
else
{
//64 + x - anglef::pi
s = 1.f - fast_sin_pi2(64.f - anglef::pi + x);
c = 1.f - fast_sin_pi2(64.f + anglef::_3pi2 - x);
}
}
else
{
if (x >= anglef::pi2)
{
//64 + anglef::pi - x
s = fast_sin_pi2(64.f + anglef::pi - x) - 1.f;
c = 1.f - fast_sin_pi2(64.f - anglef::pi2 + x);
}
else
{
s = fast_sin_pi2(64.f + x) - 1.f;
c = fast_sin_pi2(64.f + anglef::pi2 - x) - 1.f;
}
}
}
// scale a throttle using the _throttle_min and _thrust_curve_expo parameters. throttle should be in the range -100 to +100
float AP_MotorsUGV::get_scaled_throttle(float throttle) const
{
// exit immediately if throttle is zero
if (is_zero(throttle)) {
return throttle;
}
// scale using throttle_min
if (_throttle_min > 0) {
if (is_negative(throttle)) {
throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f));
} else {
throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f));
}
}
// skip further scaling if thrust curve disabled or invalid
if (is_zero(_thrust_curve_expo) || (_thrust_curve_expo > 1.0f) || (_thrust_curve_expo < -1.0f)) {
return throttle;
}
// calculate scaler
const float sign = (throttle < 0.0f) ? -1.0f : 1.0f;
const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) / 100.0f;
return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo);
}
// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// positive lateral acceleration is to the right.
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt)
{
// record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis();
_desired_lat_accel = desired_accel;
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// we expect caller will not try to control heading using rate control without a valid speed estimate
// on failure to get speed we do not attempt to steer
return 0.0f;
}
// enforce minimum speed to stop oscillations when first starting to move
if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
if (is_negative(speed)) {
speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
} else {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
}
// Calculate the desired steering rate given desired_accel and speed
const float desired_rate = desired_accel / speed;
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
}
int my_put_nbr(int nb)
{
unsigned char len;
int copy;
int diz;
if (nb == -2147483648)
{
cheat();
return (0);
}
nb = is_negative(nb);
copy = nb;
len = 1;
while (copy > 9)
{
copy = copy / 10;
len = len + 1;
}
while (len != 0)
{
diz = nb / my_pow(len-1);
my_putchar(diz + 48);
nb = nb - diz * my_pow(len-1);
len = len - 1;
}
}
// get throttle output in the range -100 to +100 given a desired rate expressed as a percentage of the rate_max (-100 to +100)
// instance can be 0 or 1
float AP_WheelRateControl::get_rate_controlled_throttle(uint8_t instance, float desired_rate_pct, float dt)
{
if (!enabled(instance)) {
return 0;
}
// determine which PID instance to use
AC_PID& rate_pid = (instance == 0) ? _rate_pid0 : _rate_pid1;
// set PID's dt
rate_pid.set_dt(dt);
// check for timeout
uint32_t now = AP_HAL::millis();
if (now - _last_update_ms > AP_WHEEL_RATE_CONTROL_TIMEOUT_MS) {
rate_pid.reset_filter();
rate_pid.reset_I();
_limit[instance].lower = false;
_limit[instance].upper = false;
}
_last_update_ms = now;
// convert desired rate as a percentage to radians/sec
float desired_rate = desired_rate_pct / 100.0f * get_rate_max_rads();
// get actual rate from wheeel encoder
float actual_rate = _wheel_encoder.get_rate(instance);
// calculate rate error and pass to pid controller
float rate_error = desired_rate - actual_rate;
rate_pid.set_input_filter_all(rate_error);
// store desired and actual for logging purposes
rate_pid.set_desired_rate(desired_rate);
rate_pid.set_actual_rate(actual_rate);
// get ff
float ff = rate_pid.get_ff(desired_rate);
// get p
float p = rate_pid.get_p();
// get i unless we hit limit on last iteration
float i = rate_pid.get_integrator();
if (((is_negative(rate_error) && !_limit[instance].lower) || (is_positive(rate_error) && !_limit[instance].upper))) {
i = rate_pid.get_i();
}
// get d
float d = rate_pid.get_d();
// constrain and set limit flags
float output = ff + p + i + d;
// set limits for next iteration
_limit[instance].upper = output >= 100.0f;
_limit[instance].lower = output <= -100.0f;
return output;
}
//main function
int main(void){
//variables for main function
char *yes = "y";
char *no = "n";
int progcontinue = 0;
int continuetest = 0;
printf("Brian Sator, masc0916\n\n");//output program setup information
for(;;){
//at the beginning of each cycle we check to see if we need to run the main routine again
if(progcontinue != 0){
break;//exit loop
}
printf("Please enter a fraction to reduce: ");//prompt user
scanf("%25s",buffer1);//read from input
flushinstream();//flush the input stream
if(in_validate(buffer1) == 1){//the input string is a valid fraction proceed with reduction
int numtest = atoi(num);
int denomtest = atoi(denom);
negative = is_negative(numtest, denomtest);
commondivisor = abs(findGCD(numtest,denomtest));
numtest = abs(numtest/commondivisor);
denomtest = abs(denomtest/commondivisor);
if(negative == 0){//check if negative
printf("The reduced fraction is: %d/%d\n\n",numtest, denomtest);
}else{
printf("The reduced fraction is: -%d/%d\n\n",numtest, denomtest);
}
numtest = 0;
numtest = 0;
}
else{//the input string is not valid, start routine over to prompt for new input
continue;
}
while(continuetest == 0){//here we check to see if user wants to reduce another fraction
printf("Do you want to reduce another fraction(y/n)? ");
scanf("%25s",buffer1);
flushinstream();
if(buffer1[0] == *no){//end program
progcontinue = 1;
continuetest = 1;
}else if(buffer1[0] == *yes){//new fraction
continuetest = 1;
}
else{//invalid input
printf("Sorry, invalid input\n\n");
}
}
continuetest = 0;
memset(num,0,30);
memset(denom,0,30);
}
printf("Program Terminated\n");
return 0;//exit program
}
void mp_int<A,T>::sub_digit(digit_type b)
{
if (is_negative())
{
const digit_type carry =
ops_type::add_single_digit(digits_, digits_, size_, b);
if (carry)
push(carry);
return;
}
if (size_ == 1)
{
if (digits_[0] < b) // example: 2 - 6 = -4
{
digits_[0] = b - digits_[0];
set_sign(-1);
}
else // example: 8 - 7 = 1 or 5 - 5 = 0
digits_[0] -= b;
}
else
{
ops_type::subtract_single_digit(digits_, digits_, size_, b);
if (!digits_[size_-1])
--size_;
}
}
inline float fast_sin(float x)
{
while (is_positive(x) && x >= anglef::_2pi) x -= anglef::_2pi;
while (is_negative(x) && x < 0) x += anglef::_2pi;
if (x >= anglef::pi)
{
if (x >= anglef::_3pi2)
{
//64 + anglef::_2pi
return 1.f - fast_sin_pi2(64.f + anglef::_2pi - x);
}
else
{
//64 + x - anglef::pi
return 1.f - fast_sin_pi2(64.f - anglef::pi + x);
}
}
else
{
if (x >= anglef::pi2)
{
//64 + anglef::pi - x
return fast_sin_pi2(64.f + anglef::pi - x) - 1.f;
}
else
{
return fast_sin_pi2(64 + x) - 1.f;
}
}
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
if (_scale_steering) {
// vectored thrust handling
if (have_vectored_thrust()) {
if (fabsf(throttle) > _vector_throttle_base) {
// scale steering down linearly as throttle increases above _vector_throttle_base
steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
}
} else {
// scale steering down as speed increase above 1m/s
if (fabsf(ground_speed) > 1.0f) {
steering *= (1.0f / fabsf(ground_speed));
} else {
// regular steering rover at low speed so set limits to stop I-term build-up in controllers
if (!have_skid_steering()) {
limit.steer_left = true;
limit.steer_right = true;
}
}
// reverse steering output if backing up
if (is_negative(ground_speed)) {
steering *= -1.0f;
}
}
} else {
// reverse steering direction when backing up
if (is_negative(throttle)) {
steering *= -1.0f;
}
}
output_throttle(SRV_Channel::k_throttle, throttle);
} else {
// handle disarmed case
if (_disarm_disable_pwm) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// set desired speed in m/s
bool Mode::set_desired_speed(float speed)
{
if (!is_negative(speed)) {
_desired_speed = speed;
return true;
}
return false;
}
int main(void) {
int iai[] = {-1,0,1,2,3,4,5,6,7,8,9,10};
int i = 0;
for( i = 0; i < sizeof(iai)/sizeof(int); i++) {
printf("\t%3d has a sign of %c\n", iai[i], is_negative(iai[i]) ? '-':'+');
}
return EXIT_SUCCESS;
}
Out floor(X x)
{
static_assert(!std::is_integral<X>::value, "type must be non-integral");
static_assert(std::is_integral<Out>::value, "type must be integral");
if (is_negative(x))
x -= 1;
return static_cast<Out>(x);
}
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt)
{
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
// if not called recently, reset input filter and desired turn rate to actual turn rate (used for accel limiting)
const uint32_t now = AP_HAL::millis();
if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
_steer_rate_pid.reset_filter();
_steer_rate_pid.reset_I();
_desired_turn_rate = _ahrs.get_yaw_rate_earth();
}
_steer_turn_last_ms = now;
// acceleration limit desired turn rate
if (is_positive(_steer_accel_max)) {
const float change_max = radians(_steer_accel_max) * dt;
desired_rate = constrain_float(desired_rate, _desired_turn_rate - change_max, _desired_turn_rate + change_max);
}
_desired_turn_rate = desired_rate;
// rate limit desired turn rate
if (is_positive(_steer_rate_max)) {
const float steer_rate_max_rad = radians(_steer_rate_max);
_desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
}
// Calculate the steering rate error (rad/sec)
// We do this in earth frame to allow for rover leaning over in hard corners
const float rate_error = (_desired_turn_rate - _ahrs.get_yaw_rate_earth());
// set PID's dt
_steer_rate_pid.set_dt(dt);
// record desired rate for logging purposes only
_steer_rate_pid.set_desired_rate(_desired_turn_rate);
// pass error to PID controller
_steer_rate_pid.set_input_filter_all(rate_error);
// get feed-forward
const float ff = _steer_rate_pid.get_ff(_desired_turn_rate);
// get p
const float p = _steer_rate_pid.get_p();
// get i unless non-skid-steering rover at low speed or steering output has hit a limit
float i = _steer_rate_pid.get_integrator();
if ((is_negative(rate_error) && !motor_limit_left) || (is_positive(rate_error) && !motor_limit_right)) {
i = _steer_rate_pid.get_i();
}
// get d
const float d = _steer_rate_pid.get_d();
// constrain and return final output
return constrain_float(ff + p + i + d, -1.0f, 1.0f);
}
// output throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100
void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle)
{
// sanity check servo function
if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3) {
return;
}
// constrain and scale output
throttle = get_scaled_throttle(throttle);
// set relay if necessary
if (_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) {
// find the output channel, if not found return
const SRV_Channel *out_chan = SRV_Channels::get_channel_for(function);
if (out_chan == nullptr) {
return;
}
const int8_t reverse_multiplier = out_chan->get_reversed() ? -1 : 1;
bool relay_high = is_negative(reverse_multiplier * throttle);
switch (function) {
case SRV_Channel::k_throttle:
case SRV_Channel::k_throttleLeft:
case SRV_Channel::k_motor1:
_relayEvents.do_set_relay(0, relay_high);
break;
case SRV_Channel::k_throttleRight:
case SRV_Channel::k_motor2:
_relayEvents.do_set_relay(1, relay_high);
break;
case SRV_Channel::k_motor3:
_relayEvents.do_set_relay(2, relay_high);
break;
default:
// do nothing
break;
}
// invert the output to always have positive value calculated by calc_pwm
throttle = reverse_multiplier * fabsf(throttle);
}
// output to servo channel
switch (function) {
case SRV_Channel::k_throttle:
case SRV_Channel::k_motor1:
case SRV_Channel::k_motor2:
case SRV_Channel::k_motor3:
SRV_Channels::set_output_scaled(function, throttle);
break;
case SRV_Channel::k_throttleLeft:
case SRV_Channel::k_throttleRight:
SRV_Channels::set_output_scaled(function, throttle * 10.0f);
break;
default:
// do nothing
break;
}
}
void ModeSteering::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
_desired_lat_accel = 0.0f;
return;
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter
// For regular steering vehicles we use the maximum lateral acceleration at full steering lock for this speed: V^2/R where R is the radius of turn.
float max_g_force;
if (is_pivot_turning) {
max_g_force = g.turn_max_g * GRAVITY_MSS;
} else {
max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
}
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
_desired_lat_accel = max_g_force * (desired_steering / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
_desired_lat_accel = -_desired_lat_accel;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
} else {
// run lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(false);
calc_throttle(target_speed, false);
}
}
void Rover::read_radio()
{
if (!hal.rcin->new_input()) {
// check if we lost RC link
control_failsafe(channel_throttle->get_radio_in());
return;
}
failsafe.last_valid_rc_ms = AP_HAL::millis();
// read the RC value
RC_Channels::set_pwm_all();
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
// apply RC skid steer mixing
if (g.skid_steer_in) {
// convert the two radio_in values from skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
const float left_input = channel_steer->norm_input();
const float right_input = channel_throttle->norm_input();
const float throttle_scaled = 0.5f * (left_input + right_input);
float steering_scaled = constrain_float(left_input - right_input, -1.0f, 1.0f);
// flip the steering direction if requesting the vehicle reverse (to be consistent with separate steering-throttle frames)
if (is_negative(throttle_scaled)) {
steering_scaled = -steering_scaled;
}
int16_t steer = channel_steer->get_radio_trim();
int16_t thr = channel_throttle->get_radio_trim();
if (steering_scaled > 0.0f) {
steer += steering_scaled * (channel_steer->get_radio_max() - channel_steer->get_radio_trim());
} else {
steer += steering_scaled * (channel_steer->get_radio_trim() - channel_steer->get_radio_min());
}
if (throttle_scaled > 0.0f) {
thr += throttle_scaled * (channel_throttle->get_radio_max() - channel_throttle->get_radio_trim());
} else {
thr += throttle_scaled * (channel_throttle->get_radio_trim() - channel_throttle->get_radio_min());
}
channel_steer->set_pwm(steer);
channel_throttle->set_pwm(thr);
}
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}
X int_power(X base, X exp)
{
static_assert(std::is_integral<X>::value,
"type must be unsigned integral");
assert(!is_negative(exp));
if (exp == 0)
return 1;
if (exp == 1)
return base;
return base * int_power(base, exp - 1);
}
T fast_power_0(T a, I n, Op op)
{
assert(!is_negative(n));
T result = identity_element(op);
while (!is_zero(n)) {
if (is_odd(n)) result = op(result, a);
a = op(a, a);
halve_non_negative(n);
}
return result;
}
std::basic_ostream<Ch, ChT>& output_pow_default
(std::basic_ostream<Ch, ChT>& out, const T& x)
{
// ѕ–≈ƒ”ѕ–≈∆ƒ≈Ќ»≈. “олько дл¤ тех T, дл¤ которых определЄн is_negative.
if(is_negative(x))
out << "(" << x << ")";
else
out << x;
return out;
}
/*************************************************
* Convert this number to a u32bit, if possible *
*************************************************/
u32bit BigInt::to_u32bit() const
{
if(is_negative())
throw Encoding_Error("BigInt::to_u32bit: Number is negative");
if(bits() >= 32)
throw Encoding_Error("BigInt::to_u32bit: Number is too big to convert");
u32bit out = 0;
for(u32bit j = 0; j != 4; ++j)
out = (out << 8) | byte_at(3-j);
return out;
}
Out round(X x)
{
static_assert(!std::is_integral<X>::value, "type must be non-integral");
static_assert(std::is_integral<Out>::value, "type must be integral");
if (static_cast<double>(x) < static_cast<double>(std::numeric_limits<Out>::lowest()))
return std::numeric_limits<Out>::lowest();
if (static_cast<double>(x) > static_cast<double>(std::numeric_limits<Out>::max()))
return std::numeric_limits<Out>::max();
if (is_negative(x))
x -= 1;
return static_cast<Out>(x + 0.5);
}
