diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index 768acd8208..418e079743 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -23,6 +23,7 @@ + @@ -41,8 +42,6 @@ - - @@ -106,6 +105,8 @@ + + @@ -179,6 +180,10 @@ + + + + @@ -221,6 +226,9 @@ + + + diff --git a/conf/modules/actuators_faulhaber.xml b/conf/modules/actuators_faulhaber.xml new file mode 100644 index 0000000000..c3698b129a --- /dev/null +++ b/conf/modules/actuators_faulhaber.xml @@ -0,0 +1,45 @@ + + + + + + Actuators Driver for the Faulhaber controller + + + + + + + + + + + + + + + uart + + +
+ +
+ + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.c b/sw/airborne/modules/actuators/actuators_faulhaber.c new file mode 100644 index 0000000000..507f18aa12 --- /dev/null +++ b/sw/airborne/modules/actuators/actuators_faulhaber.c @@ -0,0 +1,342 @@ +/* + * Copyright (C) Freek van Tienen + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/actuators/actuators_faulhaber.c" + * @author Freek van Tienen + * Serial Connection module between ap and a faulhaber motor controller + */ + +#include "modules/actuators/actuators_faulhaber.h" +#include "mcu_periph/uart.h" +#include "mcu_periph/sys_time.h" +#include "modules/datalink/downlink.h" +#include "modules/core/abi.h" +#include +#include + +static struct uart_periph *faulhaber_dev = &(FAULHABER_DEV); + +struct faulhaber_parser_t { + uint8_t state; + uint8_t node_nb; + uint8_t cmd_code; + uint8_t data_length; + uint8_t data_idx; + uint8_t data[64]; + + uint8_t calc_crc8; +}; +static struct faulhaber_parser_t faulhaber_p; +struct faulhaber_t faulhaber; + +#define Polynom 0xD5 +uint8_t faulhaber_crc8(uint8_t u8Byte, uint8_t u8CRC) +{ + uint8_t i; + u8CRC = u8CRC ^ u8Byte; + for (i = 0; i < 8; i++) { + if (u8CRC & 0x01) { + u8CRC = (u8CRC >> 1) ^ Polynom; + } else { + u8CRC >>= 1; + } + } + return u8CRC; +} + +static void faulhaber_send_command(struct uart_periph *dev, uint8_t cmd_code, uint8_t *data, uint8_t data_length) +{ + uart_put_byte(dev, 0, 'S'); // Character (S) as Start of Frame + uart_put_byte(dev, 0, data_length + 4); // Telegram length without SOF/EOF (packet length) + uint8_t crc8 = faulhaber_crc8(data_length + 4, 0xFF); // Start CRC with 0xFF + uart_put_byte(dev, 0, 0x01); // Node number of the slave (0 = Broadcast) + crc8 = faulhaber_crc8(0x01, crc8); + uart_put_byte(dev, 0, cmd_code); // See Tab. 2 + crc8 = faulhaber_crc8(cmd_code, crc8); + for (uint8_t i = 0; i < data_length; i++) { // Data area (length = packet length – 4) + uart_put_byte(dev, 0, data[i]); + crc8 = faulhaber_crc8(data[i], crc8); + } + uart_put_byte(dev, 0, crc8); // CRC8 with polynomial 0xD5 over byte 2–N + uart_put_byte(dev, 0, 'E'); // Character (E) as End of Frame + uart_send_message(dev, 0); +} + +#define UINIT 0 +#define GOT_SOF 1 +#define GOT_LENGTH 2 +#define GOT_NODE_NB 3 +#define GET_DATA 4 +#define GET_CRC 5 +#define GET_EOF 6 +#define GOT_FULL_PACKET 7 + +static void faulhaber_parser(struct faulhaber_parser_t *p, uint8_t c) +{ + switch (p->state) { + case UINIT: + if (c == 'S') { + p->state = GOT_SOF; + p->calc_crc8 = 0xFF; + } + break; + case GOT_SOF: + if (c - 4 < 0) { + p->state = UINIT; + } else { + p->data_length = c - 4; + p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8); + p->state = GOT_LENGTH; + } + break; + case GOT_LENGTH: + p->node_nb = c; + p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8); + p->state = GOT_NODE_NB; + break; + case GOT_NODE_NB: + p->cmd_code = c; + p->data_idx = 0; + p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8); + p->state = GET_DATA; + break; + case GET_DATA: + p->data[p->data_idx++] = c; + p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8); + if (p->data_idx >= p->data_length) { + p->state = GET_CRC; + } + break; + case GET_CRC: + if (p->calc_crc8 == c) { + p->state = GET_EOF; + } else { + p->state = UINIT; + } + break; + case GET_EOF: + if (c == 'E') { + p->state = GOT_FULL_PACKET; + } else { + p->state = UINIT; + } + break; + default: + p->state = UINIT; + break; + } +} + +// Controlword 0x6040.00 +// > 8) & 0xFF), ((faulhaber.target_position >> 16) & 0xFF), ((faulhaber.target_position >> 24) & 0xFF) }; // Set 0x607A.00 to 0x00000000: Target position 0 + faulhaber_send_command(faulhaber_dev, 0x02, data, 7); + faulhaber.state++; + break; + } + case 2: { + // Enable operation + uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + break; + } + case 3: { + // Start moving + uint8_t data[] = { 0x40, 0x60, 0x00, 0x3F, 0x00}; // Set 0x6040.00 to 0x003F: Start moving + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + break; + } + default: + faulhaber.mode = FH_MODE_IDLE; + faulhaber.state = 0; + break; + + } + break; + + /* FH_MODE_IDLE */ + case FH_MODE_IDLE: { + // Move to the new position + if (faulhaber.target_position != faulhaber.setpoint_position) { + faulhaber.mode = FH_MODE_POSITION; + faulhaber.state = 0; + break; + } + + // Request the position + uint8_t data[] = { 0x64, 0x60, 0x00}; // Get 0x6064.00: Get the actual position + faulhaber_send_command(faulhaber_dev, 0x01, data, 3); + faulhaber.state = 0; + break; + } + + case FH_MODE_ENABLE: + switch (faulhaber.state) { + case 0: { + // Disable drive + uint8_t data[] = { 0x40, 0x60, 0x00, 0x80, 0x00}; // Set 0x6040.00 to 0x0007: Disable drive + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + //break; + } + case 1: { + // Disable drive + uint8_t data[] = { 0x40, 0x60, 0x00, 0x06, 0x00}; // Set 0x6040.00 to 0x0006: Disable drive + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + //break; + } + case 2: { + // Enable drive + uint8_t data[] = { 0x40, 0x60, 0x00, 0x07, 0x00}; // Set 0x6040.00 to 0x0007: Enable drive + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + //break; + } + case 3: { + // Execute + uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Execute + faulhaber_send_command(faulhaber_dev, 0x02, data, 5); + faulhaber.state++; + //break; + } + default: + faulhaber.mode = FH_MODE_INIT; + faulhaber.state = 0; + break; + } + break; + + /* Do nothing */ + default: + break; + } + +} + +static void faulhaber_parse_msg(struct faulhaber_parser_t *p) +{ + // Process position message + if (p->cmd_code == 0x01 && p->data[0] == 0x64 && p->data[1] == 0x60 && p->data[2] == 0x00) { + faulhaber.real_position = p->data[3] | (p->data[4] << 8) | (p->data[5] << 16) | (p->data[6] << 24); + + struct act_feedback_t feedback = {0}; + feedback.position = (faulhaber.real_position - get_servo_min_FAULHABER(0)) / (double)(get_servo_max_FAULHABER( + 0) - get_servo_min_FAULHABER(0)) * M_PI_2; // In radians + feedback.set.position = true; + feedback.idx = get_servo_idx_FAULHABER(0); + + // Send ABI message + AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_FAULHABER_ID, &feedback, 1); + } +} + + +void actuators_faulhaber_event(void) +{ + while (uart_char_available(faulhaber_dev)) { + faulhaber_parser(&faulhaber_p, uart_getch(faulhaber_dev)); + if (faulhaber_p.state == GOT_FULL_PACKET) { + faulhaber_parse_msg(&faulhaber_p); + faulhaber_p.state = UINIT; + } + } +} + +void actuators_faulhaber_SetMode(uint8_t mode) +{ + faulhaber.mode = mode; + faulhaber.state = 0; +} diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.h b/sw/airborne/modules/actuators/actuators_faulhaber.h new file mode 100644 index 0000000000..28c253a952 --- /dev/null +++ b/sw/airborne/modules/actuators/actuators_faulhaber.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) Freek van Tienen + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/actuators/actuators_faulhaber.h" + * @author Freek van Tienen + * Serial Connection module between ap and a faulhaber motor controller + */ + +#ifndef ACTUATORS_FAULHABER_H +#define ACTUATORS_FAULHABER_H + +#include "std.h" + +enum faulhaber_modes_t { + FH_MODE_INIT, + FH_MODE_IDLE, + FH_MODE_HOME, + FH_MODE_POSITION, + FH_MODE_ENABLE +}; + +struct faulhaber_t { + enum faulhaber_modes_t mode; + uint8_t state; + + int32_t setpoint_position; + int32_t target_position; + int32_t real_position; +}; +extern struct faulhaber_t faulhaber; + +extern void actuators_faulhaber_init(void); +extern void actuators_faulhaber_periodic(void); +extern void actuators_faulhaber_event(void); +extern void actuators_faulhaber_SetMode(uint8_t mode); + + +#if USE_NPS +#define ActuatorsFaulhaberInit() {} +#define ActuatorFaulhaberSet(_i, _v) {} +#define ActuatorsFaulhaberCommit() {} +#else +#define ActuatorsFaulhaberInit() actuators_faulhaber_init() +#define ActuatorFaulhaberSet(_i, _v) { faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0)-_v) + get_servo_min_FAULHABER(0))*1000.0f; } +#define ActuatorsFaulhaberCommit() {} +#endif + +#endif /* ACTUATORS_FAULHABER_H */ diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index 5f191aceed..940af28c98 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -503,6 +503,10 @@ #define ACT_FEEDBACK_RPM_SENSOR_ID 4 #endif +#ifndef ACT_FEEDBACK_FAULHABER_ID +#define ACT_FEEDBACK_FAULHABER_ID 5 +#endif + /* * IDs of THRUST increment calculation (message 16) */