Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: fixed motor output for Disco

Browse Source
This commit is contained in:
Andrew Tridgell 2016-06-08 21:14:11 +10:00
parent 9b057ee4f1
commit 298f7bffb9
4 changed files with 100 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
View File

@ -52,9 +52,11 @@ struct PACKED bldc_info {
};
#define BEBOP_BLDC_TOGGLE_GPIO 0x4d
#define BEBOP_BLDC_GPIO_RESET (1 << 0)
#define BEBOP_BLDC_GPIO_RED (1 << 1)
#define BEBOP_BLDC_GPIO_GREEN (1 << 2)
#define BEBOP_BLDC_GPIO_0 (1 << 0)
#define BEBOP_BLDC_GPIO_1 (1 << 1)
#define BEBOP_BLDC_GPIO_2 (1 << 2)
#define BEBOP_BLDC_GPIO_3 (1 << 3)
#define BEBOP_BLDC_GPIO_POWER (1 << 4)
#define BEBOP_BLDC_STOP_PROP 0x60
@ -66,10 +68,11 @@ struct PACKED bldc_info {
#define BEBOP_BLDC_MIN_PERIOD_US 1100
#define BEBOP_BLDC_MAX_PERIOD_US 1900
#define BEBOP_BLDC_MIN_RPM 3000
#define BEBOP_BLDC_MIN_RPM 1000
/* the max rpm speed is different on Bebop 2 */
#define BEBOP_BLDC_MAX_RPM_1 11000
#define BEBOP_BLDC_MAX_RPM_2 12200
#define BEBOP_BLDC_MAX_RPM_DISCO 12500
/* Priority of the thread controlling the BLDC via i2c
* set to 14, which is the same as the UART
@ -83,6 +86,14 @@ enum {
BEBOP_BLDC_STOPPED,
};
/* values of bottom nibble of the obs data status byte */
enum BLDC_STATUS {
BEBOP_BLDC_STATUS_STOPPED=1,
BEBOP_BLDC_STATUS_RAMPUP=2,
BEBOP_BLDC_STATUS_RUNNING=4,
BEBOP_BLDC_STATUS_RAMPDOWN=5
};
using namespace Linux;
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
@ -125,7 +136,12 @@ void RCOutput_Bebop::_start_prop()
void RCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM])
{
struct bldc_ref_speed_data data;
struct PACKED bldc_ref_speed_data {
uint8_t cmd;
uint16_t rpm[BEBOP_BLDC_MOTORS_NUM];
uint8_t enable_security;
uint8_t checksum;
} data {};
int i;
data.cmd = BEBOP_BLDC_SETREFSPEED;
@ -164,8 +180,25 @@ bool RCOutput_Bebop::_get_info(struct bldc_info *info)
int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
{
struct bldc_obs_data data;
int i;
/*
the structure returned is different on the Disco from the Bebop
*/
struct PACKED bldc_obs_data {
uint16_t rpm[BEBOP_BLDC_MOTORS_NUM];
uint16_t batt_mv;
uint8_t status;
uint8_t error;
uint8_t motors_err;
uint8_t temp;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
/*
bit 0 indicates an overcurrent on the RC receiver port when high
bits #1-#6 indicate an overcurrent on the #1-#6 PPM servos
*/
uint8_t overcurrent;
#endif
uint8_t checksum;
} data;
memset(&data, 0, sizeof(data));
if (!_dev->get_semaphore()->take(0)) {
@ -175,12 +208,14 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
_dev->read_registers(BEBOP_BLDC_GETOBSDATA, (uint8_t *)&data, sizeof(data));
_dev->get_semaphore()->give();
if (data.checksum != _checksum((uint8_t *)&data, sizeof(data) - 1)) {
if (data.checksum != _checksum((uint8_t*)&data, sizeof(data)-1)) {
return -EBUSY;
}
memset(&obs, 0, sizeof(obs));
/* fill obs class */
for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) {
for (uint8_t i = 0; i < _n_motors; i++) {
/* extract 'rpm saturation bit' */
obs.rpm_saturated[i] = (data.rpm[i] & (1 << 7)) ? 1 : 0;
/* clear 'rpm saturation bit' */
@ -189,6 +224,25 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
if (obs.rpm[i] == 0) {
obs.rpm_saturated[i] = 0;
}
#if 0
printf("rpm %u %u %u %u status 0x%02x temp %u\n",
obs.rpm[i], _rpm[0], _period_us[0], _period_us_to_rpm(_period_us[0]),
(unsigned)data.status,
(unsigned)data.temp);
#endif
}
// sync our state from status. This makes us more robust to i2c errors
enum BLDC_STATUS bldc_status = (enum BLDC_STATUS)(data.status & 0x0F);
switch (bldc_status) {
case BEBOP_BLDC_STATUS_STOPPED:
case BEBOP_BLDC_STATUS_RAMPDOWN:
_state = BEBOP_BLDC_STOPPED;
break;
case BEBOP_BLDC_STATUS_RAMPUP:
case BEBOP_BLDC_STATUS_RUNNING:
_state = BEBOP_BLDC_STARTED;
break;
}
obs.batt_mv = be16toh(data.batt_mv);
@ -213,7 +267,6 @@ void RCOutput_Bebop::_toggle_gpio(uint8_t mask)
void RCOutput_Bebop::_stop_prop()
{
uint8_t data = BEBOP_BLDC_STOP_PROP;
_state = BEBOP_BLDC_STOPPED;
if (!_dev->get_semaphore()->take(0)) {
return;
@ -247,8 +300,8 @@ void RCOutput_Bebop::_play_sound(uint8_t sound)
uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us)
{
period_us = constrain_int16(period_us, _min_pwm, _max_pwm);
float period_us_fl = period_us;
float rpm_fl = (period_us_fl - _min_pwm)/(_max_pwm - _min_pwm) *
(_max_rpm - BEBOP_BLDC_MIN_RPM) + BEBOP_BLDC_MIN_RPM;
@ -299,6 +352,9 @@ void RCOutput_Bebop::init()
/* Set an initial dummy frequency */
_frequency = 50;
// enable servo power (also receiver power)
_toggle_gpio(BEBOP_BLDC_GPIO_2 | BEBOP_BLDC_GPIO_POWER);
exit:
pthread_mutex_unlock(&_mutex);
return;
@ -348,7 +404,7 @@ void RCOutput_Bebop::push()
memcpy(_period_us, _request_period_us, sizeof(_period_us));
pthread_cond_signal(&_cond);
pthread_mutex_unlock(&_mutex);
memset(_request_period_us, 0 ,sizeof(_request_period_us));
memset(_request_period_us, 0, sizeof(_request_period_us));
}
uint16_t RCOutput_Bebop::read(uint8_t ch)
@ -388,9 +444,7 @@ void RCOutput_Bebop::_run_rcout()
int ret;
struct timespec ts;
struct bldc_info info;
uint8_t bebop_bldc_channels[BEBOP_BLDC_MOTORS_NUM];
uint8_t bebop_bldc_right_front, bebop_bldc_left_front,
bebop_bldc_left_back, bebop_bldc_right_back;
uint8_t bebop_bldc_channels[BEBOP_BLDC_MOTORS_NUM] {};
int hw_version;
memset(current_period_us, 0, sizeof(current_period_us));
@ -399,11 +453,17 @@ void RCOutput_Bebop::_run_rcout()
AP_HAL::panic("failed to get BLDC info");
}
// remember _n_motors for read_obs_data()
_n_motors = info.n_motors;
#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_DISCO
uint8_t bebop_bldc_right_front, bebop_bldc_left_front,
bebop_bldc_left_back, bebop_bldc_right_back;
/* Set motor order depending on BLDC version.On bebop 1 with version 1
* keep current order. The order changes from version 2 on bebop 1 and
* remains the same as this for bebop 2
*/
if (info.version_maj == 1) {
if (info.version_maj == 1 || info.version_maj == 5) {
bebop_bldc_right_front = BEBOP_BLDC_MOTOR_1;
bebop_bldc_left_front = BEBOP_BLDC_MOTOR_2;
bebop_bldc_left_back = BEBOP_BLDC_MOTOR_3;
@ -419,6 +479,7 @@ void RCOutput_Bebop::_run_rcout()
bebop_bldc_channels[1] = bebop_bldc_left_back;
bebop_bldc_channels[2] = bebop_bldc_left_front;
bebop_bldc_channels[3] = bebop_bldc_right_back;
#endif
hw_version = Util::from(hal.util)->get_hw_arm32();
if (hw_version == UTIL_HARDWARE_BEBOP) {
@ -426,18 +487,23 @@ void RCOutput_Bebop::_run_rcout()
} else if (hw_version == UTIL_HARDWARE_BEBOP2) {
_max_rpm = BEBOP_BLDC_MAX_RPM_2;
} else if (hw_version == UTIL_HARDWARE_DISCO) {
_max_rpm = BEBOP_BLDC_MAX_RPM_2;
_max_rpm = BEBOP_BLDC_MAX_RPM_DISCO;
} else if (hw_version < 0) {
AP_HAL::panic("failed to get hw version %s", strerror(hw_version));
} else {
AP_HAL::panic("unsupported hw version %d", hw_version);
}
printf("Bebop: vers %u/%u type %u nmotors %u n_flights %u last_flight_time %u total_flight_time %u maxrpm %u\n",
(unsigned)info.version_maj, (unsigned)info.version_min, (unsigned)info.type,
(unsigned)info.n_motors, (unsigned)be16toh(info.n_flights),
(unsigned)be16toh(info.last_flight_time), (unsigned)be32toh(info.total_flight_time),
(unsigned)_max_rpm);
while (true) {
pthread_mutex_lock(&_mutex);
ret = clock_gettime(CLOCK_MONOTONIC, &ts);
if (ret != 0) {
hal.console->println("RCOutput_Bebop: bad checksum in obs data");
continue;
}
if (ts.tv_nsec > (1000000000 - BEBOP_BLDC_TIMEOUT_NS)) {
@ -457,14 +523,14 @@ void RCOutput_Bebop::_run_rcout()
/* start propellers if the speed of the 4 motors is >= min speed
* min speed set to min_pwm + 50*/
for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) {
if (current_period_us[i] <= _min_pwm + 50) {
for (i = 0; i < _n_motors; i++) {
if (current_period_us[i] <= _min_pwm + 50)
break;
}
_rpm[bebop_bldc_channels[i]] = _period_us_to_rpm(current_period_us[i]);
}
if (i < BEBOP_BLDC_MOTORS_NUM) {
if (i < _n_motors) {
/* one motor pwm value is at minimum (or under)
* if the motors are started, stop them*/
if (_state == BEBOP_BLDC_STARTED) {
@ -477,9 +543,9 @@ void RCOutput_Bebop::_run_rcout()
if (_state == BEBOP_BLDC_STOPPED) {
_start_prop();
}
}
_set_ref_speed(_rpm);
}
}
}
#endif

3
libraries/AP_HAL_Linux/RCOutput_Bebop.h
View File

@ -5,9 +5,11 @@
enum bebop_bldc_motor {
BEBOP_BLDC_MOTOR_1 = 0,
#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_DISCO
BEBOP_BLDC_MOTOR_2,
BEBOP_BLDC_MOTOR_3,
BEBOP_BLDC_MOTOR_4,
#endif
BEBOP_BLDC_MOTORS_NUM,
};
@ -78,6 +80,7 @@ private:
uint16_t _frequency;
uint16_t _min_pwm;
uint16_t _max_pwm;
uint8_t _n_motors=4;
uint8_t _state;
bool _corking = false;
uint16_t _max_rpm;

2
libraries/AP_HAL_Linux/RCOutput_Disco.cpp
View File

@ -25,6 +25,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
namespace Linux {
@ -32,6 +33,7 @@ void RCOutput_Disco::init()
{
sysfs_out.init();
bebop_out.init();
printf("RCOutput_Disco: initialised\n");
}
void RCOutput_Disco::set_freq(uint32_t chmask, uint16_t freq_hz)

9
libraries/AP_HAL_Linux/RCOutput_Disco.h
View File

@ -9,9 +9,12 @@ public:
RCOutput_Disco(void) {}
~RCOutput_Disco() {}
static RCOutput_Disco *from(AP_HAL::RCOutput *rcoutput)
static RCOutput_Bebop *from(AP_HAL::RCOutput *rcoutput)
{
return static_cast<RCOutput_Disco *>(rcoutput);
// this is used by AP_BattMonitor_Bebop to get obs data. We
// need to return the Bebop output, not ourselves
RCOutput_Disco *d = static_cast<RCOutput_Disco *>(rcoutput);
return static_cast<RCOutput_Bebop *>(&d->bebop_out);
}
void init() override;
@ -27,8 +30,8 @@ public:
private:
// Disco RC output combines methods from Sysfs and Bebop
RCOutput_Sysfs sysfs_out{0, 1, 6};
RCOutput_Bebop bebop_out;
RCOutput_Sysfs sysfs_out{0, 1, 6};
/*
use a table to provide the mapping to channel numbers in each