From 528d244bec6bfaeb405cafd4f66a56ab82fe1f7c Mon Sep 17 00:00:00 2001 From: Allen Ibara Date: Wed, 24 Jun 2009 19:54:58 +0000 Subject: [PATCH] CSC/CSCAP updates --- sw/airborne/csc/csc_ap_link.c | 13 ++ sw/airborne/csc/csc_ap_link.h | 1 + sw/airborne/csc/csc_ap_main.c | 10 +- sw/airborne/csc/csc_autopilot.c | 16 ++- sw/airborne/csc/csc_autopilot.h | 1 + sw/airborne/csc/csc_can.c | 23 +++- sw/airborne/csc/csc_msg_def.h | 27 +++- sw/airborne/csc/csc_rc_spektrum.c | 32 ++++- sw/airborne/csc/csc_servos.c | 47 ++++++- sw/airborne/csc/csc_servos.h | 4 +- sw/airborne/csc/csc_telemetry.h | 2 + sw/airborne/csc/mercury_ap.c | 46 ++++--- sw/airborne/csc/mercury_ap.h | 2 +- sw/airborne/csc/mercury_csc_main.c | 186 ++++++++++++++++++++++++++ sw/airborne/csc/mercury_main.c | 54 ++++---- sw/airborne/csc/mercury_supervision.h | 72 +++++----- sw/airborne/csc/mercury_xsens.c | 15 ++- 17 files changed, 431 insertions(+), 120 deletions(-) create mode 100644 sw/airborne/csc/mercury_csc_main.c diff --git a/sw/airborne/csc/csc_ap_link.c b/sw/airborne/csc/csc_ap_link.c index 3a2efd5ca4..5b6dfe4b81 100644 --- a/sw/airborne/csc/csc_ap_link.c +++ b/sw/airborne/csc/csc_ap_link.c @@ -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; } } diff --git a/sw/airborne/csc/csc_ap_link.h b/sw/airborne/csc/csc_ap_link.h index f024973075..9e1982d008 100644 --- a/sw/airborne/csc/csc_ap_link.h +++ b/sw/airborne/csc/csc_ap_link.h @@ -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 */ diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c index d9aff6658c..c64f135c09 100644 --- a/sw/airborne/csc/csc_ap_main.c +++ b/sw/airborne/csc/csc_ap_main.c @@ -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]); diff --git a/sw/airborne/csc/csc_autopilot.c b/sw/airborne/csc/csc_autopilot.c index 52452d4cf7..d8906c6d11 100644 --- a/sw/airborne/csc/csc_autopilot.c +++ b/sw/airborne/csc/csc_autopilot.c @@ -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]; } + diff --git a/sw/airborne/csc/csc_autopilot.h b/sw/airborne/csc/csc_autopilot.h index 3a0da2b2ef..a7f54228aa 100644 --- a/sw/airborne/csc/csc_autopilot.h +++ b/sw/airborne/csc/csc_autopilot.h @@ -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); diff --git a/sw/airborne/csc/csc_can.c b/sw/airborne/csc/csc_can.c index ce181f7224..b5a11eb834 100644 --- a/sw/airborne/csc/csc_can.c +++ b/sw/airborne/csc/csc_can.c @@ -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 */ } diff --git a/sw/airborne/csc/csc_msg_def.h b/sw/airborne/csc/csc_msg_def.h index 0940936109..ecfbbc58fd 100644 --- a/sw/airborne/csc/csc_msg_def.h +++ b/sw/airborne/csc/csc_msg_def.h @@ -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 diff --git a/sw/airborne/csc/csc_rc_spektrum.c b/sw/airborne/csc/csc_rc_spektrum.c index 5e61edb5ae..1c4757f185 100644 --- a/sw/airborne/csc/csc_rc_spektrum.c +++ b/sw/airborne/csc/csc_rc_spektrum.c @@ -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)); } diff --git a/sw/airborne/csc/csc_servos.c b/sw/airborne/csc/csc_servos.c index 346e323db6..c0c9971e97 100644 --- a/sw/airborne/csc/csc_servos.c +++ b/sw/airborne/csc/csc_servos.c @@ -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(); } + diff --git a/sw/airborne/csc/csc_servos.h b/sw/airborne/csc/csc_servos.h index 3fb867cc31..4d3c674704 100644 --- a/sw/airborne/csc/csc_servos.h +++ b/sw/airborne/csc/csc_servos.h @@ -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 */ diff --git a/sw/airborne/csc/csc_telemetry.h b/sw/airborne/csc/csc_telemetry.h index 7815832c52..422e54899a 100644 --- a/sw/airborne/csc/csc_telemetry.h +++ b/sw/airborne/csc/csc_telemetry.h @@ -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; diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c index 4093c89b17..fd028e2003 100644 --- a/sw/airborne/csc/mercury_ap.c +++ b/sw/airborne/csc/mercury_ap.c @@ -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(); } diff --git a/sw/airborne/csc/mercury_ap.h b/sw/airborne/csc/mercury_ap.h index e8523f0990..8f54e19271 100644 --- a/sw/airborne/csc/mercury_ap.h +++ b/sw/airborne/csc/mercury_ap.h @@ -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 diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c new file mode 100644 index 0000000000..0e1fa63843 --- /dev/null +++ b/sw/airborne/csc/mercury_csc_main.c @@ -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 + +#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; +} + diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c index 0263cea46a..854ed09ac8 100644 --- a/sw/airborne/csc/mercury_main.c +++ b/sw/airborne/csc/mercury_main.c @@ -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(); diff --git a/sw/airborne/csc/mercury_supervision.h b/sw/airborne/csc/mercury_supervision.h index 774490e0c4..56e49bf27a 100644 --- a/sw/airborne/csc/mercury_supervision.h +++ b/sw/airborne/csc/mercury_supervision.h @@ -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; \ } \ } diff --git a/sw/airborne/csc/mercury_xsens.c b/sw/airborne/csc/mercury_xsens.c index 4975c6ad9e..8c2f91e302 100644 --- a/sw/airborne/csc/mercury_xsens.c +++ b/sw/airborne/csc/mercury_xsens.c @@ -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();