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

HAL_Linux: added rcout implementation for bebop

Browse Source
This commit is contained in:
Julien BERAUD 2015-07-10 14:02:32 +10:00 committed by Andrew Tridgell
parent 51c3c499e2
commit 954ec71630
6 changed files with 514 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h
View File

@ -30,6 +30,7 @@ namespace Linux {
class LinuxRCOutput_AioPRU;
class LinuxRCOutput_Navio;
class LinuxRCOutput_ZYNQ;
class LinuxRCOutput_Bebop;
class LinuxSemaphore;
class LinuxScheduler;
class LinuxUtil;

1
libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h
View File

@ -21,6 +21,7 @@
#include "RCOutput_AioPRU.h"
#include "RCOutput_Navio.h"
#include "RCOutput_ZYNQ.h"
#include "RCOutput_Bebop.h"
#include "Semaphores.h"
#include "Scheduler.h"
#include "ToneAlarmDriver.h"

6
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -97,6 +97,8 @@ static LinuxRCOutput_AioPRU rcoutDriver;
static LinuxRCOutput_Navio rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ
static LinuxRCOutput_ZYNQ rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
static LinuxRCOutput_Bebop rcoutDriver;
#else
static Empty::EmptyRCOutput rcoutDriver;
#endif
@ -200,11 +202,11 @@ void HAL_Linux::init(int argc,char* const argv[]) const
scheduler->init(NULL);
gpio->init();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
i2c0->begin();
i2c->begin();
i2c1->begin();
i2c2->begin();
#else
i2c0->begin();
i2c->begin();
#endif
rcout->init(NULL);
rcin->init(NULL);

436
libraries/AP_HAL_Linux/RCOutput_Bebop.cpp Normal file
View File

@ -0,0 +1,436 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#include <endian.h>
#include <stdio.h>
#include <sys/time.h>
#include <poll.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/mman.h>
#include <pthread.h>
#include "RCOutput_Bebop.h"
/* BEBOP BLDC motor controller address and registers description */
#define BEBOP_BLDC_I2C_ADDR 0x08
#define BEBOP_BLDC_STARTPROP 0x40
#define BEBOP_BLDC_CLOCKWISE 1
#define BEBOP_BLDC_COUNTERCLOCKWISE 0
static const uint8_t bebop_motors_bitmask = (BEBOP_BLDC_COUNTERCLOCKWISE <<
(BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_BACK)) |
(BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_BACK))|
(BEBOP_BLDC_COUNTERCLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_FRONT))|
(BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_FRONT));
#define BEBOP_BLDC_SETREFSPEED 0x02
struct bldc_ref_speed_data {
uint8_t cmd;
uint16_t rpm[BEBOP_BLDC_MOTORS_NUM];
uint8_t enable_security;
uint8_t checksum;
}__attribute__((packed));
#define BEBOP_BLDC_GETOBSDATA 0x20
struct 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;
uint8_t checksum;
}__attribute__((packed));
/* description of the bldc status */
#define BEBOP_BLDC_STATUS_INIT 0
#define BEBOP_BLDC_STATUS_IDLE 1
#define BEBOP_BLDC_STATUS_RAMPING 2
#define BEBOP_BLDC_STATUS_SPINNING_1 3
#define BEBOP_BLDC_STATUS_SPINNING_2 4
#define BEBOP_BLDC_STATUS_STOPPING 5
#define BEBOP_BLDC_STATUS_CRITICAL 6
/* description of the bldc errno */
#define BEBOP_BLDC_ERRNO_EEPROM 1
#define BEBOP_BLDC_ERRNO_MOTOR_STALLED 2
#define BEBOP_BLDC_ERRNO_PROP_SECU 3
#define BEBOP_BLDC_ERRNO_COM_LOST 4
#define BEBOP_BLDC_ERRNO_BATT_LEVEL 9
#define BEBOP_BLDC_ERRNO_LIPO 10
#define BEBOP_BLDC_ERRNO_MOTOR_HW 11
#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_STOP_PROP 0x60
#define BEBOP_BLDC_CLEAR_ERROR 0x80
#define BEBOP_BLDC_PLAY_SOUND 0x82
#define BEBOP_BLDC_GET_INFO 0xA0
/* Bebop is a Quad X so the channels are :
* 1 = Front Right
* 2 = Back Left
* 3 = Front Left
* 4 = Back Right
*
* but the channels start at 0 so it is channel num - 1
*/
static const uint8_t bebop_bldc_motors[BEBOP_BLDC_MOTORS_NUM] = { BEBOP_BLDC_RIGHT_FRONT,
BEBOP_BLDC_LEFT_BACK,
BEBOP_BLDC_LEFT_FRONT,
BEBOP_BLDC_RIGHT_BACK };
#define BEBOP_BLDC_MIN_PERIOD_US 1100
#define BEBOP_BLDC_MAX_PERIOD_US 1900
#define BEBOP_BLDC_MIN_RPM 3000
#define BEBOP_BLDC_MAX_RPM 11000
/* Priority of the thread controlling the BLDC via i2c
* set to 14, which is the same as the UART
*/
#define RCOUT_BEBOP_RTPRIO 14
/* Set timeout to 500ms */
#define BEBOP_BLDC_TIMEOUT_NS 500000000
enum {
BEBOP_BLDC_STARTED,
BEBOP_BLDC_STOPPED,
};
using namespace Linux;
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
LinuxRCOutput_Bebop::LinuxRCOutput_Bebop():
_i2c_sem(NULL),
_min_pwm(BEBOP_BLDC_MIN_PERIOD_US),
_max_pwm(BEBOP_BLDC_MAX_PERIOD_US),
_state(BEBOP_BLDC_STOPPED)
{
memset(_period_us, 0, sizeof(_period_us));
memset(_rpm, 0, sizeof(_rpm));
}
uint8_t LinuxRCOutput_Bebop::_checksum(uint8_t *data, unsigned int len)
{
uint8_t checksum = data[0];
unsigned int i;
for (i = 1; i < len; i++)
checksum = checksum ^ data[i];
return checksum;
}
void LinuxRCOutput_Bebop::_start_prop()
{
uint8_t data = BEBOP_BLDC_STARTPROP;
if (!_i2c_sem->take(0))
return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data);
_i2c_sem->give();
_state = BEBOP_BLDC_STARTED;
}
void LinuxRCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM])
{
struct bldc_ref_speed_data data;
int i;
data.cmd = BEBOP_BLDC_SETREFSPEED;
for (i=0; i<BEBOP_BLDC_MOTORS_NUM; i++)
data.rpm[i] = htobe16(rpm[i]);
data.enable_security = 0;
data.checksum = _checksum((uint8_t *) &data, sizeof(data) - 1);
if (!_i2c_sem->take(0))
return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, sizeof(data), (uint8_t *)&data);
_i2c_sem->give();
}
void LinuxRCOutput_Bebop::_get_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)
{
struct bldc_obs_data data;
int i;
memset(&data, 0, sizeof(data));
if (!_i2c_sem->take(0))
return;
hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR,
BEBOP_BLDC_GETOBSDATA,
sizeof(data),
(uint8_t *)&data);
if (data.checksum != _checksum((uint8_t *)&data, sizeof(data)))
hal.console->println_P("RCOutput_Bebop: bad checksum in obs data");
if (rpm != NULL) {
for(i=0; i<BEBOP_BLDC_MOTORS_NUM; i++)
rpm[i] = be16toh(data.rpm[i]);
}
if (batt_mv != NULL)
*batt_mv = be16toh(data.batt_mv);
if (status != NULL)
*status = data.status;
if (error != NULL)
*error = data.error;
if (motors_err != NULL)
*motors_err = data.motors_err;
if (temp != NULL)
*temp = data.temp;
}
void LinuxRCOutput_Bebop::_toggle_gpio(uint8_t mask)
{
if (!_i2c_sem->take(0))
return;
hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_TOGGLE_GPIO, mask);
_i2c_sem->give();
}
void LinuxRCOutput_Bebop::_stop_prop()
{
uint8_t data = BEBOP_BLDC_STOP_PROP;
_state = BEBOP_BLDC_STOPPED;
if (!_i2c_sem->take(0))
return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data);
_i2c_sem->give();
}
void LinuxRCOutput_Bebop::_clear_error()
{
uint8_t data = BEBOP_BLDC_CLEAR_ERROR;
if (!_i2c_sem->take(0))
return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data);
_i2c_sem->give();
}
void LinuxRCOutput_Bebop::_play_sound(uint8_t sound)
{
if (!_i2c_sem->take(0))
return;
hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_PLAY_SOUND, sound);
_i2c_sem->give();
}
uint16_t LinuxRCOutput_Bebop::_period_us_to_rpm(uint16_t period_us)
{
float period_us_fl = period_us;
float rpm_fl = (period_us_fl - _min_pwm)/(_max_pwm - _min_pwm) *
(BEBOP_BLDC_MAX_RPM - BEBOP_BLDC_MIN_RPM) + BEBOP_BLDC_MIN_RPM;
return (uint16_t)rpm_fl;
}
void LinuxRCOutput_Bebop::init(void* dummy)
{
int ret=0;
struct sched_param param = { .sched_priority = RCOUT_BEBOP_RTPRIO };
pthread_attr_t attr;
_i2c_sem = hal.i2c1->get_semaphore();
if (_i2c_sem == NULL) {
hal.scheduler->panic(PSTR("RCOutput_Bebop: can't get i2c sem"));
return; /* never reached */
}
/* Initialize thread, cond, and mutex */
ret = pthread_mutex_init(&_mutex, NULL);
if (ret != 0) {
perror("RCout_Bebop: failed to init mutex\n");
return;
}
pthread_mutex_lock(&_mutex);
ret = pthread_cond_init(&_cond, NULL);
if (ret != 0) {
perror("RCout_Bebop: failed to init cond\n");
goto exit;
}
ret = pthread_attr_init(&attr);
if (ret != 0) {
perror("RCOut_Bebop: failed to init attr\n");
goto exit;
}
pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
pthread_attr_setschedparam(&attr, &param);
ret = pthread_create(&_thread, &attr, _control_thread, this);
if (ret != 0) {
perror("RCOut_Bebop: failed to create thread\n");
goto exit;
}
_clear_error();
/* Set an initial dummy frequency */
_frequency = 50;
exit:
pthread_mutex_unlock(&_mutex);
return;
}
void LinuxRCOutput_Bebop::set_freq(uint32_t chmask, uint16_t freq_hz)
{
_frequency = freq_hz;
}
uint16_t LinuxRCOutput_Bebop::get_freq(uint8_t ch)
{
return _frequency;
}
void LinuxRCOutput_Bebop::enable_ch(uint8_t ch)
{
}
void LinuxRCOutput_Bebop::disable_ch(uint8_t ch)
{
_stop_prop();
}
void LinuxRCOutput_Bebop::write(uint8_t ch, uint16_t period_us)
{
pthread_mutex_lock(&_mutex);
_period_us[ch] = period_us;
pthread_cond_signal(&_cond);
pthread_mutex_unlock(&_mutex);
}
void LinuxRCOutput_Bebop::write(uint8_t ch, uint16_t* period_us, uint8_t len)
{
for (int i = 0; i < len; i++)
write(ch + i, period_us[i]);
}
uint16_t LinuxRCOutput_Bebop::read(uint8_t ch)
{
if (ch < BEBOP_BLDC_MOTORS_NUM) {
return _period_us[ch];
} else {
return 0;
}
}
void LinuxRCOutput_Bebop::read(uint16_t* period_us, uint8_t len)
{
for (int i = 0; i < len; i++)
period_us[i] = read(0 + i);
}
void LinuxRCOutput_Bebop::set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm)
{
_min_pwm = min_pwm;
_max_pwm = max_pwm;
}
/* Separate thread to handle the Bebop motors controller */
void* LinuxRCOutput_Bebop::_control_thread(void *arg) {
LinuxRCOutput_Bebop* rcout = (LinuxRCOutput_Bebop *) arg;
rcout->_run_rcout();
return NULL;
}
void LinuxRCOutput_Bebop::_run_rcout()
{
uint16_t current_period_us[BEBOP_BLDC_MOTORS_NUM];
uint8_t i;
int ret;
struct timespec ts;
memset(current_period_us, 0, sizeof(current_period_us));
while (true) {
pthread_mutex_lock(&_mutex);
ret = clock_gettime(CLOCK_REALTIME, &ts);
if (ret != 0)
hal.console->println_P("RCOutput_Bebop: bad checksum in obs data");
if (ts.tv_nsec > (1000000000 - BEBOP_BLDC_TIMEOUT_NS))
{
ts.tv_sec += 1;
ts.tv_nsec = ts.tv_nsec + BEBOP_BLDC_TIMEOUT_NS - 1000000000;
} else {
ts.tv_nsec += BEBOP_BLDC_TIMEOUT_NS;
}
ret = 0;
while ((memcmp(_period_us, current_period_us, sizeof(_period_us)) == 0) && (ret == 0))
ret = pthread_cond_timedwait(&_cond, &_mutex, &ts);
memcpy(current_period_us, _period_us, sizeof(_period_us));
pthread_mutex_unlock(&_mutex);
/* 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)
break;
_rpm[bebop_bldc_motors[i]] = _period_us_to_rpm(current_period_us[i]);
}
if (i < BEBOP_BLDC_MOTORS_NUM) {
/* one motor pwm value is at minimum (or under)
* if the motors are started, stop them*/
if (_state == BEBOP_BLDC_STARTED) {
_stop_prop();
_clear_error();
}
} else {
/* all the motor pwm values are higher than minimum
* if the bldc is stopped, start it*/
if (_state == BEBOP_BLDC_STOPPED)
_start_prop();
}
_set_ref_speed(_rpm);
}
}
#endif

67
libraries/AP_HAL_Linux/RCOutput_Bebop.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef __AP_HAL_LINUX_RCOUTPUT_BEBOP_H__
#define __AP_HAL_LINUX_RCOUTPUT_BEBOP_H__
#include <AP_HAL_Linux.h>
enum bebop_bldc_motor {
BEBOP_BLDC_RIGHT_FRONT = 0,
BEBOP_BLDC_LEFT_FRONT,
BEBOP_BLDC_LEFT_BACK,
BEBOP_BLDC_RIGHT_BACK,
BEBOP_BLDC_MOTORS_NUM,
};
enum bebop_bldc_sound {
BEBOP_BLDC_SOUND_NONE = 0,
BEBOP_BLDC_SOUND_SHORT_BEEP,
BEBOP_BLDC_SOUND_BOOT_BEEP,
BEBOP_BLDC_SOUND_BEBOP,
};
class Linux::LinuxRCOutput_Bebop : public AP_HAL::RCOutput {
public:
LinuxRCOutput_Bebop();
void init(void* dummy);
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm);
private:
AP_HAL::Semaphore *_i2c_sem;
uint16_t _period_us[BEBOP_BLDC_MOTORS_NUM];
uint16_t _rpm[BEBOP_BLDC_MOTORS_NUM];
uint16_t _frequency;
uint16_t _min_pwm;
uint16_t _max_pwm;
uint8_t _state;
uint8_t _checksum(uint8_t *data, unsigned int len);
void _start_prop();
void _set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]);
void _get_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);
void _toggle_gpio(uint8_t mask);
void _stop_prop();
void _clear_error();
void _play_sound(uint8_t sound);
uint16_t _period_us_to_rpm(uint16_t period_us);
/* thread related members */
pthread_t _thread;
pthread_mutex_t _mutex;
pthread_cond_t _cond;
void _run_rcout();
static void *_control_thread(void *arg);
};
#endif // __AP_HAL_LINUX_RCOUTPUT_BEBOP_H__

5
libraries/AP_HAL_Linux/Storage.cpp
View File

@ -20,7 +20,11 @@ using namespace Linux;
// name the storage file after the sketch so you can use the same board
// card for ArduCopter and ArduPlane
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
#define STORAGE_DIR "/data/ftp/internal_000/APM"
#else
#define STORAGE_DIR "/var/APM"
#endif
#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
extern const AP_HAL::HAL& hal;
@ -35,6 +39,7 @@ void LinuxStorage::_storage_create(void)
}
for (uint16_t loc=0; loc<sizeof(_buffer); loc += LINUX_STORAGE_MAX_WRITE) {
if (write(fd, &_buffer[loc], LINUX_STORAGE_MAX_WRITE) != LINUX_STORAGE_MAX_WRITE) {
perror("write");
hal.scheduler->panic("Error filling " STORAGE_FILE);
}
}