diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index c8940fcc77..2fe66d9876 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -13,6 +13,8 @@ . ${R}etc/init.d/rc.rover_defaults + + param set-default BAT1_N_CELLS 4 param set-default EKF2_GBIAS_INIT 0.01 @@ -46,15 +48,15 @@ param set-default GND_SPEED_THR_SC 1 param set-default NAV_ACC_RAD 0.5 -# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians -param set-default GND_MAX_ANG 3.1415 - # Set geometry & output configration param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_DIS1 1500 -param set-default PWM_MAIN_DIS2 1500 -param set-default PWM_MAIN_TIM0 50 -param set-default PWM_MAIN_TIM1 50 + + +param set-default RBCLW_ADDRESS 130 +param set-default RBCLW_SER_CFG 202 +param set-default ROBOCLAW_FUNC1 101 +param set-default ROBOCLAW_FUNC2 102 +param set-default ROBOCLAW_REV 1 +param set-default SENS_EN_INA226 0 +param set-default SER_GPS2_BAUD 57600 diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index 28e0cee058..d4aeeee5d3 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -158,15 +158,15 @@ bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; - float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; + float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f; if (stop_motors) { - setMotorSpeed(Motor::Left, 0.f); setMotorSpeed(Motor::Right, 0.f); + setMotorSpeed(Motor::Left, 0.f); } else { - setMotorSpeed(Motor::Left, left_motor_output); setMotorSpeed(Motor::Right, right_motor_output); + setMotorSpeed(Motor::Left, left_motor_output); } return true; @@ -214,12 +214,12 @@ int Roboclaw::readEncoder() uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; uint8_t buffer_speed_left[ENCODER_SPEED_MESSAGE_SIZE]; - if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left, + if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { return ERROR; } - if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right, + if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { return ERROR; } @@ -228,10 +228,10 @@ int Roboclaw::readEncoder() return ERROR; } - int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]); int32_t speed_right = swapBytesInt32(&buffer_speed_right[0]); - int32_t position_left = swapBytesInt32(&buffer_positon[4]); + int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]); int32_t position_right = swapBytesInt32(&buffer_positon[0]); + int32_t position_left = swapBytesInt32(&buffer_positon[4]); wheel_encoders_s wheel_encoders{}; wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; @@ -249,7 +249,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value) Command command; // send command - if (motor == Motor::Left) { + if (motor == Motor::Right) { if (value > 0) { command = Command::DriveForwardMotor1; @@ -257,7 +257,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value) command = Command::DriveBackwardsMotor1; } - } else if (motor == Motor::Right) { + } else if (motor == Motor::Left) { if (value > 0) { command = Command::DriveForwardMotor2; @@ -277,10 +277,10 @@ void Roboclaw::setMotorDutyCycle(Motor motor, float value) Command command; // send command - if (motor == Motor::Left) { + if (motor == Motor::Right) { command = Command::DutyCycleMotor1; - } else if (motor == Motor::Right) { + } else if (motor == Motor::Left) { command = Command::DutyCycleMotor2; } else { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 2c506cc4e2..8683d7477d 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -961,10 +961,10 @@ mixer: actuators: - actuator_type: 'motor' instances: - - name: 'Left Motor' - position: [ 0, -1., 0 ] - name: 'Right Motor' position: [ 0, 1., 0 ] + - name: 'Left Motor' + position: [ 0, -1., 0 ] 7: # Motors (6DOF) actuators: