diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index 8249768d9c..cd73fe90c1 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -30,6 +30,7 @@ namespace Linux { class LinuxRCOutput_AioPRU; class LinuxRCOutput_Navio; class LinuxRCOutput_ZYNQ; + class LinuxRCOutput_Bebop; class LinuxSemaphore; class LinuxScheduler; class LinuxUtil; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index cf3d95cad9..301f16ddb7 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -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" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index d538ebabe5..abaaf2ef86 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -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); diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp new file mode 100644 index 0000000000..1bc36d11fb --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -0,0 +1,436 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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; itake(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; itake(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, ¶m); + 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 diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h new file mode 100644 index 0000000000..4cf975a2db --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -0,0 +1,67 @@ +#ifndef __AP_HAL_LINUX_RCOUTPUT_BEBOP_H__ +#define __AP_HAL_LINUX_RCOUTPUT_BEBOP_H__ + +#include + +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__ diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp index 1df94f617b..b1029e349c 100644 --- a/libraries/AP_HAL_Linux/Storage.cpp +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -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; locpanic("Error filling " STORAGE_FILE); } }