dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Roboclaw: Working temporary version that drives around

This commit is contained in:
PerFrivik 2023-10-10 16:04:20 +02:00 committed by Matthias Grob
parent 184993daa3
commit c7e780cb6d
4 changed files with 62 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/px4/fmu-v5x/default.px4board
View File

@ -15,7 +15,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_COMMON_DISTANCE_SENSOR=n
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_DRIVERS_GPIO_MCP23009=y

60
src/drivers/roboclaw/RoboClaw.cpp
View File

@ -136,16 +136,28 @@ RoboClaw::~RoboClaw()
close(_uart);
}
void
RoboClaw::vehicle_control_poll()
{
if (_vehicle_thrust_setpoint_sub.updated()) {
_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint);
}
if (_vehicle_torque_setpoint_sub.updated()) {
_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint);
}
}
void RoboClaw::taskMain()
{
// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
uint8_t rbuff[6];
int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
// uint8_t rbuff[6];
// int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
if (err_code <= 0) {
PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
return;
}
// if (err_code <= 0) {
// PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
// return;
// }
// This main loop performs two different tasks, asynchronously:
// - Send actuator_controls_0 to the Roboclaw as soon as they are available
@ -155,6 +167,9 @@ void RoboClaw::taskMain()
// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
// printf("i am in main");
int waitTime = 100_ms;
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
@ -173,8 +188,13 @@ void RoboClaw::taskMain()
while (!taskShouldExit) {
// printf("i am running, thrust %f \n", (double)vehicle_thrust_setpoint.xyz[0]);
int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
vehicle_control_poll();
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
_parameters_update();
@ -189,12 +209,14 @@ void RoboClaw::taskMain()
|| _actuatorArmed.force_failsafe;
if (disarmed) {
// printf("i am disarmed \n");
setMotorDutyCycle(MOTOR_1, 0.f);
setMotorDutyCycle(MOTOR_2, 0.f);
} else {
const float throttle = _actuatorControls.control[actuator_controls_s::INDEX_THROTTLE];
const float yaw = _actuatorControls.control[actuator_controls_s::INDEX_YAW];
const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change
// printf("thrust %f\n", (double)throttle);
const float throttle = (double)vehicle_torque_setpoint.xyz[2];
const float scale = 0.3f;
setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale);
setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale);
@ -485,7 +507,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
if (err_code <= 0) {
printf("select error: %d\n", err_code);
// printf("select error: %d\n", err_code);
return err_code;
}
@ -493,7 +515,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
//printf("Read: %d\n", err_code);
if (err_code <= 0) {
printf("read error: %d\n", err_code);
// printf("read error: %d\n", err_code);
return err_code;
} else {
@ -560,17 +582,17 @@ void RoboClaw::_parameters_update()
_parameters.serial_baud_rate = B2400;
break;
case 9600:
_parameters.serial_baud_rate = B9600;
break;
// case 9600:
// _parameters.serial_baud_rate = B9600;
// break;
case 19200:
_parameters.serial_baud_rate = B19200;
break;
// case 19200:
// _parameters.serial_baud_rate = B19200;
// break;
case 38400:
_parameters.serial_baud_rate = B38400;
break;
// case 38400:
// _parameters.serial_baud_rate = B38400;
// break;
case 57600:
_parameters.serial_baud_rate = B57600;

22
src/drivers/roboclaw/RoboClaw.hpp
View File

@ -47,15 +47,20 @@
#include <stdio.h>
#include <termios.h>
#include <lib/parameters/param.h>
#include <drivers/device/i2c.h>
#include <sys/select.h>
#include <sys/time.h>
#include <pthread.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wheel_encoders.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/device/i2c.h>
#include <sys/select.h>
#include <sys/time.h>
#include <pthread.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/Subscription.hpp>
/**
* This is a driver for the RoboClaw motor controller
@ -205,6 +210,14 @@ private:
int _paramSub{-1};
parameter_update_s _paramUpdate;
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)};
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
uORB::PublicationMulti<wheel_encoders_s> _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)};
wheel_encoders_s _wheelEncoderMsg[2];
@ -213,6 +226,7 @@ private:
int32_t _motorSpeeds[2] {0, 0};
void _parameters_update();
void vehicle_control_poll();
static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
int _sendUnsigned7Bit(e_command command, float data);

4
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -113,7 +113,7 @@ RoverPositionControl::vehicle_control_mode_poll()
void
RoverPositionControl::manual_control_setpoint_poll()
{
if (_control_mode.flag_control_manual_enabled) {
if (true) {
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f);
@ -431,7 +431,7 @@ RoverPositionControl::Run()
matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon);
matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
if (false) {
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {