diff --git a/sw/airborne/csc/arm7/buss_twi_blmc_hw.c b/sw/airborne/csc/arm7/buss_twi_blmc_hw.c index c581c4cb66..18ece97a63 100644 --- a/sw/airborne/csc/arm7/buss_twi_blmc_hw.c +++ b/sw/airborne/csc/arm7/buss_twi_blmc_hw.c @@ -3,7 +3,7 @@ uint8_t twi_blmc_nb_err; -static uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; +uint8_t motor_power[BUSS_TWI_BLMC_NB]; static volatile bool_t buss_twi_blmc_status; static volatile bool_t buss_twi_blmc_i2c_done; static volatile uint8_t buss_twi_blmc_idx; @@ -13,7 +13,7 @@ const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR; void motors_init ( void ) { uint8_t i; for (i=0; i 255) - buss_twi_blmc_motor_power[id] = 255; + motor_power[id] = 255; else - buss_twi_blmc_motor_power[id] = value; + motor_power[id] = value; } static void buss_twi_blmc_send_next() { - i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx]; + i2c_buf[0] = motor_power[buss_twi_blmc_idx]; i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); } diff --git a/sw/airborne/csc/arm7/buss_twi_blmc_hw.h b/sw/airborne/csc/arm7/buss_twi_blmc_hw.h index a44a4845da..ac5df8ffc9 100644 --- a/sw/airborne/csc/arm7/buss_twi_blmc_hw.h +++ b/sw/airborne/csc/arm7/buss_twi_blmc_hw.h @@ -10,6 +10,8 @@ #define BUSS_TWI_BLMC_STATUS_IDLE 0 #define BUSS_TWI_BLMC_STATUS_BUSY 1 +extern uint8_t motor_power[BUSS_TWI_BLMC_NB]; + void motors_init ( void ); void motors_set_motor(uint8_t id, int16_t value); void motors_commit(); diff --git a/sw/airborne/csc/arm7/props_csc.h b/sw/airborne/csc/arm7/props_csc.h index ce608d206c..6f548921fc 100644 --- a/sw/airborne/csc/arm7/props_csc.h +++ b/sw/airborne/csc/arm7/props_csc.h @@ -1,7 +1,6 @@ #ifndef PROPS_CSC_H #define PROPS_CSC_H -#include "LPC21xx.h" #include "airframe.h" #include "actuators.h" #include "sys_time.h" diff --git a/sw/airborne/csc/csc_autopilot.c b/sw/airborne/csc/csc_autopilot.c index 45725219c6..dd70bdb560 100644 --- a/sw/airborne/csc/csc_autopilot.c +++ b/sw/airborne/csc/csc_autopilot.c @@ -33,6 +33,8 @@ #include "string.h" #include "radio_control.h" #include "pwm_input.h" +#include "LPC21xx.h" +#include "print.h" struct control_gains csc_gains; struct control_reference csc_reference; @@ -42,12 +44,12 @@ float csc_vane_angle; float csc_vane_angle_offset = 180; static const int xsens_id = 0; -float csc_yaw_weight; float csc_yaw_rudder; float csc_yaw_aileron; float csc_yaw_deadband; float csc_yaw_setpoint_rate; float csc_yaw_setpoint_range; +float csc_vane_weight; #define PWM_INPUT_COUNTS_PER_REV 61358. static void update_vane_angle( void ) @@ -66,10 +68,10 @@ void csc_ap_init( void ) csc_gains.yaw_kp = 5000; csc_gains.yaw_kd = 800; csc_gains.yaw_ki = 10; - csc_yaw_weight = 0; csc_yaw_rudder = 0.33; csc_yaw_aileron = 0; csc_yaw_deadband = 1.00; + csc_vane_weight = 1.0; csc_trims.elevator = 1800; csc_trims.aileron = -60; @@ -130,7 +132,10 @@ static void calculate_errors(struct control_reference *errors) errors->eulers.phi = xsens_eulers.phi - csc_reference.eulers.phi; errors->eulers.theta = xsens_eulers.theta - csc_reference.eulers.theta; - errors->eulers.psi = xsens_eulers.psi - csc_reference.eulers.psi; + //errors->eulers.psi = xsens_eulers.psi - csc_reference.eulers.psi; + //errors->eulers.psi = csc_vane_angle - csc_reference.eulers.psi; + errors->eulers.psi = (csc_vane_angle*csc_vane_weight + xsens_eulers.psi*(1 - csc_vane_weight)) + - csc_reference.eulers.psi; errors->rates.p = xsens_rates.p - csc_reference.rates.p; errors->rates.q = xsens_rates.q - csc_reference.rates.q; @@ -140,6 +145,7 @@ static void calculate_errors(struct control_reference *errors) errors->eulers_i.theta += xsens_eulers.theta; errors->eulers_i.psi += xsens_eulers.psi; + /* Deadband in yaw -- prevents it going nuts around an angle */ float yaw_deadband = RadOfDeg(csc_yaw_deadband); if (errors->eulers.psi <= yaw_deadband && errors->eulers.psi >= -yaw_deadband) { @@ -160,9 +166,7 @@ static void calculate_reference(struct control_reference *reference, int time) { reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW] + 100*csc_yaw_setpoint_range*sin(0.01*time*csc_yaw_setpoint_rate); 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 = M_PI / 6.0 * rc_values[RADIO_ROLL]; reference->eulers.phi /= MAX_PPRZ; reference->eulers.theta /= MAX_PPRZ; @@ -172,13 +176,13 @@ static void calculate_reference(struct control_reference *reference, int time) void csc_ap_periodic(int time) { static int counter = 0; + update_vane_angle(); calculate_reference(&csc_reference, time); calculate_errors(&csc_errors); - update_vane_angle(); - 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_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_trims.aileron; commands[COMMAND_PITCH] = -csc_gains.pitch_kp * csc_errors.eulers.theta diff --git a/sw/airborne/csc/csc_autopilot.h b/sw/airborne/csc/csc_autopilot.h index 266e6a1798..a1948ea8c2 100644 --- a/sw/airborne/csc/csc_autopilot.h +++ b/sw/airborne/csc/csc_autopilot.h @@ -58,7 +58,7 @@ extern struct control_reference csc_reference; extern struct control_trims csc_trims; extern float csc_vane_angle; extern float csc_vane_angle_offset; -extern float csc_yaw_weight; +extern float csc_vane_weight; extern float csc_yaw_rudder; extern float csc_yaw_aileron; extern float csc_yaw_deadband; diff --git a/sw/airborne/csc/csc_servos.c b/sw/airborne/csc/csc_servos.c index 8b63762e73..1408ab5e28 100644 --- a/sw/airborne/csc/csc_servos.c +++ b/sw/airborne/csc/csc_servos.c @@ -12,9 +12,13 @@ #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)}; + SYS_TICS_OF_USEC(SERVO_S2_MAX-SERVO_S2_MIN), + SYS_TICS_OF_USEC(SERVO_S3_MAX-SERVO_S3_MIN), + SYS_TICS_OF_USEC(SERVO_S4_MAX-SERVO_S4_MIN)}; static uint32_t csc_servos_min[] = {SYS_TICS_OF_USEC(SERVO_S1_MIN), - SYS_TICS_OF_USEC(SERVO_S2_MIN)}; + SYS_TICS_OF_USEC(SERVO_S2_MIN), + SYS_TICS_OF_USEC(SERVO_S3_MIN), + SYS_TICS_OF_USEC(SERVO_S4_MIN)}; void csc_servos_init(void) diff --git a/sw/airborne/csc/csc_telemetry.h b/sw/airborne/csc/csc_telemetry.h index abeed4812c..7008b29b92 100644 --- a/sw/airborne/csc/csc_telemetry.h +++ b/sw/airborne/csc/csc_telemetry.h @@ -49,6 +49,10 @@ #endif /* PROPS_NB */ +#ifdef BUSS_TWI_BLMC_NB +#define PERIODIC_SEND_MERCURY_PROPS() DOWNLINK_SEND_MERCURY_PROPS(&(motor_power[0]),&(motor_power[1]),&(motor_power[2]),&(motor_power[3])) +#endif /* BUSS_TWI_BLMC_NB */ + #ifdef COMMANDS_NB #include "commands.h" #define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands) diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c index 2faf743780..ca0b582193 100644 --- a/sw/airborne/csc/mercury_csc_main.c +++ b/sw/airborne/csc/mercury_csc_main.c @@ -31,8 +31,12 @@ #include "init_hw.h" #include "sys_time.h" #include "led.h" + +#ifdef USE_BUSS_TWI_BLMC_MOTOR #include "buss_twi_blmc_hw.h" #include "i2c.h" +#endif + #include "csc_servos.h" @@ -94,7 +98,10 @@ static void csc_main_init( void ) { #ifdef USE_I2C0 i2c_init(); #endif + + #ifdef USE_BUSS_TWI_BLMC_MOTOR motors_init(); + #endif int_enable(); } @@ -135,6 +142,7 @@ static void csc_main_event( void ) { #define MIN_SERVO SYS_TICS_OF_USEC(1000) #define MAX_SERVO SYS_TICS_OF_USEC(2000) +#ifdef USE_BUSS_TWI_BLMC_MOTOR static void on_prop_cmd(struct CscPropCmd *cmd) { for(uint8_t i = 0; i < BUSS_TWI_BLMC_NB; i++) @@ -144,6 +152,9 @@ static void on_prop_cmd(struct CscPropCmd *cmd) ++can_msg_count; } +#else +static void on_prop_cmd(struct CscPropCmd *cmd) {} +#endif static void on_servo_cmd(struct CscServoCmd *cmd)