void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _throttle_radio_min;
}
}
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle);
}
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// send output to each motor
hal.rcout->cork();
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
}
}
hal.rcout->push();
}
// output_test_num - spin a motor connected to the specified output channel
// (should only be performed during testing)
// If a motor output channel is remapped, the mapped channel is used.
// Returns true if motor output is set, false otherwise
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
{
if (!armed()) {
return false;
}
// Is channel in supported range?
if (output_channel > AP_MOTORS_MAX_NUM_MOTORS -1) {
return false;
}
// Is motor enabled?
if (!motor_enabled[output_channel]) {
return false;
}
rc_write(output_channel, pwm); // output
return true;
}
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
switch (_spool_mode) {
case SHUT_DOWN: {
// no output
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_actuator[i] = 0.0f;
}
}
break;
}
case GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
}
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
}
}
break;
}
// convert output to PWM and send to each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, output_to_pwm(_actuator[i]));
}
}
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Dual::output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// swash servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// swash servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// swash servo 4
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// swash servo 5
rc_write(AP_MOTORS_MOT_5, pwm);
break;
case 6:
// swash servo 6
rc_write(AP_MOTORS_MOT_6, pwm);
break;
case 7:
// main rotor
rc_write(AP_MOTORS_HELI_DUAL_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// output_min - sends minimum values out to the motors
void AP_Motors6DOF::output_min()
{
int8_t i;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
//limit.throttle_lower = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
// fill the motor_out[] array for HIL use and send minimum value to each motor
// ToDo find a field to store the minimum pwm instead of hard coding 1500
hal.rcout->cork();
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
rc_write(i, 1500);
//rc_write(i, _throttle_radio_min);
}
}
hal.rcout->push();
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Single::output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// swash servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// swash servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
if (_acro_tail && _ext_gyro_gain_acro > 0) {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_acro);
} else {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_std);
}
}
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// main rotor
rc_write(AP_MOTORS_HELI_SINGLE_RSC, pwm);
break;
default:
// do nothing
break;
}
}
// output a thrust to all motors that match a given motor mask. This
// is used to control motors enabled for forward flight. Thrust is in
// the range 0 to 1
void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
{
const int16_t pwm_min = get_pwm_output_min();
const int16_t pwm_range = get_pwm_output_max() - pwm_min;
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
int16_t motor_out;
if (mask & (1U<<i)) {
/*
apply rudder mixing differential thrust
copter frame roll is plane frame yaw (this is only
used by tiltrotors and tailsitters)
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f);
} else {
motor_out = pwm_min;
}
rc_write(i, motor_out);
}
}
}
// move_yaw
void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out)
{
_yaw_servo.servo_out = constrain_int16(yaw_out, -4500, 4500);
if (_yaw_servo.servo_out != yaw_out) {
limit.yaw = true;
}
_yaw_servo.calc_pwm();
rc_write(AP_MOTORS_MOT_4, _yaw_servo.radio_out);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro);
} else {
write_aux(_ext_gyro_gain_std);
}
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) {
// output yaw servo to tail rsc
write_aux(_yaw_servo.servo_out);
}
}
void AP_MotorsSingle::output_to_motors()
{
if (!_flags.initialised_ok) {
return;
}
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
rc_write(AP_MOTORS_MOT_5, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(0));
break;
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle());
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
}
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_out));
rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5]));
rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6]));
break;
}
}
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsSingle::output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// flap servo 1
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 2:
// flap servo 2
rc_write(AP_MOTORS_MOT_2, pwm);
break;
case 3:
// flap servo 3
rc_write(AP_MOTORS_MOT_3, pwm);
break;
case 4:
// flap servo 4
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 5:
// spin motor 1
rc_write(AP_MOTORS_MOT_5, pwm);
break;
case 6:
// spin motor 2
rc_write(AP_MOTORS_MOT_6, pwm);
break;
default:
// do nothing
break;
}
}
//
// move_actuators - moves swash plate and tail rotor
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
{
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified cyclic_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
float total_out = pythagorous2((float)pitch_out, (float)roll_out);
if (total_out > _cyclic_max) {
float ratio = (float)_cyclic_max / total_out;
roll_out *= ratio;
pitch_out *= ratio;
limit.roll_pitch = true;
}
// constrain collective input
_collective_out = coll_in;
if (_collective_out <= 0) {
_collective_out = 0;
limit.throttle_lower = true;
}
if (_collective_out >= 1000) {
_collective_out = 1000;
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && _collective_out < _land_collective_min) {
_collective_out = _land_collective_min;
limit.throttle_lower = true;
}
// scale collective pitch
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
// if servo output not in manual mode, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
// rudder feed forward based on collective
// the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
// also not required if we are using external gyro
if ((_main_rotor.get_control_output() > _rsc_idle_output) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
}
} else {
yaw_offset = 0;
}
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
// ToDo: add main rotor cyclic power?
_main_rotor_power = ((float)(abs(_collective_out - _collective_mid_pwm)) / _collective_range);
_main_rotor.set_motor_load(_main_rotor_power);
// swashplate servos
_swash_servo_1.servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_swash_servo_1.radio_trim-1500);
_swash_servo_2.servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_swash_servo_2.radio_trim-1500);
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
_swash_servo_1.servo_out += 500;
_swash_servo_2.servo_out += 500;
}
_swash_servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_swash_servo_3.radio_trim-1500);
// use servo_out to calculate pwm_out and radio_out
_swash_servo_1.calc_pwm();
_swash_servo_2.calc_pwm();
_swash_servo_3.calc_pwm();
hal.rcout->cork();
// actually move the servos
rc_write(AP_MOTORS_MOT_1, _swash_servo_1.radio_out);
rc_write(AP_MOTORS_MOT_2, _swash_servo_2.radio_out);
rc_write(AP_MOTORS_MOT_3, _swash_servo_3.radio_out);
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
hal.rcout->push();
}
void AP_MotorsTri::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_2, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_thrust_to_pwm(_thrust_right));
rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left));
rc_write(AP_MOTORS_MOT_4, calc_thrust_to_pwm(_thrust_rear));
rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg)));
break;
}
}
// write_aux - converts servo_out parameter value (0 to 1 range) to pwm and outputs to aux channel (ch7)
void AP_MotorsHeli_Single::write_aux(float servo_out)
{
if (_servo_aux) {
rc_write(AP_MOTORS_HELI_SINGLE_AUX, calc_pwm_output_0to1(servo_out, _servo_aux));
}
}
// write_aux - outputs pwm onto output aux channel (ch7)
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli_Single::write_aux(int16_t servo_out)
{
_servo_aux.servo_out = servo_out;
_servo_aux.calc_pwm();
rc_write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out);
}
static void
daemonize (void)
{
int fork_rv;
int i;
int fd;
char *pid;
/* We never daemonize when we initialize from a dump file. */
if (rcd_options_get_dump_file () != NULL)
return;
if (rcd_options_get_non_daemon_flag ())
return;
#ifdef NEED_KERNEL_FD_WORKAROUND
/*
* This is an evil hack and I hate it, but it works around a broken ass
* kernel bug.
*/
for (i = 0; i < 256; i++) fopen ("/dev/null", "r");
#endif
fork_rv = fork ();
if (fork_rv < 0) {
g_printerr ("rcd: fork failed!\n");
exit (-1);
}
/* The parent exits. */
if (fork_rv > 0)
exit (0);
/* Clear out the PID, just in case we are daemonizing late. */
pid_for_messages = 0;
/* A daemon should always be in its own process group. */
setsid ();
/* Change our CWD to / */
chdir ("/");
/* Close all file descriptors. */
for (i = getdtablesize (); i >= 0; --i)
close (i);
fd = open ("/dev/null", O_RDWR); /* open /dev/null as stdin */
g_assert (fd == STDIN_FILENO);
if (! g_file_test ("/var/log/rcd", G_FILE_TEST_EXISTS)) {
if (mkdir ("/var/log/rcd",
S_IRUSR | S_IWUSR | S_IXUSR |
S_IRGRP | S_IXGRP |
S_IROTH | S_IXOTH) != 0) {
rc_debug (RC_DEBUG_LEVEL_WARNING,
"Can't create directory '/var/log/rcd'");
}
}
/* Open a new file for our logging file descriptor. This
will be the fd 1, stdout. */
fd = open ("/var/log/rcd/rcd-messages",
O_WRONLY | O_CREAT | O_APPEND,
S_IRUSR | S_IWUSR);
g_assert (fd == STDOUT_FILENO);
fd = dup (fd); /* dup fd to stderr */
g_assert (fd == STDERR_FILENO);
/* Open /var/run/rcd.pid and write out our PID */
fd = open ("/var/run/rcd.pid", O_WRONLY | O_CREAT | O_TRUNC, 0644);
pid = g_strdup_printf ("%d", getpid ());
rc_write (fd, pid, strlen (pid));
g_free (pid);
close (fd);
rcd_shutdown_add_handler ((RCDShutdownFn) unlink, "/var/run/rcd.pid");
rcd_shutdown_add_handler ((RCDShutdownFn) unlink, "/var/lock/subsys/rcd");
}
//
// move_actuators - moves swash plate and tail rotor
// - expected ranges:
// roll : -1 ~ +1
// pitch: -1 ~ +1
// collective: 0 ~ 1
// yaw: -1 ~ +1
//
void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)
{
float yaw_offset = 0.0f;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_heliflags.inverted_flight) {
coll_in = 1 - coll_in;
}
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified cyclic_max
// coming into this equation at 4500 or less
float total_out = norm(pitch_out, roll_out);
if (total_out > (_cyclic_max/4500.0f)) {
float ratio = (float)(_cyclic_max/4500.0f) / total_out;
roll_out *= ratio;
pitch_out *= ratio;
limit.roll_pitch = true;
}
// constrain collective input
float collective_out = coll_in;
if (collective_out <= 0.0f) {
collective_out = 0.0f;
limit.throttle_lower = true;
}
if (collective_out >= 1.0f) {
collective_out = 1.0f;
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) {
collective_out = (_land_collective_min/1000.0f);
limit.throttle_lower = true;
}
// if servo output not in manual mode, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
// rudder feed forward based on collective
// the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
// also not required if we are using external gyro
if ((_main_rotor.get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
// the 4.5 scaling factor is to bring the values in line with previous releases
yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct) / 4.5f;
}
} else {
yaw_offset = 0.0f;
}
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
// ToDo: add main rotor cyclic power?
if (collective_out > _collective_mid_pct) {
// +ve motor load for +ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / (1.0f - _collective_mid_pct));
} else {
// -ve motor load for -ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / _collective_mid_pct);
}
// swashplate servos
float collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)/1000.0f;
float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * coll_out_scaled;
float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * coll_out_scaled;
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
servo1_out += 0.5f;
servo2_out += 0.5f;
}
float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * coll_out_scaled;
// rescale from -1..1, so we can use the pwm calc that includes trim
servo1_out = 2*servo1_out - 1;
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
}
int cli_frontend_init (int argc, char **argv)
{
machine_config drv;
char buffer[128];
char *cmd_name;
int game_index;
int i;
gamename = NULL;
game_index = -1;
/* clear all core options */
memset(&options,0,sizeof(options));
/* create the rc object */
rc = cli_rc_create();
if (!rc)
{
osd_die ("error on rc creation\n");
}
/* parse the commandline */
got_gamename = 0;
if (rc_parse_commandline(rc, argc, argv, 2, config_handle_arg))
{
osd_die ("error while parsing cmdline\n");
}
/* determine global configfile name */
cmd_name = win_strip_extension(win_basename(argv[0]));
if (!cmd_name)
{
osd_die ("who am I? cannot determine the name I was called with\n");
}
sprintf (buffer, "%s.ini", cmd_name);
/* parse mame.ini/mess.ini even if called with another name */
if (mame_stricmp(cmd_name, APPNAME) != 0)
{
if (parse_config (APPNAME".ini", NULL))
exit(1);
}
/* parse cmd_name.ini */
if (parse_config (buffer, NULL))
exit(1);
#ifdef MAME_DEBUG
if (parse_config( "debug.ini", NULL))
exit(1);
#endif
/* if requested, write out cmd_name.ini (normally "mame.ini") */
if (createconfig)
{
rc_save(rc, buffer, 0);
exit(0);
}
if (showconfig)
{
sprintf (buffer, " %s running parameters", cmd_name);
rc_write(rc, stdout, buffer);
exit(0);
}
if (showusage)
{
fprintf(stdout, "Usage: %s [" GAMENOUN "] [options]\n" "Options:\n", cmd_name);
/* actual help message */
rc_print_help(rc, stdout);
exit(0);
}
/* no longer needed */
free(cmd_name);
/* handle playback */
if (playbackname != NULL)
{
options.playback = mame_fopen(playbackname,0,FILETYPE_INPUTLOG,0);
if (!options.playback)
{
osd_die("failed to open %s for playback\n", playbackname);
}
}
/* check for game name embedded in .inp header */
if (options.playback)
{
inp_header inp_header;
/* read playback header */
mame_fread(options.playback, &inp_header, sizeof(inp_header));
if (!isalnum(inp_header.name[0])) /* If first byte is not alpha-numeric */
mame_fseek(options.playback, 0, SEEK_SET); /* old .inp file - no header */
else
{
for (i = 0; (drivers[i] != 0); i++) /* find game and play it */
{
if (strcmp(drivers[i]->name, inp_header.name) == 0)
{
game_index = i;
gamename = (char *)drivers[i]->name;
printf("Playing back previously recorded " GAMENOUN " %s (%s) [press return]\n",
drivers[game_index]->name,drivers[game_index]->description);
getchar();
break;
}
}
}
}
/* check for frontend options, horrible 1234 hack */
if (frontend_help(gamename) != 1234)
exit(0);
gamename = win_basename(gamename);
gamename = win_strip_extension(gamename);
/* if not given by .inp file yet */
if (game_index == -1)
{
/* do we have a driver for this? */
for (i = 0; drivers[i]; i++)
if (mame_stricmp(gamename,drivers[i]->name) == 0)
{
game_index = i;
break;
}
}
#ifdef MAME_DEBUG
if (game_index == -1)
{
/* pick a random game */
if (strcmp(gamename,"random") == 0)
{
i = 0;
while (drivers[i]) i++; /* count available drivers */
srand(time(0));
/* call rand() once to get away from the seed */
rand();
game_index = rand() % i;
fprintf(stderr, "running %s (%s) [press return]",drivers[game_index]->name,drivers[game_index]->description);
getchar();
}
}
#endif
/* we give up. print a few approximate matches */
if (game_index == -1)
{
fprintf(stderr, "\n\"%s\" approximately matches the following\n"
"supported " GAMESNOUN " (best match first):\n\n", gamename);
show_approx_matches();
exit(1);
}
/* ok, got a gamename */
/* if this is a vector game, parse vector.ini first */
expand_machine_driver(drivers[game_index]->drv, &drv);
if (drv.video_attributes & VIDEO_TYPE_VECTOR)
if (parse_config ("vector.ini", NULL))
exit(1);
/* nice hack: load source_file.ini (omit if referenced later any) */
{
const game_driver *tmp_gd;
const char *start;
/* remove the path and the .c suffix from the source file name */
start = strrchr(drivers[game_index]->source_file, '/');
if (!start)
start = strrchr(drivers[game_index]->source_file, '\\');
if (!start)
start = drivers[game_index]->source_file - 1;
sprintf(buffer, "%s", start + 1);
buffer[strlen(buffer) - 2] = 0;
tmp_gd = drivers[game_index];
while (tmp_gd != NULL)
{
if (strcmp(tmp_gd->name, buffer) == 0) break;
tmp_gd = tmp_gd->clone_of;
}
if (tmp_gd == NULL)
/* not referenced later, so load it here */
{
strcat(buffer, ".ini");
if (parse_config (buffer, NULL))
exit(1);
}
}
/* now load gamename.ini */
/* this possibly checks for clonename.ini recursively! */
if (parse_config (NULL, drivers[game_index]))
exit(1);
/* handle record option */
if (recordname)
{
options.record = mame_fopen(recordname,0,FILETYPE_INPUTLOG,1);
if (!options.record)
{
osd_die("failed to open %s for recording\n", recordname);
}
}
if (options.record)
{
inp_header inp_header;
memset(&inp_header, '\0', sizeof(inp_header));
strcpy(inp_header.name, drivers[game_index]->name);
/* MAME32 stores the MAME version numbers at bytes 9 - 11
* MAME DOS keeps this information in a string, the
* Windows code defines them in the Makefile.
*/
/*
inp_header.version[0] = 0;
inp_header.version[1] = VERSION;
inp_header.version[2] = BETA_VERSION;
*/
mame_fwrite(options.record, &inp_header, sizeof(inp_header));
}
if (statename)
options.savegame = statename;
#if defined(MAME_DEBUG) && defined(NEW_DEBUGGER)
if (debugscript)
debug_source_script(debugscript);
#endif
/* need a decent default for debug width/height */
if (options.debug_width == 0)
options.debug_width = 640;
if (options.debug_height == 0)
options.debug_height = 480;
options.debug_depth = 8;
/* no sound is indicated by a 0 samplerate */
if (!enable_sound)
options.samplerate = 0;
/* set the artwork options */
options.use_artwork = ARTWORK_USE_ALL;
if (use_backdrops == 0)
options.use_artwork &= ~ARTWORK_USE_BACKDROPS;
if (use_overlays == 0)
options.use_artwork &= ~ARTWORK_USE_OVERLAYS;
if (use_bezels == 0)
options.use_artwork &= ~ARTWORK_USE_BEZELS;
if (!use_artwork)
options.use_artwork = ARTWORK_USE_NONE;
{
/* first start with the game's built in orientation */
int orientation = drivers[game_index]->flags & ORIENTATION_MASK;
options.ui_orientation = orientation;
if (options.ui_orientation & ORIENTATION_SWAP_XY)
{
/* if only one of the components is inverted, switch them */
if ((options.ui_orientation & ROT180) == ORIENTATION_FLIP_X ||
(options.ui_orientation & ROT180) == ORIENTATION_FLIP_Y)
options.ui_orientation ^= ROT180;
}
/* override if no rotation requested */
if (video_norotate)
orientation = options.ui_orientation = ROT0;
/* rotate right */
if (video_ror)
{
/* if only one of the components is inverted, switch them */
if ((orientation & ROT180) == ORIENTATION_FLIP_X ||
(orientation & ROT180) == ORIENTATION_FLIP_Y)
orientation ^= ROT180;
orientation ^= ROT90;
}
/* rotate left */
if (video_rol)
{
/* if only one of the components is inverted, switch them */
if ((orientation & ROT180) == ORIENTATION_FLIP_X ||
(orientation & ROT180) == ORIENTATION_FLIP_Y)
orientation ^= ROT180;
orientation ^= ROT270;
}
/* auto-rotate right (e.g. for rotating lcds), based on original orientation */
if (video_autoror && (drivers[game_index]->flags & ORIENTATION_SWAP_XY) )
{
/* if only one of the components is inverted, switch them */
if ((orientation & ROT180) == ORIENTATION_FLIP_X ||
(orientation & ROT180) == ORIENTATION_FLIP_Y)
orientation ^= ROT180;
orientation ^= ROT90;
}
/* auto-rotate left (e.g. for rotating lcds), based on original orientation */
if (video_autorol && (drivers[game_index]->flags & ORIENTATION_SWAP_XY) )
{
/* if only one of the components is inverted, switch them */
if ((orientation & ROT180) == ORIENTATION_FLIP_X ||
(orientation & ROT180) == ORIENTATION_FLIP_Y)
orientation ^= ROT180;
orientation ^= ROT270;
}
/* flip X/Y */
if (video_flipx)
orientation ^= ORIENTATION_FLIP_X;
if (video_flipy)
orientation ^= ORIENTATION_FLIP_Y;
blit_flipx = ((orientation & ORIENTATION_FLIP_X) != 0);
blit_flipy = ((orientation & ORIENTATION_FLIP_Y) != 0);
blit_swapxy = ((orientation & ORIENTATION_SWAP_XY) != 0);
if( options.vector_width == 0 && options.vector_height == 0 )
{
options.vector_width = 640;
options.vector_height = 480;
}
if( blit_swapxy )
{
int temp;
temp = options.vector_width;
options.vector_width = options.vector_height;
options.vector_height = temp;
}
}
return game_index;
}
void AP_MotorsSingle::output_to_motors()
{
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough+_yaw_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough+_yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough+_yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough+_yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
break;
}
}
void AP_MotorsQuad::output(){
AP_MotorsMatrix::output();
rc_write(AP_MOTORS_MAX_NUM_MOTORS - 1, 1500.0f + (_tilt_pitch / 45.0f) * 500.0f);
}
void AP_MotorsSingle::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough - _yaw_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough - _yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough - _yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough - _yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
break;
}
}
//
// move_actuators - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -1 ~ +1
// pitch: -1 ~ +1
// collective: 0 ~ 1
// yaw: -1 ~ +1
//
void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
if (pitch_out < -_cyclic_max/4500.0f) {
pitch_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true;
}
if (pitch_out > _cyclic_max/4500.0f) {
pitch_out = _cyclic_max/4500.0f;
limit.roll_pitch = true;
}
} else {
if (roll_out < -_cyclic_max/4500.0f) {
roll_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true;
}
if (roll_out > _cyclic_max/4500.0f) {
roll_out = _cyclic_max/4500.0f;
limit.roll_pitch = true;
}
}
float yaw_compensation = 0.0f;
// if servo output not in manual mode, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
// add differential collective pitch yaw compensation
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
yaw_compensation = _dcp_yaw_effect * roll_out;
} else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM
yaw_compensation = _dcp_yaw_effect * pitch_out;
}
yaw_out = yaw_out + yaw_compensation;
}
// scale yaw and update limits
if (yaw_out < -_cyclic_max/4500.0f) {
yaw_out = -_cyclic_max/4500.0f;
limit.yaw = true;
}
if (yaw_out > _cyclic_max/4500.0f) {
yaw_out = _cyclic_max/4500.0f;
limit.yaw = true;
}
// constrain collective input
float collective_out = collective_in;
if (collective_out <= 0.0f) {
collective_out = 0.0f;
limit.throttle_lower = true;
}
if (collective_out >= 1.0f) {
collective_out = 1.0f;
limit.throttle_upper = true;
}
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
collective2_out = _collective2_mid_pct;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) {
collective_out = _land_collective_min/1000.0f;
limit.throttle_lower = true;
}
// scale collective pitch for front swashplate (servos 1,2,3)
float collective_scaler = ((float)(_collective_max-_collective_min))/1000.0f;
float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)/1000.0f;
// scale collective pitch for rear swashplate (servos 4,5,6)
float collective2_scaler = ((float)(_collective2_max-_collective2_min))/1000.0f;
float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)/1000.0f;
// feed power estimate into main rotor controller
// ToDo: add main rotor cyclic power?
_rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct));
// swashplate servos
float servo1_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)/0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
float servo2_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)/0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
float servo3_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)/0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
float servo4_out = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)/0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
float servo5_out = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)/0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
float servo6_out = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)/0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
// rescale from -1..1, so we can use the pwm calc that includes trim
servo1_out = 2*servo1_out - 1;
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
servo4_out = 2*servo4_out - 1;
servo5_out = 2*servo5_out - 1;
servo6_out = 2*servo6_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(servo4_out, _swash_servo_4));
rc_write(AP_MOTORS_MOT_5, calc_pwm_output_1to1(servo5_out, _swash_servo_5));
rc_write(AP_MOTORS_MOT_6, calc_pwm_output_1to1(servo6_out, _swash_servo_6));
}
static void ramcloudIncr(struct ramcloudData *d, wstr key, uint64_t num, int positive)
{
if (num > INT64_MAX) {
wheatLog(WHEAT_WARNING, "%s num %ld can't larger than %ld", __func__, num, INT64_MAX);
sliceTo(&d->storage_response, (uint8_t*)CLIENT_ERROR, sizeof(CLIENT_ERROR)-1);
return ;
}
static int max_len = 100;
char value[max_len];
uint32_t actual_len;
uint64_t version;
uint16_t flag;
again:
wheatLog(WHEAT_DEBUG, "%s incr key %s %ld", __func__, key, num);
Status s = rc_read(global.client, d->table_id, key, wstrlen(key), NULL, &version, value, max_len, &actual_len);
if (s != STATUS_OK) {
if (s != STATUS_OBJECT_DOESNT_EXIST) {
wheatLog(WHEAT_WARNING, " failed to read %s: %s", key, statusToString(s));
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
} else {
sliceTo(&d->storage_response, (uint8_t*)NOT_FOUND, sizeof(NOT_FOUND)-1);
}
return ;
}
if (actual_len > max_len) {
sliceTo(&d->storage_response, (uint8_t*)CLIENT_ERROR, sizeof(CLIENT_ERROR)-1);
return ;
}
flag = *(uint16_t*)&value[actual_len-FLAG_SIZE];
value[actual_len-FLAG_SIZE] = '\0';
long long n = atoll(value);
if (n == 0) {
if (value[0] != '0') {
sliceTo(&d->storage_response, (uint8_t*)CLIENT_ERROR, sizeof(CLIENT_ERROR)-1);
return ;
}
}
if (positive) {
n += num;
} else {
if (num > n)
n = 0;
else
n -= num;
}
int l = sprintf(value, "%llu", n);
memcpy(&value[l], &flag, FLAG_SIZE);
struct RejectRules rule;
memset(&rule, 0, sizeof(rule));
rule.doesntExist = 1;
rule.versionNeGiven = 1;
rule.givenVersion = version;
s = rc_write(global.client, d->table_id, key, wstrlen(key), value, l+FLAG_SIZE,
&rule, &version);
if (s != STATUS_OK) {
if (s == STATUS_WRONG_VERSION) {
goto again;
}
if (s == STATUS_OBJECT_DOESNT_EXIST) {
sliceTo(&d->storage_response, (uint8_t*)NOT_FOUND, sizeof(NOT_FOUND)-1);
return ;
}
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
wheatLog(WHEAT_WARNING, "%s failed to incr %s: %s", __func__, key, statusToString(s));
return ;
} else {
l = sprintf(value, "%llu\r\n", n);
d->retrieval_response = wstrNewLen(value, l);
sliceTo(&d->storage_response, (uint8_t*)d->retrieval_response, wstrlen(d->retrieval_response));
}
}
static void ramcloudAppend(struct ramcloudData *d, wstr key, struct array *vals, uint32_t vlen, uint16_t flag, int append)
{
uint32_t actual_len;
uint64_t version;
uint64_t len = RAMCLOUD_DEFAULT_VALUE_LEN;
again:
wheatLog(WHEAT_DEBUG, "%s read key %s", __func__, key);
wstr origin_val = wstrNewLen(NULL, len);
wstr val;
Status s = rc_read(global.client, d->table_id, key, wstrlen(key), NULL, &version, origin_val, len, &actual_len);
if (s != STATUS_OK) {
if (s != STATUS_OBJECT_DOESNT_EXIST) {
wheatLog(WHEAT_WARNING, " failed to read %s: %s", key, statusToString(s));
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
} else {
sliceTo(&d->storage_response, (uint8_t*)NOT_STORED, sizeof(NOT_STORED)-1);
}
wstrFree(origin_val);
return ;
}
while (actual_len > len) {
wstrFree(origin_val);
len = actual_len;
origin_val = wstrNewLen(NULL, actual_len);
if (origin_val == NULL) {
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
wheatLog(WHEAT_WARNING, " failed to alloc memory");
return ;
}
s = rc_read(global.client, d->table_id, key, wstrlen(key), NULL, &version, origin_val, len, &actual_len);
if (s != STATUS_OK) {
if (s != STATUS_OBJECT_DOESNT_EXIST) {
wheatLog(WHEAT_WARNING, " failed to read %s: %s", key, statusToString(s));
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
} else {
sliceTo(&d->storage_response, (uint8_t*)NOT_STORED, sizeof(NOT_STORED)-1);
}
wstrFree(origin_val);
return ;
}
}
wstrupdatelen(origin_val, actual_len-FLAG_SIZE);
if (append) {
val = origin_val;
// reduce flag size
} else {
val = wstrNewLen(NULL, vlen);
}
int i = 0;
while (i < narray(vals)) {
struct slice *slice;
slice = arrayIndex(vals, i);
val = wstrCatLen(val, (char*)slice->data, slice->len);
len += slice->len;
++i;
}
if (!append) {
val = wstrCatLen(val, origin_val, wstrlen(origin_val));
wstrFree(origin_val);
}
val = wstrCatLen(val, (char*)&flag, FLAG_SIZE);
struct RejectRules rule;
memset(&rule, 0, sizeof(rule));
rule.doesntExist = 1;
rule.versionNeGiven = 1;
rule.givenVersion = version;
s = rc_write(global.client, d->table_id, key, wstrlen(key), val, wstrlen(val),
&rule, NULL);
wstrFree(val);
if (s != STATUS_OK) {
if (s == STATUS_OBJECT_DOESNT_EXIST) {
sliceTo(&d->storage_response, (uint8_t*)NOT_STORED, sizeof(NOT_STORED)-1);
return ;
} else if (s == STATUS_WRONG_VERSION) {
goto again;
}
wheatLog(WHEAT_WARNING, " failed to write %s: %s", key, statusToString(s));
sliceTo(&d->storage_response, (uint8_t*)SERVER_ERROR, sizeof(SERVER_ERROR)-1);
return ;
}
sliceTo(&d->storage_response, (uint8_t*)STORED, sizeof(STORED)-1);
}
