Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge branch 'dev' of github.com:paparazzi/paparazzi into dev

Browse Source
This commit is contained in:
Bernard Davison
2012-03-11 10:10:58 +11:00
parent faca3eeb4f 1045759f72
commit b0490fb86f
37 changed files with 453 additions and 1807 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/example_heli_lisam.xml
+259
View File
@@ -0,0 +1,259 @@
<!-- this is a synerani vehicle equiped with Lisa/M and generic china pwm motor controllers -->
<airframe name="jt_lisam">
<servos>
<servo name="GAS" no="0" min="1100" neutral="1100" max="1900"/>
<servo name="CIC_LEFT" no="2" min="1900" neutral="1500" max="1100"/>
<servo name="CIC_RIGHT" no="3" min="1900" neutral="1500" max="1100"/>
<servo name="CIC_FRONT" no="1" min="1900" neutral="1500" max="1100"/>
<servo name="TAIL" no="4" min="1100" neutral="1500" max="1900"/>
<servo name="GYRO_GAIN" no="6" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
<axis name="THRUST" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
</commands>
<section name="MIXER">
<define name="TCURVE" value="1"/>
<define name="SUPERVISION_SCALE" value="9600" />
</section>
<rc_commands>
<set command="THRUST" value="@THRUST * SUPERVISION_SCALE / MAX_PPRZ"/>
<set command="ROLL" value="@ROLL * SUPERVISION_SCALE / MAX_PPRZ"/>
<set command="PITCH" value="@PITCH * SUPERVISION_SCALE / MAX_PPRZ"/>
<set command="YAW" value="@YAW * SUPERVISION_SCALE / MAX_PPRZ"/>
</rc_commands>
<command_laws>
<let var="halfway" value="(@THRUST >= (3800) ? 1 : 0)"/>
<let var="thrust" value="@THRUST * MAX_PPRZ / SUPERVISION_SCALE" />
<let var="roll" value="@ROLL * MAX_PPRZ / SUPERVISION_SCALE" />
<let var="pitch" value="@PITCH * MAX_PPRZ / SUPERVISION_SCALE" />
<let var="yaw" value="@YAW * MAX_PPRZ / SUPERVISION_SCALE" />
<let var="collective" value="$thrust * 1.8 - (MAX_PPRZ * 0.9)" />
<set servo="GAS" value="(3800 * 1.6) + $halfway * ($thrust - 3800) + (1 - $halfway) * ($thrust - 3800) * 1.6" />
<set servo="CIC_LEFT" value="((-$pitch/2)+($roll*0.7))+($collective)"/>
<set servo="CIC_RIGHT" value="(($pitch/2)+($roll*0.7))-($collective)"/>
<set servo="CIC_FRONT" value="$pitch+($collective)"/>
<set servo="TAIL" value="$yaw"/>
<set servo="GYRO_GAIN" value="-MAX_PPRZ/2"/>
</command_laws>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<!--
AP_MODE_RATE_DIRECT AP_MODE_ATTITUDE_DIRECT AP_MODE_HOVER_DIRECT
AP_MODE_RATE_RC_CLIMB AP_MODE_ATTITUDE_RC_CLIMB
AP_MODE_ATTITUDE_CLIMB AP_MODE_HOVER_CLIMB
AP_MODE_RATE_Z_HOLD AP_MODE_ATTITUDE_Z_HOLD AP_MODE_HOVER_Z_HOLD
AP_MODE_NAV
-->
<section name="IMU" prefix="IMU_">
<!--
<define name="GYRO_P_CHAN" value="1"/>
<define name="GYRO_Q_CHAN" value="0"/>
<define name="GYRO_R_CHAN" value="2"/>
-->
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="1"/>
<define name="GYRO_R_SIGN" value="1"/>
<define name="GYRO_P_NEUTRAL" value="-31"/>
<define name="GYRO_Q_NEUTRAL" value="-48"/>
<define name="GYRO_R_NEUTRAL" value="-17"/>
<define name="GYRO_P_SENS" value="4.412" integer="16"/>
<define name="GYRO_Q_SENS" value="4.412" integer="16"/>
<define name="GYRO_R_SENS" value="4.412" integer="16"/>
<!--
<define name="ACCEL_X_CHAN" value="3"/>
<define name="ACCEL_Y_CHAN" value="5"/>
<define name="ACCEL_Z_CHAN" value="6"/>
-->
<define name="ACCEL_X_SIGN" value="1"/>
<define name="ACCEL_Y_SIGN" value="1"/>
<define name="ACCEL_Z_SIGN" value="1"/>
<define name="ACCEL_X_NEUTRAL" value="10"/>
<define name="ACCEL_Y_NEUTRAL" value="1"/>
<define name="ACCEL_Z_NEUTRAL" value="-3"/>
<define name="ACCEL_X_SENS" value="38.4436411998" integer="16"/>
<define name="ACCEL_Y_SENS" value="38.7116161958" integer="16"/>
<define name="ACCEL_Z_SENS" value="39.970909149" integer="16"/>
<!--
<define name="MAG_X_CHAN" value="4"/>
<define name="MAG_Y_CHAN" value="0"/>
<define name="MAG_Z_CHAN" value="2"/>
<define name="MAG_45_HACK" value="1"/>
-->
<define name="MAG_X_SIGN" value="1"/>
<define name="MAG_Y_SIGN" value=" 1"/>
<define name="MAG_Z_SIGN" value="1"/>
<define name="MAG_X_NEUTRAL" value="-56"/>
<define name="MAG_Y_NEUTRAL" value="-171"/>
<define name="MAG_Z_NEUTRAL" value="-100"/>
<define name="MAG_X_SENS" value="4.3833130016" integer="16"/>
<define name="MAG_Y_SENS" value="4.59820229976" integer="16"/>
<define name="MAG_Z_SENS" value="4.3085068988" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 0.)"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="12000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_THETA" value="RadOfDeg(45.)"/>
<define name="SP_MAX_PSI" value="RadOfDeg(45.)"/>
<define name="SP_MAX_R" value="RadOfDeg(90.)"/>
<define name="SP_MAX_P" value="RadOfDeg(90.)"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_R" value="RadOfDeg(600)"/>
<define name="REF_ZETA_R" value="0.90"/>
<define name="REF_MAX_R" value="RadOfDeg(400.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.90"/>
<define name="REF_MAX_Q" value="RadOfDeg(500.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_P" value="RadOfDeg(600)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(220.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PSI_PGAIN" value="-750"/>
<define name="PSI_DGAIN" value="-370"/>
<define name="PSI_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="-800"/>
<define name="THETA_DGAIN" value="-240"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="-4000"/>
<define name="PHI_DGAIN" value="-600"/>
<define name="PHI_IGAIN" value="-10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="3.3" integer="16"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KI" value="-20"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<define name="INV_M" value ="0.21"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="MAG_UPDATE_YAW_ONLY" value="1"/>
<!-- magnetic field for Santa cruz from http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp -->
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
</section>
<section name="MISC">
<define name="FACE_REINJ_1" value="1024"/>
</section>
<!--
<modules main_freq="512">
<load name="vehicle_interface_overo_link.xml"/>
</modules>
-->
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_1.0">
<!-- <define name="BOOZ_START_DELAY" value="1"/> -->
<subsystem name="radio_control" type="ppm"/>
<subsystem name="actuators" type="heli"/>
<subsystem name="telemetry" type="transparent"/>
<define name="SERVO_HZ" value="50"/>
<define name="SERVO_HZ_SECONDARY" value="50"/>
<define name="USE_SERVOS_7AND8"/>
<define name="ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED"/>
</target>
<!--
<target name="sim" board="pc">
<subsystem name="fdm" type="nps"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="actuators" type="mkk"/>
</target>
-->
<subsystem name="imu" type="aspirin_v1.5"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="ahrs" type="int_cmpl_euler"/>
<subsystem name="stabilization" type="euler"/>
</firmware>
</airframe>
conf/airframes/fraser_lisa_m_rotorcraft.xml
+1 -4
View File
@@ -23,11 +23,8 @@
<subsystem name="actuators" type="mkk"/>
<subsystem name="imu" type="aspirin_v1.5"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="euler"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<!--subsystem name="ahrs" type="float_cmpl">
<define name="AHRS_PROPAGATE_QUAT"/>
</subsystem-->
</firmware>
<firmware name="setup">
conf/autopilot/subsystems/rotorcraft/stabilization_int_quat.makefile
+5
View File
@@ -0,0 +1,5 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile
+7
View File
@@ -0,0 +1,7 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.CFLAGS += -DUSE_SETPOINTS_WITH_TRANSITIONS
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c
conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile
+1 -6
View File
@@ -1,6 +1 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint.c
$(error The stabilization quaternion subsystem has been changed, for normal rotorcrafts please use <subsystem name="stabilization" type="int_quat"/>, for transitioning rotorcrafts (quadshot) use <subsystem name="stabilization" type="int_quat_transition"/>)
sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
+3 -2
View File
@@ -55,9 +55,10 @@ extern void sys_tick_irq_handler(void);
/** Busy wait, in microseconds */
/* for now empty shell */
// FIXME: directly use the SysTick->VAL here
static inline void sys_time_usleep(uint32_t us) {
uint32_t end = GET_CUR_TIME_USEC() + us;
while ((uint32_t)GET_CUR_TIME_USEC() < end);
}
#endif /* SYS_TIME_ARCH_H */
sw/airborne/boards/lisa_l/baro_board.c
+7
View File
@@ -66,7 +66,14 @@ void baro_periodic(void) {
void baro_board_send_config_abs(void) {
#ifndef BARO_LOW_GAIN
#pragma message "Using High LisaL Baro Gain: Do not use below 1000hPa"
baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83);
#else
#pragma message "Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more"
//config register should be 0x84 in low countries, or 0x86 in normal countries
baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x84, 0x83);
#endif
}
void baro_board_send_config_diff(void) {
sw/airborne/csc/arm7/props_csc.h
-15
View File
@@ -1,15 +0,0 @@
#ifndef PROPS_CSC_H
#define PROPS_CSC_H
#include "generated/airframe.h"
#include "firmwares/rotorcraft/actuators.h"
#include "mcu_periph/sys_time.h"
#include "csc_ap_link.h"
#include "csc_msg_def.h"
extern uint8_t csc_prop_speeds[PROPS_NB];
void props_set(uint8_t i, uint8_t speed);
void props_commit();
#endif /* PROPS_CSC_H */
sw/airborne/csc/csc_adc.c
-51
View File
@@ -1,51 +0,0 @@
#include "csc_adc.h"
#include "csc_ap_link.h"
#include <stdio.h>
#include "mcu_periph/uart.h"
#include "print.h"
#include "LPC21xx.h"
#include "led.h"
#include "mcu_periph/adc.h"
#include ACTUATORS
#include "csc_servos.h"
#include "mcu_periph/sys_time.h"
#define ADC_NB_CSC 4
struct adc_buf adc_bufs[ADC_NB_CSC];
uint8_t vsupply;
uint16_t adc_slider;
uint16_t adc_values[ADC_NB_CSC];
#define ADC_VDIV 5.7
#define ADC_VOLT 3.28
#define ADC_FACTOR 1024.0 * ADC_VOLT * ADC_VDIV
#ifndef ADC_AV_NB
#define ADC_AV_NB 8
#endif
#define ADC_VSUPPLY 0
#define ADC_SLIDER 3
void csc_adc_init(void)
{
adc_init();
for (int i = 0; i < ADC_NB_CSC; i++) {
adc_buf_channel(i, &adc_bufs[i], ADC_AV_NB);
}
}
void csc_adc_periodic(void)
{
vsupply = adc_bufs[ADC_VSUPPLY].sum / adc_bufs[ADC_VSUPPLY].av_nb_sample / ADC_FACTOR * 10;
adc_slider = adc_bufs[ADC_SLIDER].sum / adc_bufs[ADC_SLIDER].av_nb_sample;
for (int i = 0; i < ADC_NB_CSC; i++) {
adc_values[i] = adc_bufs[i].sum / adc_bufs[i].av_nb_sample;
}
}
sw/airborne/csc/csc_ap_main.c
-221
View File
@@ -1,221 +0,0 @@
/*
* $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 "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic.h"
#include "generated/airframe.h"
#include "commands.h"
#include "subsystems/radio_control.h"
#include "booz/booz2_gps.h"
//#include "ap_subsystems/datalink/downlink.h"
#include "csc_servos.h"
#include "csc_telemetry.h"
#include "csc_adc.h"
#include "csc_xsens.h"
#include "csc_autopilot.h"
#include "csc_can.h"
#include "pwm_input.h"
#include "csc_ap_link.h"
#include "led.h"
#include "subsystems/datalink/pprz_transport.h"
#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
#define PPRZ_MODE_OF_RC(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? PPRZ_MODE_MANUAL : PPRZ_MODE_AUTO1)
extern uint8_t vsupply;
uint8_t pprz_mode = PPRZ_MODE_AUTO1;
static uint16_t cpu_time = 0;
uint8_t csc_trims_set = 0;
struct Booz_gps_state booz_gps_state;
struct NedCoor_i booz_ins_gps_pos_cm_ned;
struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
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)
{
int aux2_flag;
static int last_aux2_flag = -1;
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_and_aux2 & ~(3 << 13)) - 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);
aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1;
rc_values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_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) {
csc_ap_clear_ierrors();
SetCommandsFromRC(commands, rc_values);
}
}
static void on_gpsacc_cmd( struct CscGPSAccMsg *msg )
{
booz_gps_state.pacc = msg->pacc;
booz_gps_state.sacc = msg->sacc;
}
static void on_gpsfix_cmd( struct CscGPSFixMsg *msg )
{
booz_gps_state.fix = msg->gps_fix;
booz_gps_state.num_sv = msg->num_sv;
booz_ins_gps_speed_cm_s_ned.x = msg->vx;
booz_ins_gps_speed_cm_s_ned.y = msg->vy;
booz_ins_gps_speed_cm_s_ned.z = msg->vz;
}
static void on_gpspos_cmd( struct CscGPSPosMsg *msg )
{
switch (msg->axis) {
case CSC_GPS_AXIS_IDX_X:
booz_ins_gps_pos_cm_ned.x = msg->val;
break;
case CSC_GPS_AXIS_IDX_Y:
booz_ins_gps_pos_cm_ned.y = msg->val;
break;
case CSC_GPS_AXIS_IDX_Z:
booz_ins_gps_pos_cm_ned.z = msg->val;
break;
default:
// Invalid msg
break;
}
}
static void csc_main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
Uart0Init();
Uart1Init();
xsens_init();
csc_adc_init();
ppm_init();
#ifdef USE_PWM_INPUT
pwm_input_init();
#endif
csc_ap_link_init();
csc_ap_link_set_servo_cmd_cb(&nop);
csc_ap_link_set_motor_cmd_cb(&nop);
csc_ap_link_set_rc_cmd_cb(on_rc_cmd);
csc_ap_link_set_gpsfix_cb(on_gpsfix_cmd);
csc_ap_link_set_gpspos_cb(on_gpspos_cmd);
csc_ap_link_set_gpsacc_cb(on_gpsacc_cmd);
actuators_init();
csc_ap_init();
mcu_int_enable();
}
static void csc_main_periodic( void )
{
static uint32_t csc_loops = 0;
PeriodicSendAp(DefaultChannel);
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(cpu_time, &booz_ins_gps_pos_cm_ned, &booz_ins_gps_speed_cm_s_ned);
if (csc_trims_set) {
csc_trims_set = 0;
csc_ap_set_trims();
}
#ifdef ACTUATORS
SetActuatorsFromCommands(commands);
#endif
SendCscFromActuators();
}
static void csc_main_event( void )
{
csc_can_event();
xsens_event_task();
DatalinkEvent();
}
int main( void ) {
csc_main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
csc_main_periodic();
csc_main_event();
}
return 0;
}
sw/airborne/csc/csc_baro.c
-169
View File
@@ -1,169 +0,0 @@
#include "std.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "csc_booz2_ins.h"
#include "csc_ap_link.h"
#include "mcu_periph/spi_arch.h"
#include "csc_baro.h"
uint8_t baro_scp_status;
uint32_t baro_scp_pressure;
uint16_t baro_scp_temperature;
float baro_scp_alt;
bool_t baro_scp_available;
static void baro_scp_start_high_res_measurement(void);
static void spi1_isr(void);
void baro_scp_periodic(void) {
if (cpu_time_sec > 1) {
if (baro_scp_status == STA_UNINIT) {
baro_scp_start_high_res_measurement();
baro_scp_status = STA_INITIALISING;
} else {
spi1_isr();
}
}
}
#define SS_PIN 30
#define SS_IODIR IO0DIR
#define SS_IOSET IO0SET
#define SS_IOCLR IO0CLR
#define ScpSelect() SetBit(SS_IOCLR,SS_PIN)
#define ScpUnselect() SetBit(SS_IOSET,SS_PIN)
// DRDY1 aka P0.15
#define DRDY1_PIN 15
#define DRDY1_IODIR IO0DIR
#define DRDY1_IOPIN IO0PIN
#define DRDY1_PINSEL() PINSEL0 = (PINSEL0 & ~(3 << 30)) | (0 << 30)
#define SSP1_SCK_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 2)) | (2 << 2))
#define SSP1_MISO_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 4)) | (2 << 4))
#define SSP1_MOSI_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 6)) | (2 << 6))
#define SSP1_SSEL_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 8)) | (2 << 8))
static uint8_t baro_scp_read_reg_8(uint8_t regno)
{
uint8_t value;
uint8_t foo0 __attribute__ ((unused));
ScpSelect();
sys_time_usleep(20);
S1SPDR = regno << 2;
while(~S1SPSR & _BV(7));
foo0 = S1SPDR;
S1SPDR = 0;
while(~S1SPSR & _BV(7));
value = S1SPDR;
sys_time_usleep(10);
ScpUnselect();
return value;
}
static void baro_scp_write_reg_8(uint8_t regno, uint8_t value)
{
uint8_t foo0 __attribute__ ((unused));
uint8_t foo1 __attribute__ ((unused));
ScpSelect();
foo0 = S1SPSR;
foo0 = S1SPDR;
sys_time_usleep(20);
S1SPDR = (regno << 2) | (1 << 1);
while(~S1SPSR & _BV(7));
foo0 = S1SPDR;
S1SPDR = value;
foo1 = S1SPDR;
while(~S1SPSR & _BV(7));
sys_time_usleep(10);
ScpUnselect();
}
static uint16_t baro_scp_read_reg_16(uint8_t regno)
{
uint8_t lsb, msb;
uint8_t foo0 __attribute__ ((unused));
ScpSelect();
sys_time_usleep(20);
S1SPDR = regno << 2;
while(~S1SPSR & _BV(7));
foo0 = S1SPDR;
S1SPDR = 0;
while(~S1SPSR & _BV(7));
msb = S1SPDR;
S1SPDR = 0;
while(~S1SPSR & _BV(7));
lsb = S1SPDR;
sys_time_usleep(10);
ScpUnselect();
return (uint16_t) ((msb << 8) | lsb);
}
void baro_scp_init( void )
{
SSP1_SCK_PINSEL();
SSP1_MISO_PINSEL();
SSP1_MOSI_PINSEL();
SSP1_SSEL_PINSEL();
DRDY1_PINSEL();
S1SPCR = _BV(3) | _BV(4) | _BV(5);
S1SPCCR = 0x80;
baro_scp_status = STA_UNINIT;
}
void spi1_isr()
{
uint32_t msb;
uint16_t lsb;
if (bit_is_set(DRDY1_IOPIN, DRDY1_PIN)) {
sys_time_usleep(10);
baro_scp_temperature = baro_scp_read_reg_16(0x21);
msb = baro_scp_read_reg_8(0x1F);
lsb = baro_scp_read_reg_16(0x20);
//if (baro_scp_temperature & 0x2000) {
// baro_scp_temperature |= 0xC000;
//}
baro_scp_temperature = baro_scp_temperature & (0xFFFF >> 2);
baro_scp_temperature *= 5;
baro_scp_pressure = lsb;
baro_scp_pressure |= (msb << 16);
baro_scp_pressure *= 25;
//baro_scp_alt = 9000 - baro_scp_pressure * 0.00084366;
baro_scp_status = STA_VALID;
}
}
/* write 0x0A to 0x03 */
static void baro_scp_start_high_res_measurement(void) {
baro_scp_write_reg_8(0x3, 0xA);
}
sw/airborne/csc/csc_main.c
-228
View File
@@ -1,228 +0,0 @@
/*
* $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 "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "csc_telemetry.h"
#include "generated/periodic.h"
#include "subsystems/datalink/downlink.h"
#include "i2c.h"
#include "csc_servos.h"
#include "csc_throttle.h"
#include "csc_adc.h"
#include "csc_rc_spektrum.h"
#include "csc_msg_def.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);
#define SERVO_TIMEOUT (CPU_TICKS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
static uint32_t servo_cmd_timeout = 0;
static uint32_t can_msg_count = 0;
// gps stuff stolen from antoine's code
#include "booz/booz2_gps.h"
#include "math/pprz_geodetic_int.h"
struct LtpDef_i booz_ins_ltp_def;
bool_t booz_ins_ltp_initialised;
struct NedCoor_i booz_ins_gps_pos_cm_ned;
struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
static void csc_main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef SPEKTRUM_LINK
spektrum_init();
#endif
#if USE_GPS
booz2_gps_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_adc_init();
#ifdef USE_I2C0
i2c_init();
#endif
// be sure to call servos_init after uart1 init since they are sharing pins
csc_servos_init();
#ifdef USE_CSC_THROTTLE
csc_throttle_init();
#endif
mcu_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(DefaultChannel);
#endif
if (servo_cmd_timeout > SERVO_TIMEOUT) {
LED_OFF(CAN_LED);
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 inline void on_gps_event(void) {
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
if (!booz_ins_ltp_initialised) {
ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
}
struct CscGPSFixMsg fix_msg;
struct CscGPSAccMsg acc_msg;
struct CscGPSPosMsg pos_msg;
fix_msg.gps_fix = (booz_gps_state.fix == BOOZ2_GPS_FIX_3D);
fix_msg.vx = booz_ins_gps_speed_cm_s_ned.x;
fix_msg.vy = booz_ins_gps_speed_cm_s_ned.y;
fix_msg.vz = booz_ins_gps_speed_cm_s_ned.z;
csc_ap_send_msg(CSC_GPS_FIX_ID, (const uint8_t *) &fix_msg, sizeof(fix_msg));
acc_msg.pacc = booz_gps_state.pacc;
acc_msg.sacc = booz_gps_state.sacc;
csc_ap_send_msg(CSC_GPS_ACC_ID, (const uint8_t *) &acc_msg, sizeof(acc_msg));
pos_msg.val = booz_ins_gps_pos_cm_ned.x;
pos_msg.axis = CSC_GPS_AXIS_IDX_X;
csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
pos_msg.val = booz_ins_gps_pos_cm_ned.y;
pos_msg.axis = CSC_GPS_AXIS_IDX_Y;
csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
pos_msg.val = booz_ins_gps_pos_cm_ned.z;
pos_msg.axis = CSC_GPS_AXIS_IDX_Z;
csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
}
static void csc_main_event( void ) {
csc_can_event();
#ifdef USE_CSC_THROTTLE
csc_throttle_event_task();
#endif
#ifdef SPEKTRUM_LINK
spektrum_event_task();
#endif
#if USE_GPS
Booz2GpsEvent(on_gps_event);
#endif
}
#define MIN_SERVO CPU_TICKS_OF_USEC(1000)
#define MAX_SERVO CPU_TICKS_OF_USEC(2000)
static inline void on_servo_cmd(struct CscServoCmd *cmd)
{
uint32_t servos_checked[4];
uint32_t i;
for (i=0; i<4; i++)
servos_checked[i] = cmd->servos[i];
// servos_checked[i] = Chop(servos[i],MIN_SERVO, MAX_SERVO);
csc_servos_set(servos_checked);
servo_cmd_timeout = 0;
++can_msg_count;
// DOWNLINK_SEND_CSC_CAN_MSG(&can1_rx_msg.frame, &can1_rx_msg.id,
// &can1_rx_msg.dat_a, &can1_rx_msg.dat_b);
// DOWNLINK_SEND_ADC_GENERIC(&servos[0], &servos[1]);
}
static inline 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;
#ifdef USE_CSC_THROTTLE
csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2);
#endif
}
int main( void ) {
csc_main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
csc_main_periodic();
csc_main_event();
}
return 0;
}
sw/airborne/csc/csc_protocol.c
-190
View File
@@ -1,190 +0,0 @@
/*
* $Id:$
*
* Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
*
* 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 <stdint.h>
#include <string.h>
#include "csc_protocol.h"
#include "mcu_periph/can.h"
#include "csc_msg_def.h"
// #include "mcu.h"
// #include "mcu_periph/sys_time.h"
// #include "subsystems/datalink/downlink.h"
#define CSCP_QUEUE_LEN 8
struct cscp_msg {
uint8_t id;
uint8_t len;
uint8_t data[8];
};
struct cscp_msg_queue {
int head;
int tail;
int full;
int empty;
struct cscp_msg msgs[CSCP_QUEUE_LEN];
} cscp_msg_queue;
/*
* This handle is being used internally in the protocol driver
* data points to user supplied memory (user allocated struct)
* this way we make sure that the user allocates only the amount
* of memory necessary and we don't have to mess around with malloc
*/
struct cscp_callback_handle {
cscp_callback_t callback;
void *data;
};
struct cscp_callback_handle cscp_callback_handles[CSC_ID_COUNT];
void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len);
void cscp_queue_init(void);
int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len);
int cscp_peek_msg_id(void);
int cscp_dequeue(uint8_t *buf);
void cscp_callback_init(void);
void cscp_init(void)
{
can_init(cscp_can_rx_callback);
cscp_queue_init();
cscp_callback_init();
}
static inline uint32_t cscp_can_id(uint8_t client_id, uint8_t msg_id)
{
return ((client_id & 0xF) << 7) | (msg_id & 0x7F);
}
static inline uint32_t cscp_msg_id(uint32_t id)
{
return (id & 0x7F);
}
int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, uint8_t len)
{
uint32_t id = cscp_can_id(client_id, msg_id);
return can_transmit(id, buf, len);
}
void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len)
{
/* just store the incoming data in a buffer with very little processing */
/* TODO we should handle the return value of enqueue somehow,
* the return value tells us if the queue had enough space to
* store our message or not
*/
uint32_t msg_id = cscp_msg_id(id);
cscp_enqueue(msg_id, buf, len);
}
void cscp_event(void)
{
/* this periodig is the main csc protocol event dispatcher */
int msg_id = cscp_peek_msg_id();
if (msg_id == -1){
return;
}
if(cscp_callback_handles[msg_id].callback){
cscp_dequeue(cscp_callback_handles[msg_id].data);
cscp_callback_handles[msg_id].callback(cscp_callback_handles[msg_id].data);
return;
}
}
void cscp_queue_init(void)
{
cscp_msg_queue.head = 0;
cscp_msg_queue.tail = 0;
cscp_msg_queue.full = 0;
cscp_msg_queue.empty = 1;
}
int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len)
{
if (cscp_msg_queue.full) {
cscp_msg_queue.empty = 0;
return 1;
}
if(!cscp_callback_handles[msg_id].callback) {
return 2;
}
cscp_msg_queue.msgs[cscp_msg_queue.tail].id = msg_id;
cscp_msg_queue.msgs[cscp_msg_queue.tail].len = len;
memcpy(cscp_msg_queue.msgs[cscp_msg_queue.tail].data, buf, len);
cscp_msg_queue.tail = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN;
cscp_msg_queue.empty = 0;
if (cscp_msg_queue.head == cscp_msg_queue.tail) {
cscp_msg_queue.full = 1;
}
return 0;
}
int cscp_peek_msg_id(void)
{
if (cscp_msg_queue.empty) {
return -1;
}
return cscp_msg_queue.msgs[cscp_msg_queue.head].id;
}
int cscp_dequeue(uint8_t *buf)
{
if (cscp_msg_queue.empty) {
return 1;
}
memcpy(buf,
cscp_msg_queue.msgs[cscp_msg_queue.head].data,
cscp_msg_queue.msgs[cscp_msg_queue.head].len);
cscp_msg_queue.head = (cscp_msg_queue.head + 1) % CSCP_QUEUE_LEN;
cscp_msg_queue.full = 0;
if (cscp_msg_queue.head == cscp_msg_queue.tail) {
cscp_msg_queue.empty = 1;
}
return 0;
}
void cscp_callback_init(void)
{
memset(cscp_callback_handles, 0, sizeof(cscp_callback_handles));
}
void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t *data)
{
cscp_callback_handles[msg_id].callback = callback;
cscp_callback_handles[msg_id].data = data;
}
sw/airborne/csc/csc_servos.c
-76
View File
@@ -1,76 +0,0 @@
#include "csc_servos.h"
#include "LPC21xx.h"
#include "std.h"
#include "mcu_periph/sys_time.h"
#include "firmwares/rotorcraft/actuators.h"
#include "generated/airframe.h"
#include ACTUATORS
#define CSC_SERVOS_NB 4
static uint32_t csc_servos_rng[] = {CPU_TICKS_OF_USEC(SERVO_S1_MAX-SERVO_S1_MIN),
CPU_TICKS_OF_USEC(SERVO_S2_MAX-SERVO_S2_MIN),
CPU_TICKS_OF_USEC(SERVO_S3_MAX-SERVO_S3_MIN),
CPU_TICKS_OF_USEC(SERVO_S4_MAX-SERVO_S4_MIN)};
static uint32_t csc_servos_min[] = {CPU_TICKS_OF_USEC(SERVO_S1_MIN),
CPU_TICKS_OF_USEC(SERVO_S2_MIN),
CPU_TICKS_OF_USEC(SERVO_S3_MIN),
CPU_TICKS_OF_USEC(SERVO_S4_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/((1<<16)-2) + csc_servos_min[id]);
}
}
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
Actuator(0) = val[0];
Actuator(1) = val[1];
Actuator(2) = val[2];
Actuator(3) = val[3];
#else
Actuator(0) = val[0];
Actuator(5) = val[1];
Actuator(4) = val[2];
Actuator(3) = val[3];
#endif
csc_servos_commit();
}
sw/airborne/csc/mercury_csc_main.c
-262
View File
@@ -1,262 +0,0 @@
/*
* $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 "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "csc_vane.h"
#ifdef USE_BUSS_TWI_BLMC_MOTOR
#include "buss_twi_blmc_hw.h"
#endif
#include "i2c.h"
#include "csc_servos.h"
#include "airspeed.h"
#include "baro_ets.h"
#include "airspeed_ets.h"
// #include "ams5812.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "csc_telemetry.h"
#include "generated/periodic.h"
#include "subsystems/datalink/downlink.h"
#include "pwm_input.h"
#include "csc_airspeed.h"
#include "csc_baro.h"
#include "csc_bat_monitor.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, int idx);
#define SERVO_TIMEOUT (CPU_TICKS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
#define AIRSPEED_TIMEOUT (CPU_TICKS_OF_SEC(0.01) / 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_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
actuators_init();
#ifdef USE_UART0
Uart0Init();
#endif
#ifdef USE_UART1
Uart1Init();
#endif
#ifdef SPEKTRUM_LINK
spektrum_init();
#endif
#ifdef USE_PWM_INPUT
pwm_input_init();
#endif
// be sure to call servos_init after uart1 init since they are sharing pins
csc_servos_init();
#ifdef USE_VANE_SENSOR
#ifndef USE_PWM_INPUT
pwm_input_init();
#endif
#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);
#ifdef ADC
csc_adc_init();
#endif
#ifdef USE_I2C0
i2c0_init();
#endif
#ifdef USE_BUSS_TWI_BLMC_MOTOR
motors_init();
#endif
#if USE_AIRSPEED
airspeed_init();
#endif
#ifdef USE_AMS5812
csc_ams5812_init();
#endif
#if USE_BARO_SCP
baro_scp_init();
#endif
#ifdef USE_BAT_MONITOR
csc_bat_monitor_init();
#endif
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(DefaultChannel);
#endif
#ifdef USE_VANE_SENSOR
csc_vane_periodic();
#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);
}
csc_adc_periodic();
if ((csc_loops % AIRSPEED_TIMEOUT) == 0) {
} else if ((csc_loops % AIRSPEED_TIMEOUT) == 1) {
#if USE_BARO_ETS
baro_ets_periodic();
#endif
#if USE_AIRSPEED
csc_airspeed_periodic();
#endif
#ifdef USE_AMS5812
csc_ams5812_periodic();
csc_ap_link_send_pressure(ams5812_pressure[0], ams5812_pressure[1]);
#endif
}
#if USE_AIRSPEED
airspeed_update();
#endif
#if USE_BARO_SCP
baro_scp_periodic();
csc_ap_link_send_baro(baro_scp_pressure, baro_scp_temperature, baro_scp_status);
#endif
#ifdef USE_BAT_MONITOR
csc_bat_monitor_periodic();
#endif
}
static void csc_main_event( void ) {
csc_can_event();
#ifdef USE_BUSS_TWI_BLMC_MOTOR
motors_event();
#endif
#ifdef SPEKTRUM_LINK
spektrum_event_task();
#endif
}
#define MIN_SERVO CPU_TICKS_OF_USEC(1000)
#define MAX_SERVO CPU_TICKS_OF_USEC(2000)
#ifdef USE_BUSS_TWI_BLMC_MOTOR
static void on_prop_cmd(struct CscPropCmd *cmd, int idx)
{
for(uint8_t i = 0; i < 4; i++)
motors_set_motor(i + idx * 4, cmd->speeds[i]);
motors_commit(idx == 1);
++can_msg_count;
}
#else
static void on_prop_cmd(struct CscPropCmd *cmd, int idx) {}
#endif
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) {}
int main( void ) {
csc_main_init();
// Uart0PrintString("Hello");
while(1) {
if (sys_time_check_and_ack_timer(0)) {
csc_main_periodic();
}
csc_main_event();
}
return 0;
}
sw/airborne/csc/mercury_main.c
-171
View File
@@ -1,171 +0,0 @@
/*
* $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 "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic.h"
#include "generated/airframe.h"
#include "commands.h"
#include "csc_msg_def.h"
#include ACTUATORS
#include "subsystems/imu.h"
#include "booz/ahrs/ahrs_aligner.h"
#include "booz/ahrs.h"
#include "mercury_xsens.h"
#include "csc_telemetry.h"
#include "csc_adc.h"
#include "mercury_ap.h"
#include "booz/booz2_autopilot.h"
#include "booz/guidance/booz2_guidance_v.h"
#include "csc_can.h"
#include "csc_baro.h"
#include "subsystems/radio_control.h"
#include "booz/stabilization/stabilization_attitude.h"
extern uint8_t vsupply;
#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
#define PPRZ_MODE_MOTORS_OFF 0
#define PPRZ_MODE_MOTORS_ON 1
#define PPRZ_MODE_IN_FLIGHT 2
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) {
if (sys_time_check_and_ack_timer(0))
csc_main_periodic();
csc_main_event();
}
return 0;
}
static void nop(struct CscCanMsg *msg)
{
}
static void on_rc_cmd(struct CscRCMsg *msg)
{
uint16_t aux2_flag;
radio_control.values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
radio_control.values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
radio_control.values[RADIO_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
pprz_mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1;
radio_control.values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
radio_control.values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000);
radio_control.values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
radio_control.time_since_last_frame = 0;
radio_control.status = RC_OK;
}
static inline void csc_main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
Uart0Init();
Uart1Init();
imu_init();
ahrs_aligner_init();
ahrs_init();
xsens_init();
stabilization_attitude_init();
booz2_guidance_v_init();
booz_ins_init();
csc_adc_init();
baro_scp_init();
csc_ap_link_init();
csc_ap_link_set_servo_cmd_cb(&nop);
csc_ap_link_set_motor_cmd_cb(&nop);
csc_ap_link_set_rc_cmd_cb(on_rc_cmd);
actuators_init();
props_init();
csc_ap_init();
mcu_int_enable();
stabilization_attitude_enter();
}
static inline void csc_main_periodic( void )
{
static uint32_t csc_loops = 0;
PeriodicSendAp();
radio_control_periodic();
cpu_time++;
if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
csc_adc_periodic();
}
xsens_periodic_task();
baro_scp_periodic();
csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
!(pprz_mode > PPRZ_MODE_MOTORS_OFF && ahrs_aligner.status == AHRS_ALIGNER_LOCKED));
}
static inline void csc_main_event( void )
{
csc_can_event();
xsens_event_task();
DatalinkEvent();
}
sw/airborne/csc/ppm_bridge_main.c
-131
View File
@@ -1,131 +0,0 @@
/*
* $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 "mcu.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "generated/periodic.h"
#include "generated/airframe.h"
#include "commands.h"
#include "subsystems/radio_control.h"
#include "csc_telemetry.h"
#include "led.h"
#include "subsystems/datalink/pprz_transport.h"
#define RC_PROTOCOL_SYNC 13999
extern uint8_t vsupply;
static uint16_t cpu_time = 0;
static void csc_main_init( void ) {
mcu_init();
sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
led_init();
Uart0Init();
Uart1Init();
ppm_init();
// Configure P0.21 as GPIO, output and then pull high as we use it to drive ppm input transistor
PINSEL1 = PINSEL1 & ~(0x3 << 10);
IO0DIR = IO0DIR | (0x1 << 21);
IO0PIN = IO0DIR | (0x1 << 21);
mcu_int_enable();
}
static void csc_main_periodic( void )
{
PeriodicSendAp(DefaultChannel);
radio_control_periodic_task();
cpu_time++;
}
static void send_short( int16_t s )
{
Uart1Transmit(s >> 8);
Uart1Transmit(s & 0xFF);
}
static void send_channel ( int16_t c )
{
send_short(c);
send_short(~c);
}
static void on_rc_event( void )
{
#ifdef SWAP_STICKS
int temp;
temp = rc_values[RADIO_YAW];
rc_values[RADIO_YAW] = rc_values[RADIO_ROLL];
rc_values[RADIO_ROLL] = temp;
#endif
// 7 channels
// integers -9600 to +9600
// sync bytes
send_channel(RC_PROTOCOL_SYNC);
for (int i = 0; i < RADIO_CTL_NB; i++) {
send_channel(rc_values[i]);
}
LED_TOGGLE(3);
}
static void csc_main_event( void )
{
RadioControlEventCheckAndHandle(on_rc_event);
DatalinkEvent();
}
int main( void ) {
csc_main_init();
while(1) {
if (sys_time_check_and_ack_timer(0))
csc_main_periodic();
csc_main_event();
}
return 0;
}
sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c
+2
View File
@@ -43,7 +43,9 @@
#endif
#define Actuator(_x) actuators_pwm_values[_x]
#ifndef ChopServo
#define ChopServo(x,a,b) Chop(x, a, b)
#endif
#define ActuatorsCommit actuators_pwm_commit
int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
sw/airborne/firmwares/rotorcraft/autopilot.h
+1
View File
@@ -45,6 +45,7 @@
#define AP_MODE_HOVER_CLIMB 10
#define AP_MODE_HOVER_Z_HOLD 11
#define AP_MODE_NAV 12
#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
extern uint8_t autopilot_mode;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+2 -2
View File
@@ -146,12 +146,12 @@ void guidance_h_read_rc(bool_t in_flight) {
break;
case GUIDANCE_H_MODE_HOVER:
STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
stabilization_attitude_read_rc_ref(&guidance_h_rc_sp, in_flight);
break;
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) {
STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
stabilization_attitude_read_rc_ref(&guidance_h_rc_sp, in_flight);
guidance_h_rc_sp.psi = 0;
}
else {
sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c → sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
+4 -57
View File
@@ -4,7 +4,7 @@
*
*/
/** \file quat_setpoint.c
/** \file quat_setpoint_int.c
* \brief Quaternion setpoint generation
*
*/
@@ -12,7 +12,7 @@
#include "subsystems/ahrs.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "stabilization/quat_setpoint.h"
#include "stabilization/quat_setpoint_int.h"
#include "stabilization.h"
#include "messages.h"
@@ -55,60 +55,7 @@ static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initia
QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2);
}
static void update_sp_quat_from_eulers(void) {
struct Int32RMat sp_rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(sp_rmat, stab_att_sp_euler);
#else
INT32_RMAT_OF_EULERS_321(sp_rmat, stab_att_sp_euler);
#endif
INT32_QUAT_OF_RMAT(stab_att_sp_quat, sp_rmat);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
/* FIXME: what is up with this???
void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane)
{
pprz_t roll = radio_control.values[RADIO_ROLL];
pprz_t pitch = radio_control.values[RADIO_PITCH];
pprz_t yaw = radio_control.values[RADIO_YAW];
struct Int32Quat prev_sp_quat, q_e2s, temp_quat;
struct Int32RMat R_e2s;
struct Int32Rates sticks_w_bn_e, sticks_w_bn_s;
QUAT_COPY(prev_sp_quat, stab_att_sp_quat);
sticks_w_bn_e.p = APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_H;
sticks_w_bn_e.q = APPLY_DEADBAND(pitch, STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE;
sticks_w_bn_e.r = APPLY_DEADBAND(yaw, STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_RATE;
// Don't let the sticks command a theta rotation in vane mode
if (enable_alpha_vane) {
sticks_w_bn_e.q = 0;
}
if (enable_beta_vane) {
sticks_w_bn_e.r = 0;
}
// q_e2s = q_sp (comp) q_b^(-1)
INT_QUAT_INV_COMP_NORM_SHORTEST(q_e2s, ahrs.ltp_to_body_quat, stab_att_sp_quat);
INT_RMAT_OF_QUAT(R_e2s, q_e2s);
INT_RMAT_RATEMULT(sticks_w_bn_s, R_e2s, sticks_w_bn_e);
INT_QUAT_DIFFERENTIAL(temp_quat, sticks_w_bn_s, 1/RC_UPDATE_FREQ);
INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, temp_quat);
// update euler setpoints for telemetry
INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat);
}
*/
void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight) {
void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
// FIXME: wtf???
#ifdef AIRPLANE_STICKS
@@ -147,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_fl
INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat);
}
void stabilization_attitude_sp_enter()
void stabilization_attitude_sp_enter(void)
{
// reset setpoint to "hover"
reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat);
sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h → sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h
+1 -4
View File
@@ -13,9 +13,6 @@
void stabilization_attitude_sp_enter(void);
// FIXME: apparently unused... still needed??
//void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane);
void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight);
void stabilization_attitude_read_rc_absolute(bool_t in_flight);
#endif
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
-2
View File
@@ -28,8 +28,6 @@
#include STABILISATION_ATTITUDE_H
extern void stabilization_attitude_init(void);
extern void stabilization_attitude_read_rc(bool_t in_flight);
extern void stabilization_attitude_read_beta_vane(float beta);
extern void stabilization_attitude_read_alpha_vane(float alpha);
extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_run(bool_t in_flight);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+1 -3
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -69,7 +67,7 @@ void stabilization_attitude_init(void) {
void stabilization_attitude_read_rc(bool_t in_flight) {
STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight);
stabilization_attitude_read_rc_ref(&stab_att_sp_euler, in_flight);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+1
View File
@@ -213,6 +213,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);
// FIXME: this is very dangerous! only works if this really includes all commands
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+29 -78
View File
@@ -1,5 +1,4 @@
/*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -20,12 +19,15 @@
* Boston, MA 02111-1307, USA.
*/
/** \file stabilization_attitude_quat_int.c
* \brief Booz quaternion attitude stabilization
/** @file stabilization_attitude_quat_int.c
* Rotorcraft quaternion attitude stabilization
*/
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/quat_setpoint.h"
#if USE_SETPOINTS_WITH_TRANSITIONS
#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#endif
#include <stdio.h>
#include "math/pprz_algebra_float.h"
@@ -46,46 +48,6 @@ struct Int32Eulers stabilization_att_sum_err;
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
//static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
/*
static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
*/
#define IERROR_SCALE 1024
#define GAIN_PRESCALER_FF 1
#define GAIN_PRESCALER_P 1
@@ -96,43 +58,21 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
*/
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
/*
void stabilization_attitude_gain_schedule(uint8_t idx)
{
if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) {
// This could be bad -- Just say no.
return;
}
gain_idx = idx;
stabilization_attitude_ref_schedule(idx);
}
*/
void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
#endif
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
//FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
INT_EULERS_ZERO( stabilization_att_sum_err );
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
@@ -208,18 +148,29 @@ void stabilization_attitude_run(bool_t enable_integrator) {
INT_EULERS_ZERO( stabilization_att_sum_err );
}
/* compute the feed forward command */
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel);
/* compute the feed back command */
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) {
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
Bound(stabilization_cmd[i], -200, 200);
}
/* sum feedforward and feedback */
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
/* bound the result */
Bound(stabilization_cmd[COMMAND_ROLL], -200, 200);
Bound(stabilization_cmd[COMMAND_PITCH], -200, 200);
Bound(stabilization_cmd[COMMAND_YAW], -200, 200);
}
void stabilization_attitude_read_rc(bool_t in_flight) {
STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight);
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
stabilization_attitude_read_rc_setpoint(in_flight);
#endif
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+2 -2
View File
@@ -23,8 +23,8 @@
#include "firmwares/rotorcraft/stabilization.h"
struct Int32Eulers stab_att_sp_euler;
struct Int32Eulers stab_att_ref_euler;
struct Int32Eulers stab_att_sp_euler; ///< with #REF_ANGLE_FRAC
struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
struct Int32Rates stab_att_ref_rate;
struct Int32Rates stab_att_ref_accel;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
-53
View File
@@ -26,59 +26,6 @@
#include "stabilization_attitude_ref_int.h"
#include "subsystems/radio_control.h"
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
}
/*
* Radio Control
*/
#define SP_MAX_PHI ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
#define SP_MAX_THETA ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA)
#define SP_MAX_R ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R)
#define RC_UPDATE_FREQ 40
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
_sp.phi = \
((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
_sp.theta = \
((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \
((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
ANGLE_REF_NORMALIZE(_sp.psi); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \
} \
}
#define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
EULERS_ADD(stab_att_sp_euler,_add_sp); \
ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
+3 -3
View File
@@ -20,8 +20,8 @@
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
#ifndef STABILISATION_ATTITUDE_REF_FLOAT_H
#define STABILISATION_ATTITUDE_REF_FLOAT_H
#include "generated/airframe.h"
@@ -39,4 +39,4 @@ struct FloatRefModel {
extern struct FloatRefModel stab_att_ref_model[];
#endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */
#endif /* STABILISATION_ATTITUDE_REF_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+61 -10
View File
@@ -20,17 +20,25 @@
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_INT_H
#ifndef STABILISATION_ATTITUDE_REF_INT_H
#define STABILISATION_ATTITUDE_REF_INT_H
extern struct Int32Eulers stab_att_sp_vi_euler; /* vehicle interface */
extern struct Int32Eulers stab_att_sp_rc_euler; /* radio control */
extern struct Int32Eulers stab_att_sp_euler; /* sum of the above */
extern struct Int32Quat stab_att_sp_quat;
#include "generated/airframe.h"
#include "math/pprz_algebra_int.h"
// FIXME: move stabilization_attitude_read_rc_ref somewere else, so we don't need these includes here???
#include "subsystems/radio_control.h"
#include "subsystems/ahrs.h"
/*
* CAUTION! stabilization euler has the euler angles with REF_ANGLE_FRAC, but stab quat with INT32_ANGLE_FRAC
*/
extern struct Int32Eulers stab_att_sp_euler;
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Eulers stab_att_ref_euler;
extern struct Int32Quat stab_att_ref_quat;
extern struct Int32Rates stab_att_ref_rate;
extern struct Int32Rates stab_att_ref_accel;
extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC
extern struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
struct Int32RefModel {
struct Int32Rates omega;
@@ -39,4 +47,47 @@ struct Int32RefModel {
extern struct Int32RefModel stab_att_ref_model;
#endif /* BOOZ_STABILISATION_ATTITUDE_REF_INT_H */
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
}
#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
#define SP_MAX_THETA (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA)
#define SP_MAX_R (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R)
#define RC_UPDATE_FREQ 40
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
static inline void stabilization_attitude_read_rc_ref(struct Int32Eulers *sp, bool_t in_flight) {
sp->phi = ((int32_t)-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ)
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ)
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += ((int32_t)-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ)
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
ANGLE_REF_NORMALIZE(sp->psi);
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
}
}
#endif /* STABILISATION_ATTITUDE_REF_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+1 -6
View File
@@ -31,7 +31,6 @@
#include "subsystems/ahrs.h"
#include "stabilization_attitude_ref_float.h"
#include "quat_setpoint.h"
#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT
@@ -104,11 +103,7 @@ void stabilization_attitude_ref_enter()
/*
* Reference
*/
#ifdef BOOZ_AP_PERIODIC_PRESCALE
#define DT_UPDATE ((float) BOOZ_AP_PERIODIC_PRESCALE / (float) PERIODIC_FREQ)
#else
#define DT_UPDATE (1./512.)
#endif
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
void stabilization_attitude_ref_update() {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
-2
View File
@@ -1,6 +1,4 @@
/*
* $Id: stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+20 -37
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -21,8 +19,8 @@
* Boston, MA 02111-1307, USA.
*/
/** \file stabilization_attitude_ref_int.c
* \brief Booz attitude reference generation (quaternion int version)
/** @file stabilization_attitude_ref_int.c
* Rotorcraft attitude reference generation (quaternion int version)
*
*/
@@ -31,7 +29,10 @@
#include "subsystems/ahrs.h"
#include "stabilization_attitude_ref_int.h"
//#include "quat_setpoint.h"
#if USE_SETPOINTS_WITH_TRANSITIONS
#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#endif
#define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
#define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
@@ -63,9 +64,9 @@
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
struct Int32Eulers stab_att_sp_euler;
struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
struct Int32Quat stab_att_sp_quat;
struct Int32Eulers stab_att_ref_euler;
struct Int32Eulers stab_att_ref_euler; ///< with #INT32_ANGLE_FRAC
struct Int32Quat stab_att_ref_quat;
struct Int32Rates stab_att_ref_rate;
struct Int32Rates stab_att_ref_accel;
@@ -75,14 +76,6 @@ struct Int32RefModel stab_att_ref_model = {
{STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
};
/*
static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
*/
static void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi;
@@ -90,26 +83,14 @@ static void reset_psi_ref_from_body(void) {
stab_att_ref_accel.r = 0;
}
static void update_ref_quat_from_eulers(void) {
struct Int32RMat ref_rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler);
#else
INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler);
#endif
INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat);
INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
}
void stabilization_attitude_ref_init(void) {
INT_EULERS_ZERO(stab_att_sp_euler);
INT32_QUAT_ZERO( stab_att_sp_quat);
INT32_QUAT_ZERO(stab_att_sp_quat);
INT_EULERS_ZERO(stab_att_ref_euler);
INT32_QUAT_ZERO( stab_att_ref_quat);
INT_RATES_ZERO( stab_att_ref_rate);
INT_RATES_ZERO( stab_att_ref_accel);
INT32_QUAT_ZERO(stab_att_ref_quat);
INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel);
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
@@ -123,23 +104,25 @@ void stabilization_attitude_ref_init(void) {
void stabilization_attitude_ref_enter()
{
reset_psi_ref_from_body();
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_sp_enter();
memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
#else
update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler);
#endif
/* set reference rate and acceleration to zero */
memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
//update_ref_quat_from_eulers();
}
/*
* Reference
*/
#define DT_UPDATE (1./512.)
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
#define F_UPDATE_RES 9
#include "messages.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
void stabilization_attitude_ref_update() {
/* integrate reference attitude */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+35 -19
View File
@@ -22,26 +22,14 @@
#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/quat_setpoint.h"
#include "subsystems/radio_control.h"
#include "math/pprz_algebra_float.h"
#include "stabilization_attitude_ref_int.h"
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
#include "subsystems/ahrs.h"
#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
}
#define RC_UPDATE_FREQ 40.
#define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ)
// FIXME: unused, what was it supposed to be?
//#define ROLL_COEF_H (STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ)
@@ -62,14 +50,42 @@
#define PITCH_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_PITCH] > STABILIZATION_ATTITUDE_DEADBAND_E || \
radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E)
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
#define STABILIZATION_ATTITUDE_READ_RC(_sp, _in_flight) do { stabilization_attitude_read_rc_absolute(_sp, _in_flight); } while(0)
#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) do {} while(0)
#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) {}
static inline void update_quat_from_eulers(struct Int32Quat *quat, struct Int32Eulers *eulers) {
struct Int32RMat rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(rmat, *eulers);
#else
INT32_RMAT_OF_EULERS_321(rmat, *eulers);
#endif
INT32_QUAT_OF_RMAT(*quat, rmat);
INT32_QUAT_WRAP_SHORTEST(*quat);
}
static inline void stabilization_attitude_read_rc_setpoint(bool_t in_flight) {
stab_att_sp_euler.phi = ((int32_t)-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
stab_att_sp_euler.theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
stab_att_sp_euler.psi += ((int32_t)-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(stab_att_sp_euler.psi);
}
}
else { /* if not flying, use current yaw as setpoint */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
}
/* update quaternion setpoint from euler setpoint */
update_quat_from_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
}
void stabilization_attitude_ref_enter(void);
void stabilization_attitude_ref_schedule(uint8_t idx);
#endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */
sw/airborne/mcu_periph/sys_time.h
+3 -3
View File
@@ -52,9 +52,9 @@ struct sys_time_timer {
};
struct sys_time {
uint32_t nb_sec; ///< full seconds since startup
uint32_t nb_sec_rem; ///< remainder of second in CPU_TICKS
uint32_t nb_tick; ///< in SYS_TICKS with SYS_TIME_RESOLUTION
volatile uint32_t nb_sec; ///< full seconds since startup
volatile uint32_t nb_sec_rem; ///< remainder of second in CPU_TICKS
volatile uint32_t nb_tick; ///< in SYS_TICKS with SYS_TIME_RESOLUTION
struct sys_time_timer timer[SYS_TIME_NB_TIMER];
};
sw/airborne/modules/digital_cam/led_cam_ctrl.h
+2
View File
@@ -102,6 +102,8 @@ static inline void dc_send_command(uint8_t cmd)
DC_PUSH(DC_POWER_LED);
break;
#endif
default:
break;
}
}
sw/airborne/subsystems/datalink/xbee.c
+2
View File
@@ -41,6 +41,8 @@ struct xbee_transport xbee_tp;
#define AT_AP_MODE "ATAP1\r"
#define AT_EXIT "ATCN\r"
uint32_t xbee_delay_time;
void xbee_init( void ) {
xbee_tp.status = XBEE_UNINIT;