Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Updates to CSC code

Browse Source
This commit is contained in:
Allen Ibara
2009-07-22 01:04:37 +00:00
parent ecc069d5c5
commit 3efd8e1773
8 changed files with 44 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/csc/arm7/buss_twi_blmc_hw.c
+6 -6
View File
@@ -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<BUSS_TWI_BLMC_NB;i++)
buss_twi_blmc_motor_power[i] = 0;
motor_power[i] = 0;
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
twi_blmc_nb_err = 0;
buss_twi_blmc_i2c_done = TRUE;
@@ -23,16 +23,16 @@ void motors_set_motor(uint8_t id, int16_t value)
{
// insert range checks
if(value < 0)
buss_twi_blmc_motor_power[id] = 0;
motor_power[id] = 0;
else if(value > 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);
}
sw/airborne/csc/arm7/buss_twi_blmc_hw.h
+2
View File
@@ -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();
sw/airborne/csc/arm7/props_csc.h
-1
View File
@@ -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"
sw/airborne/csc/csc_autopilot.c
+14 -10
View File
@@ -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
sw/airborne/csc/csc_autopilot.h
+1 -1
View File
@@ -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;
sw/airborne/csc/csc_servos.c
+6 -2
View File
@@ -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)
sw/airborne/csc/csc_telemetry.h
+4
View File
@@ -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)
sw/airborne/csc/mercury_csc_main.c
+11
View File
@@ -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)