Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

CSC/CSCAP updates

Browse Source
This commit is contained in:
Allen Ibara
2009-06-24 19:54:58 +00:00
parent 0bc1a5ae26
commit 528d244bec
17 changed files with 431 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/csc/csc_ap_link.c
+13
View File
@@ -8,6 +8,7 @@ int32_t csc_ap_link_error_cnt;
static void (* servo_msg_cb)(struct CscServoCmd *);
static void (* motor_msg_cb)(struct CscMotorMsg *);
static void (* rc_msg_cb)(struct CscRCMsg *);
static void (* prop_msg_cb)(struct CscPropCmd *);
void csc_ap_link_send_adc(float adc1, float adc2)
{
@@ -57,6 +58,11 @@ void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg))
motor_msg_cb = cb;
}
void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd))
{
prop_msg_cb = cb;
}
void csc_ap_link_set_rc_cmd_cb(void (* cb)(struct CscRCMsg *msg))
{
rc_msg_cb = cb;
@@ -95,6 +101,13 @@ static void on_ap_link_msg( struct CscCanMsg *msg)
rc_msg_cb((struct CscRCMsg *) &msg->dat_a);
}
break;
case CSC_PROP_CMD_ID:
if (len != sizeof(struct CscPropCmd)){
LED_ON(ERROR_LED);
csc_ap_link_error_cnt++;
}else
prop_msg_cb((struct CscPropCmd *) &msg->dat_a);
break;
}
}
sw/airborne/csc/csc_ap_link.h
+1
View File
@@ -15,6 +15,7 @@ void csc_ap_link_send_status(uint32_t loops, uint32_t msgs);
void csc_ap_link_send_adc(float adc1, float adc2);
void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd));
void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg));
void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd));
void csc_ap_link_set_rc_cmd_cb(void (* cb)(struct CscRCMsg *msg));
#endif /* CSC_AP_LINK_H */
sw/airborne/csc/csc_ap_main.c
+6 -4
View File
@@ -71,11 +71,13 @@ static void nop(struct CscCanMsg *msg)
static void on_rc_cmd(struct CscRCMsg *msg)
{
rc_values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
rc_values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
rc_values[RADIO_YAW] = CSC_RC_SCALE*(msg->left_stick_horizontal - CSC_RC_OFFSET);
uint8_t mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
rc_values[RADIO_MODE] = mode ? -7000 : ( (mode == 1) ? 0 : 7000);
rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
rc_values[RADIO_ROLL] = -RADIO_SCALE * (msg->right_stick_horizontal - ROLL_OFFSET);
rc_values[RADIO_PITCH] = -RADIO_SCALE * (msg->right_stick_vertical - PITCH_OFFSET);
rc_values[RADIO_YAW] = RADIO_SCALE * (msg->left_stick_horizontal - YAW_OFFSET);
rc_values[RADIO_MODE] = RADIO_SCALE * (msg->flap_mix - MODE_OFFSET);
time_since_last_ppm = 0;
rc_status = RC_OK;
pprz_mode = PPRZ_MODE_OF_RC(rc_values[RADIO_MODE]);
sw/airborne/csc/csc_autopilot.c
+10 -6
View File
@@ -39,6 +39,7 @@ struct control_reference csc_errors;
struct control_trims csc_trims;
static const int xsens_id = 0;
float csc_yaw_weight;
void csc_ap_init( void )
{
@@ -69,7 +70,7 @@ static void update_ki(float new_ki, float *ki, float *ierror)
*ki = new_ki;
}
#define I_CMD_BOUND 0.5
#define I_CMD_BOUND 0.4
static void bound_ierror(float igain, float *ierror)
{
@@ -128,9 +129,11 @@ static void calculate_errors(struct control_reference *errors)
static void calculate_reference(struct control_reference *reference)
{
reference->eulers.phi = M_PI / 6.0 * rc_values[RADIO_ROLL];
reference->eulers.theta = M_PI / 6.0 * rc_values[RADIO_PITCH];
reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW];
reference->eulers.theta = M_PI / 6.0 * rc_values[RADIO_PITCH];
// Mix reference command with yaw reference command to prevent
// fighting ourselves
reference->eulers.phi = M_PI / 6.0 * rc_values[RADIO_ROLL];// + csc_yaw_weight * reference->eulers.psi;
reference->eulers.phi /= MAX_PPRZ;
reference->eulers.theta /= MAX_PPRZ;
@@ -143,9 +146,9 @@ void csc_ap_periodic( void )
calculate_reference(&csc_reference);
calculate_errors(&csc_errors);
commands[COMMAND_ROLL] = -csc_gains.roll_kp * csc_errors.eulers.phi
+ csc_gains.roll_kd * csc_errors.rates.p
- csc_gains.roll_ki * csc_errors.eulers_i.phi;
commands[COMMAND_ROLL] = -csc_gains.roll_kp * (csc_errors.eulers.phi + csc_errors.eulers.psi * csc_yaw_weight)
+ csc_gains.roll_kd * (csc_errors.rates.p + csc_errors.rates.r * csc_yaw_weight)
- csc_gains.roll_ki * (csc_errors.eulers_i.phi + csc_errors.eulers_i.psi * csc_yaw_weight);
commands[COMMAND_ROLL] += csc_trims.aileron;
commands[COMMAND_PITCH] = -csc_gains.pitch_kp * csc_errors.eulers.theta
@@ -172,3 +175,4 @@ void csc_ap_set_trims( void )
csc_trims.elevator = commands[COMMAND_PITCH];
csc_trims.rudder = commands[COMMAND_YAW];
}
sw/airborne/csc/csc_autopilot.h
+1
View File
@@ -56,6 +56,7 @@ struct control_trims {
extern struct control_gains csc_gains;
extern struct control_reference csc_reference;
extern struct control_trims csc_trims;
extern float csc_yaw_weight;
void csc_autopilot_set_roll_ki(float ki);
void csc_autopilot_set_pitch_ki(float ki);
sw/airborne/csc/csc_can.c
+22 -1
View File
@@ -1,4 +1,5 @@
#include "csc_can.h"
#include "string.h"
#include "LPC21xx.h"
#include "armVIC.h"
@@ -12,6 +13,9 @@ bool_t can1_msg_received;
struct CscCanMsg can1_rx_msg;
static void (* can1_callback)(struct CscCanMsg *);
static bool_t can1_msg_transmit_queued = 0;
static struct CscCanMsg can1_tx_queued_msg;
static void CAN1_Rx_ISR ( void ) __attribute__((naked));
static void CAN1_Tx_ISR ( void ) __attribute__((naked));
static void CAN1_Err_ISR ( void ) __attribute__((naked));
@@ -61,12 +65,23 @@ void csc_can1_init(void(* callback)(struct CscCanMsg *msg)) {
// Get out of reset Mode
C1MOD = 0;
}
static inline uint32_t csc_can1_tx_available()
{
return (C1SR & 0x00000004L);
}
void csc_can1_send(struct CscCanMsg* msg) {
if (!(C1SR & 0x00000004L)) { /* transmit channel not available */
if (!csc_can1_tx_available()) { /* transmit channel not available */
if(!can1_msg_transmit_queued){
can1_msg_transmit_queued = 1;
memcpy(&can1_tx_queued_msg,msg,sizeof(struct CscCanMsg));
}
// LED_ON(2);
return;
}
@@ -121,6 +136,12 @@ void csc_can_event(void)
can1_callback(&can1_rx_msg);
can1_msg_received = FALSE;
}
if(can1_msg_transmit_queued && csc_can1_tx_available()){
csc_can1_send(&can1_tx_queued_msg);
can1_msg_transmit_queued = 0;
}
#endif /* USE_CAN1 */
}
sw/airborne/csc/csc_msg_def.h
+20 -7
View File
@@ -1,18 +1,28 @@
#ifndef CSC_MSG_DEF_H
#define CSC_MSG_DEF_H
#define CSC_SERVO_CMD_ID 0
#define CSC_MOTOR_CMD_ID 1
#define CSC_MOTOR_STATUS_ID 2
#define CSC_BOARD_STATUS_ID 3
#define CSC_BOARD_ADCVOLTS_ID 4
#define CSC_RC_ID 5
#include "paparazzi.h"
#define CSC_SERVO_CMD_ID 0
#define CSC_MOTOR_CMD_ID 1
#define CSC_PROP_CMD_ID 2
#define CSC_MOTOR_STATUS_ID 3
#define CSC_BOARD_STATUS_ID 4
#define CSC_BOARD_ADCVOLTS_ID 5
#define CSC_RC_ID 6
/* Received from the autopilot */
struct CscServoCmd {
uint16_t servos[4];
} __attribute__((packed));
/* For simple blades which have a positive speed,
and variable pitch blades. */
struct CscPropCmd {
uint8_t speeds[4];
} __attribute__((packed));
/* Send and Received between autopilot and csc */
struct CscMotorMsg {
uint8_t cmd_id;
@@ -34,7 +44,10 @@ struct CscRCMsg {
uint16_t right_stick_vertical;
uint16_t right_stick_horizontal;
uint16_t left_stick_horizontal;
uint16_t flap_mix;
uint16_t left_stick_vertical_and_flap_mix;
} __attribute__((packed));
#define CSC_RC_SCALE 20
#define CSC_RC_OFFSET 2*(MAX_PPRZ/CSC_RC_SCALE) /* Sorry this is a bit arbitrary. - mmt */
#endif
sw/airborne/csc/csc_rc_spektrum.c
+27 -5
View File
@@ -31,6 +31,7 @@
#include "led.h"
#include "paparazzi.h"
#include "downlink.h"
#include "messages.h"
#include "uart.h"
@@ -66,18 +67,39 @@ void spektrum_periodic_task ( void )
}
#define ROLL_OFFSET 1544
#define PITCH_OFFSET 2551
#define THRUST_OFFSET 3793
#define YAW_OFFSET 3585
#define MODE_OFFSET 5632
// Called after receipt of valid message
static void spektrum_parse_msg( )
{
struct spektrum_frame *frame = (struct spektrum_frame *) msg_buf;
struct CscRCMsg msg;
struct CscRCMsg msg;
int16_t flap_flag;
// only copy the channels we need for now to fit within can frame size
msg.right_stick_vertical = bswap_16(frame->right_vertical);
msg.right_stick_horizontal = bswap_16(frame->right_horizontal);
msg.left_stick_horizontal = bswap_16(frame->left_horizontal);
msg.flap_mix = bswap_16(frame->flap_mix);
msg.right_stick_horizontal = (bswap_16(frame->right_horizontal) - ROLL_OFFSET) + CSC_RC_OFFSET;
msg.right_stick_vertical = (bswap_16(frame->right_vertical) - PITCH_OFFSET) + CSC_RC_OFFSET;
msg.left_stick_horizontal = (bswap_16(frame->left_horizontal) - YAW_OFFSET) + CSC_RC_OFFSET;
msg.left_stick_vertical_and_flap_mix = -1*(bswap_16(frame->left_vertical) - THRUST_OFFSET) + CSC_RC_OFFSET;
flap_flag = CSC_RC_SCALE * (bswap_16(frame->flap_mix) - MODE_OFFSET);
if(flap_flag > MAX_PPRZ/2){
flap_flag = 0;
} else if (flap_flag > MIN_PPRZ/2){
flap_flag = 1;
} else {
flap_flag = 2;
}
flap_flag = flap_flag << 13;
msg.left_stick_vertical_and_flap_mix = msg.left_stick_vertical_and_flap_mix | flap_flag;
csc_ap_send_msg(CSC_RC_ID, &msg, sizeof(struct CscRCMsg));
}
sw/airborne/csc/csc_servos.c
+45 -2
View File
@@ -7,13 +7,55 @@
#include ACTUATORS
#define CSC_SERVOS_NB 4
#define SERVOS_PERIOD (SYS_TICS_OF_SEC((1./250.))); /* 250 Hz */
#define SERVOS_PERIOD (SYS_TICS_OF_SEC((1./250.))) /* 250 Hz */
#include "airframe.h"
static uint32_t csc_servos_rng[] = {SYS_TICS_OF_USEC(SERVO_S1_MAX-SERVO_S1_MIN),
SYS_TICS_OF_USEC(SERVO_S2_MAX-SERVO_S2_MIN)};
static uint32_t csc_servos_min[] = {((1<<16)-1)*SYS_TICS_OF_USEC(SERVO_S1_MIN),
((1<<16)-1)*SYS_TICS_OF_USEC(SERVO_S2_MIN)};
void csc_servos_init(void)
{
actuators_init();
}
/* val == 0 literally mean off, otherwise, val 1-255 are continuous angle */
void csc_servo_normalized_set(uint8_t id, uint16_t val)
{
if(id > 3) return;
if(val == 0) csc_servo_set(id,0);
else{
uint32_t ticks = csc_servos_rng[id]*(val-1);
csc_servo_set(id,((ticks + csc_servos_min[id])/((1<<16) - 1)));
}
}
inline void csc_servos_commit()
{
ActuatorsCommit();
}
void csc_servo_set(uint8_t id, uint32_t val)
{
if(id > 3) return;
#ifdef CSC_MOTORS_I2C
Actuator(id) = val;
#else
switch(id){
case 0: Actuator(0) = val; break;
case 1: Actuator(5) = val; break;
case 2: Actuator(4) = val; break;
case 3: Actuator(3) = val; break;
}
#endif
}
void csc_servos_set(int32_t* val)
{
#ifdef CSC_MOTORS_I2C
@@ -28,6 +70,7 @@ void csc_servos_set(int32_t* val)
Actuator(3) = val[3];
#endif
ActuatorsCommit();
csc_servos_commit();
}
sw/airborne/csc/csc_servos.h
+3 -1
View File
@@ -18,7 +18,9 @@ PWM6 RXD1 EINT3 P0.9 3
extern void csc_servos_init(void);
extern void csc_servos_set(int32_t* val);
extern void csc_servo_set(uint8_t id, uint32_t ticks);
extern void csc_servo_normalized_set(uint8_t id, uint16_t val);
extern inline void csc_servos_commit();
#endif /* CSC_SERVOS_H */
sw/airborne/csc/csc_telemetry.h
+2
View File
@@ -53,7 +53,9 @@
#define PERIODIC_SEND_QUAD_STATUS() DOWNLINK_SEND_QUAD_STATUS(&rc_status, &pprz_mode, &vsupply, &cpu_time)
#endif /* RADIO_CONTROL */
#ifdef SERVOS_NB
#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
#endif
extern uint8_t telemetry_mode_Ap;
sw/airborne/csc/mercury_ap.c
+26 -20
View File
@@ -28,12 +28,14 @@
#include "mercury_xsens.h"
#include "booz2_autopilot.h"
#include "booz2_stabilization.h"
#include "booz2_stabilization_attitude.h"
#include "led.h"
#include "pprz_algebra_float.h"
#include "string.h"
#include "radio_control.h"
#include "mercury_supervision.h"
#include "actuators.h"
#include "actuators.h"
#include "props_csc.h"
static const int xsens_id = 0;
@@ -131,27 +133,27 @@ void csc_ap_init(void) {
// adapted from booz2_commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd
// mmt -- 06/16/09
pprz_t csc_ap_commands[COMMANDS_NB];
const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
#define CscSetCommands(_in_cmd, _in_flight, _motors_on) { \
csc_ap_commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \
csc_ap_commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \
csc_ap_commands[COMMAND_YAW] = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
csc_ap_commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \
commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \
commands[COMMAND_YAW] = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
}
pprz_t mixed_commands[SERVOS_NB];
pprz_t mixed_commands[PROPS_NB];
void csc_ap_periodic(void) {
void csc_ap_periodic(int8_t _in_flight, int8_t _motors_on) {
// RunOnceEvery(50, nav_periodic_task_10Hz())
booz2_autopilot_motors_on = 1; // HACK for now mmt
booz2_autopilot_motors_on = _motors_on;
booz2_autopilot_in_flight = _in_flight;
booz2_stabilization_attitude_run(booz2_autopilot_in_flight);
/* if ( !booz2_autopilot_motors_on || */
/* if ( !booz2_autopilot_motors_on ){ */
/* if( booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || */
/* booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) { */
/* CscSetCommands(csc_ap_commands_failsafe, */
@@ -159,19 +161,23 @@ void csc_ap_periodic(void) {
/* } else { */
booz2_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ;
booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ + 118;
CscSetCommands(booz2_stabilization_cmd,
booz2_autopilot_in_flight, booz2_autopilot_motors_on);
// }
// TODO insert calls to servos_csc
// mix motors
BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on);
props_set(PROP_UPPER_LEFT, mixed_commands[PROP_UPPER_LEFT]);
props_set(PROP_LOWER_RIGHT,mixed_commands[PROP_LOWER_RIGHT]);
props_set(PROP_LOWER_LEFT, mixed_commands[PROP_LOWER_LEFT]);
props_set(PROP_UPPER_RIGHT,mixed_commands[PROP_UPPER_RIGHT]);
props_commit();
Actuator(SERVO_S1) = (1<<15)+((1<<14)/MAX_PPRZ)*rc_values[RADIO_YAW];
Actuator(SERVO_S2) = (1<<15)-((1<<14)/MAX_PPRZ)*rc_values[RADIO_YAW];
ActuatorsCommit();
BOOZ2_SUPERVISION_RUN(mixed_commands, csc_ap_commands, booz2_autopilot_motors_on);
Actuator(SERVO_UPPER_LEFT) = (uint8_t)mixed_commands[SERVO_UPPER_LEFT];
Actuator(SERVO_LOWER_RIGHT)= (uint8_t)mixed_commands[SERVO_LOWER_RIGHT];
Actuator(SERVO_LOWER_LEFT) = (uint8_t)mixed_commands[SERVO_LOWER_LEFT];
Actuator(SERVO_UPPER_RIGHT)= (uint8_t)mixed_commands[SERVO_UPPER_RIGHT];
ActuatorsCommit();
}
sw/airborne/csc/mercury_ap.h
+1 -1
View File
@@ -27,7 +27,7 @@
void csc_ap_init( void );
void csc_ap_periodic (void );
void csc_ap_periodic (int8_t flight, int8_t motors );
#endif
sw/airborne/csc/mercury_csc_main.c
+186
View File
@@ -0,0 +1,186 @@
/*
* $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
*
* Copyright (C) 2008 Antoine Drouin
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include <inttypes.h>
#include "csc_main.h"
#include "std.h"
#include "init_hw.h"
#include "sys_time.h"
#include "led.h"
#include "buss_twi_blmc_hw.h"
#include "i2c.h"
#include "csc_servos.h"
#include "interrupt_hw.h"
#include "uart.h"
#include "csc_telemetry.h"
#include "periodic.h"
#include "downlink.h"
#include "csc_throttle.h"
#include "csc_adc.h"
#include "csc_rc_spektrum.h"
#include "csc_can.h"
#include "csc_ap_link.h"
static inline void on_servo_cmd(struct CscServoCmd *cmd);
static inline void on_motor_cmd(struct CscMotorMsg *msg);
static inline void on_prop_cmd(struct CscPropCmd *msg);
#define SERVO_TIMEOUT (SYS_TICS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
static uint32_t servo_cmd_timeout = 0;
static uint32_t can_msg_count = 0;
static void csc_main_init( void ) {
hw_init();
sys_time_init();
led_init();
actuators_init();
csc_servos_init();
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef SPEKTRUM_LINK
spektrum_init();
#endif
csc_ap_link_init();
csc_ap_link_set_servo_cmd_cb(on_servo_cmd);
csc_ap_link_set_motor_cmd_cb(on_motor_cmd);
csc_ap_link_set_prop_cmd_cb(on_prop_cmd);
csc_adc_init();
// be sure to call servos_init after uart1 init since they are sharing pins
#ifdef USE_I2C0
i2c_init();
#endif
motors_init();
csc_throttle_init();
int_enable();
}
static void csc_main_periodic( void ) {
static uint32_t zeros[4] = {0, 0, 0, 0};
static uint32_t csc_loops = 0;
#ifdef DOWNLINK
PeriodicSendAp();
#endif
if (servo_cmd_timeout > SERVO_TIMEOUT) {
csc_servos_set(zeros);
} else {
servo_cmd_timeout++;
}
if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
csc_ap_link_send_status(csc_loops, can_msg_count);
}
if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
csc_adc_periodic();
}
}
static void csc_main_event( void ) {
csc_can_event();
csc_throttle_event_task();
#ifdef SPEKTRUM_LINK
spektrum_event_task();
#endif
}
#define MIN_SERVO SYS_TICS_OF_USEC(1000)
#define MAX_SERVO SYS_TICS_OF_USEC(2000)
static void on_prop_cmd(struct CscPropCmd *cmd)
{
for(uint8_t i = 0; i < BUSS_TWI_BLMC_NB; i++)
motors_set_motor(i,cmd->speeds[i]);
motors_commit();
++can_msg_count;
}
static void on_servo_cmd(struct CscServoCmd *cmd)
{
uint16_t* servos = (uint16_t*)(cmd);
uint8_t i;
// uint32_t servos_checked[4];
for(i = 0; i < 4; i++)
csc_servo_normalized_set(i,servos[i]);
csc_servos_commit();
servo_cmd_timeout = 0;
++can_msg_count;
}
static void on_motor_cmd(struct CscMotorMsg *msg)
{
// always send to throttle_id zero, only one motorcontrol per csc board
const static uint8_t throttle_id = 0;
csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2);
}
int main( void ) {
csc_main_init();
while(1) {
if (sys_time_periodic())
csc_main_periodic();
csc_main_event();
}
return 0;
}
sw/airborne/csc/mercury_main.c
+24 -30
View File
@@ -38,7 +38,8 @@
#include "airframe.h"
#include "commands.h"
#include "csc_servos.h"
#include "csc_msg_def.h"
#include ACTUATORS
#include "booz2_imu.h"
#include "booz_ahrs_aligner.h"
#include "booz_ahrs.h"
@@ -54,16 +55,17 @@
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define PPRZ_MODE_MOTORS_OFF 0
#define PPRZ_MODE_MOTORS_ON 1
#define PPRZ_MODE_IN_FLIGHT 2
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define PPRZ_MODE_OF_RC(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? PPRZ_MODE_MANUAL : PPRZ_MODE_AUTO1)
uint8_t pprz_mode = PPRZ_MODE_AUTO1;
uint8_t pprz_mode = PPRZ_MODE_MOTORS_OFF;
static uint16_t cpu_time = 0;
static inline void csc_main_init( void );
static inline void csc_main_periodic( void );
static inline void csc_main_event( void );
int main( void ) {
csc_main_init();
while(1) {
@@ -79,27 +81,20 @@ static void nop(struct CscCanMsg *msg)
}
#define RADIO_SCALE 20
#define ROLL_OFFSET 1544
#define PITCH_OFFSET 2551
#define YAW_OFFSET 3585
#define MODE_OFFSET 5632
static void on_rc_cmd(struct CscRCMsg *msg)
{
rc_values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
rc_values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
rc_values[RADIO_YAW] = CSC_RC_SCALE*(msg->left_stick_horizontal - CSC_RC_OFFSET);
pprz_mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
rc_values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000);
rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
rc_values[RADIO_ROLL] = -RADIO_SCALE * (msg->right_stick_horizontal - ROLL_OFFSET);
rc_values[RADIO_PITCH] = -RADIO_SCALE * (msg->right_stick_vertical - PITCH_OFFSET);
rc_values[RADIO_YAW] = RADIO_SCALE * (msg->left_stick_horizontal - YAW_OFFSET);
rc_values[RADIO_MODE] = RADIO_SCALE * (msg->flap_mix - MODE_OFFSET);
time_since_last_ppm = 0;
rc_status = RC_OK;
pprz_mode = PPRZ_MODE_OF_RC(rc_values[RADIO_MODE]);
if (pprz_mode == PPRZ_MODE_MANUAL)
SetCommandsFromRC(commands);
}
STATIC_INLINE void csc_main_init( void ) {
static inline void csc_main_init( void ) {
hw_init();
sys_time_init();
@@ -127,6 +122,8 @@ STATIC_INLINE void csc_main_init( void ) {
csc_ap_link_set_rc_cmd_cb(on_rc_cmd);
actuators_init();
props_init();
csc_ap_init();
int_enable();
@@ -134,29 +131,26 @@ STATIC_INLINE void csc_main_init( void ) {
}
STATIC_INLINE void csc_main_periodic( void )
static inline void csc_main_periodic( void )
{
static uint32_t csc_loops = 0;
PeriodicSendAp();
radio_control_periodic_task();
if (rc_status == RC_REALLY_LOST) {
pprz_mode = PPRZ_MODE_AUTO1;
}
cpu_time++;
if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
csc_adc_periodic();
}
xsens_periodic_task();
if (pprz_mode == PPRZ_MODE_AUTO1){
csc_ap_periodic();
}
csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED);
}
STATIC_INLINE void csc_main_event( void )
static inline void csc_main_event( void )
{
csc_can_event();
xsens_event_task();
sw/airborne/csc/mercury_supervision.h
+36 -36
View File
@@ -9,10 +9,10 @@
/* #define TRIM_BACK (-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) */
/* #define TRIM_LEFT ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) */
/* #define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ */
/* _mot_cmd[SERVO_FRONT] = _dt + _de - _dr + TRIM_FRONT; \ */
/* _mot_cmd[SERVO_RIGHT] = _dt - _da + _dr + TRIM_RIGHT; \ */
/* _mot_cmd[SERVO_BACK] = _dt - _de - _dr + TRIM_BACK; \ */
/* _mot_cmd[SERVO_LEFT] = _dt + _da + _dr + TRIM_LEFT; \ */
/* _mot_cmd[PROP_FRONT] = _dt + _de - _dr + TRIM_FRONT; \ */
/* _mot_cmd[PROP_RIGHT] = _dt - _da + _dr + TRIM_RIGHT; \ */
/* _mot_cmd[PROP_BACK] = _dt - _de - _dr + TRIM_BACK; \ */
/* _mot_cmd[PROP_LEFT] = _dt + _da + _dr + TRIM_LEFT; \ */
/* } */
/* #else */
#define TRIM_FRONT ( SUPERVISION_TRIM_E+SUPERVISION_TRIM_R)
@@ -20,49 +20,49 @@
#define TRIM_BACK (-SUPERVISION_TRIM_E+SUPERVISION_TRIM_R)
#define TRIM_LEFT ( SUPERVISION_TRIM_A-SUPERVISION_TRIM_R)
#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \
_mot_cmd[SERVO_UPPER_LEFT] = _dt + _da + _de + _dr + TRIM_FRONT; \
_mot_cmd[SERVO_LOWER_RIGHT] = _dt - _da + _de - _dr + TRIM_RIGHT; \
_mot_cmd[SERVO_LOWER_LEFT] = _dt + _da - _de - _dr + TRIM_BACK; \
_mot_cmd[SERVO_UPPER_RIGHT] = _dt - _da - _de + _dr + TRIM_LEFT; \
_mot_cmd[PROP_UPPER_LEFT] = _dt + _da - _de + _dr + TRIM_FRONT; \
_mot_cmd[PROP_LOWER_RIGHT] = _dt - _da + _de + _dr + TRIM_RIGHT; \
_mot_cmd[PROP_LOWER_LEFT] = _dt + _da + _de - _dr + TRIM_BACK; \
_mot_cmd[PROP_UPPER_RIGHT] = _dt - _da - _de - _dr + TRIM_LEFT; \
}
//#endif
#define SUPERVISION_FIND_MAX_MOTOR(_mot_cmd, _max_mot) { \
_max_mot = (-32767-1); /* INT16_MIN;*/ \
if (_mot_cmd[SERVO_UPPER_LEFT] > _max_mot) \
max_mot = _mot_cmd[SERVO_UPPER_LEFT]; \
if (_mot_cmd[SERVO_LOWER_RIGHT] > _max_mot) \
max_mot = _mot_cmd[SERVO_LOWER_RIGHT]; \
if (_mot_cmd[SERVO_LOWER_LEFT] > _max_mot) \
max_mot = _mot_cmd[SERVO_LOWER_LEFT]; \
if (_mot_cmd[SERVO_UPPER_RIGHT] > _max_mot) \
max_mot = _mot_cmd[SERVO_UPPER_RIGHT]; \
if (_mot_cmd[PROP_UPPER_LEFT] > _max_mot) \
max_mot = _mot_cmd[PROP_UPPER_LEFT]; \
if (_mot_cmd[PROP_LOWER_RIGHT] > _max_mot) \
max_mot = _mot_cmd[PROP_LOWER_RIGHT]; \
if (_mot_cmd[PROP_LOWER_LEFT] > _max_mot) \
max_mot = _mot_cmd[PROP_LOWER_LEFT]; \
if (_mot_cmd[PROP_UPPER_RIGHT] > _max_mot) \
max_mot = _mot_cmd[PROP_UPPER_RIGHT]; \
}
#define SUPERVISION_FIND_MIN_MOTOR(_mot_cmd, _min_mot) { \
_min_mot = (32767); /*INT16_MAX;*/ \
if (_mot_cmd[SERVO_UPPER_LEFT] < _min_mot) \
min_mot = _mot_cmd[SERVO_UPPER_LEFT]; \
if (_mot_cmd[SERVO_LOWER_RIGHT] < _min_mot) \
min_mot = _mot_cmd[SERVO_LOWER_RIGHT]; \
if (_mot_cmd[SERVO_LOWER_LEFT] < _min_mot) \
min_mot = _mot_cmd[SERVO_LOWER_LEFT]; \
if (_mot_cmd[SERVO_UPPER_RIGHT] < _min_mot) \
min_mot = _mot_cmd[SERVO_UPPER_RIGHT]; \
if (_mot_cmd[PROP_UPPER_LEFT] < _min_mot) \
min_mot = _mot_cmd[PROP_UPPER_LEFT]; \
if (_mot_cmd[PROP_LOWER_RIGHT] < _min_mot) \
min_mot = _mot_cmd[PROP_LOWER_RIGHT]; \
if (_mot_cmd[PROP_LOWER_LEFT] < _min_mot) \
min_mot = _mot_cmd[PROP_LOWER_LEFT]; \
if (_mot_cmd[PROP_UPPER_RIGHT] < _min_mot) \
min_mot = _mot_cmd[PROP_UPPER_RIGHT]; \
}
#define SUPERVISION_OFFSET_MOTORS(_mot_cmd, _offset) { \
_mot_cmd[SERVO_UPPER_LEFT] += _offset; \
_mot_cmd[SERVO_LOWER_RIGHT] += _offset; \
_mot_cmd[SERVO_LOWER_LEFT] += _offset; \
_mot_cmd[SERVO_UPPER_RIGHT] += _offset; \
_mot_cmd[PROP_UPPER_LEFT] += _offset; \
_mot_cmd[PROP_LOWER_RIGHT] += _offset; \
_mot_cmd[PROP_LOWER_LEFT] += _offset; \
_mot_cmd[PROP_UPPER_RIGHT] += _offset; \
}
#define SUPERVISION_BOUND_MOTORS(_mot_cmd) { \
Bound(_mot_cmd[SERVO_UPPER_LEFT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[SERVO_LOWER_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[SERVO_LOWER_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[SERVO_UPPER_RIGHT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[PROP_UPPER_LEFT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[PROP_LOWER_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[PROP_LOWER_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
Bound(_mot_cmd[PROP_UPPER_RIGHT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
}
@@ -84,10 +84,10 @@
SUPERVISION_BOUND_MOTORS(_out); \
} \
else { \
_out[SERVO_UPPER_LEFT] = 0; \
_out[SERVO_LOWER_RIGHT] = 0; \
_out[SERVO_LOWER_LEFT] = 0; \
_out[SERVO_UPPER_RIGHT] = 0; \
_out[PROP_UPPER_LEFT] = 0; \
_out[PROP_LOWER_RIGHT] = 0; \
_out[PROP_LOWER_LEFT] = 0; \
_out[PROP_UPPER_RIGHT] = 0; \
} \
}
sw/airborne/csc/mercury_xsens.c
+8 -7
View File
@@ -200,6 +200,7 @@ static uint8_t xsens_msg_idx[XSENS_COUNT];
static uint8_t ck[XSENS_COUNT];
static uint8_t send_ck[XSENS_COUNT];
void xsens_init( void )
{
for (int i = 0; i < XSENS_COUNT; i++) {
@@ -294,15 +295,15 @@ void xsens_parse_msg( uint8_t xsens_id ) {
uint8_t offset = 0;
// test RAW modes else calibrated modes
if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
booz_imu.accel_unscaled.x = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.accel_unscaled.x = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.accel_unscaled.y = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.accel_unscaled.z = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.p = -1*XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.q = -1*XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.accel_unscaled.z = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.p = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.gyro_unscaled.r = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.y = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
booz_imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
Booz2ImuScaleGyro();
Booz2ImuScaleAccel();
Booz2ImuScaleMag();